GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/tools/sequencer.cpp Lines: 0 58 0.0 %
Date: 2023-03-13 12:09:37 Branches: 0 96 0.0 %

Line Branch Exec Source
1
/*
2
 * Copyright 2010,
3
 * François Bleibel,
4
 * Olivier Stasse,
5
 *
6
 * CNRS/AIST
7
 *
8
 */
9
10
#include <dynamic-graph/factory.h>
11
#include <dynamic-graph/pool.h>
12
13
#include <sot/core/debug.hh>
14
#include <sot/core/exception-tools.hh>
15
#include <sot/core/sequencer.hh>
16
#include <sot/core/sot.hh>
17
18
using namespace dynamicgraph::sot;
19
using namespace dynamicgraph;
20
21
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Sequencer, "Sequencer");
22
23
Sequencer::Sequencer(const std::string &name)
24
    : Entity(name),
25
      timeInit(-1),
26
      playMode(false),
27
      outputStreamPtr(NULL),
28
      noOutput(false),
29
      triggerSOUT(boost::bind(&Sequencer::trigger, this, _1, _2), sotNOSIGNAL,
30
                  "Sequencer(" + name + ")::output(dummy)::trigger") {
31
  sotDEBUGIN(5);
32
33
  signalRegistration(triggerSOUT);
34
  triggerSOUT.setNeedUpdateFromAllChildren(true);
35
36
  sotDEBUGOUT(5);
37
}
38
39
Sequencer::~Sequencer(void) {
40
  sotDEBUGIN(5);
41
42
  sotDEBUGOUT(5);
43
  return;
44
}
45
46
/* --- SPECIFIC EVENT ------------------------------------------------------- */
47
/* --- SPECIFIC EVENT ------------------------------------------------------- */
48
/* --- SPECIFIC EVENT ------------------------------------------------------- */
49
50
class sotEventTaskBased : public Sequencer::sotEventAbstract {
51
 protected:
52
  TaskAbstract *taskPtr;
53
  const std::string defaultTaskName;
54
55
 public:
56
  sotEventTaskBased(const std::string name = "", TaskAbstract *task = NULL)
57
      : sotEventAbstract(name), taskPtr(task), defaultTaskName("NULL") {}
58
59
  void init(std::istringstream &cmdArgs) {
60
    cmdArgs >> std::ws;
61
    if (cmdArgs.good()) {
62
      std::string taskname;
63
      cmdArgs >> taskname;
64
      sotDEBUG(15) << "Add task " << taskname << std::endl;
65
      taskPtr = dynamic_cast<TaskAbstract *>(
66
          &dynamicgraph::PoolStorage::getInstance()->getEntity(taskname));
67
    }
68
  }
69
  virtual void display(std::ostream &os) const {
70
    if (taskPtr)
71
      os << taskPtr->getName();
72
    else
73
      os << "NULL";
74
  }
75
  virtual const std::string &getName() const {
76
    if (taskPtr)
77
      return taskPtr->getName();
78
    else
79
      return defaultTaskName;
80
  }
81
};
82
83
class sotEventAddATask : public sotEventTaskBased {
84
 public:
85
  sotEventAddATask(const std::string name = "", TaskAbstract *task = NULL)
86
      : sotEventTaskBased(name, task) {
87
    eventType = EVENT_ADD;
88
  }
89
90
  void operator()(Sot *sotptr) {
91
    sotDEBUGIN(15);
92
    sotDEBUG(45) << "Sot = " << sotptr << ". Task = " << taskPtr << "."
93
                 << std::endl;
94
    if ((NULL != sotptr) && (NULL != taskPtr)) sotptr->push(*taskPtr);
95
    sotDEBUGOUT(15);
96
  }
97
98
  virtual void display(std::ostream &os) const {
99
    os << "Add<";
100
    sotEventTaskBased::display(os);
101
    os << ">";
102
  }
103
};
104
105
class sotEventRemoveATask : public sotEventTaskBased {
106
 public:
107
  sotEventRemoveATask(const std::string name = "", TaskAbstract *task = NULL)
108
      : sotEventTaskBased(name, task) {
109
    eventType = EVENT_RM;
110
  }
111
112
  void operator()(Sot *sotptr) {
113
    sotDEBUGIN(15);
114
    sotDEBUG(45) << "Sot = " << sotptr << ". Task = " << taskPtr << "."
115
                 << std::endl;
116
    if ((NULL != sotptr) && (NULL != taskPtr)) sotptr->remove(*taskPtr);
117
    sotDEBUGOUT(15);
118
  }
119
120
  virtual void display(std::ostream &os) const {
121
    os << "Remove<";
122
    sotEventTaskBased::display(os);
123
    os << ">";
124
  }
125
};
126
127
class sotEventCmd : public Sequencer::sotEventAbstract {
128
 protected:
129
  std::string cmd;
130
131
 public:
132
  sotEventCmd(const std::string cmdLine = "")
133
      : sotEventAbstract(cmdLine + "<cmd>"), cmd(cmdLine) {
134
    eventType = EVENT_CMD;
135
    sotDEBUGINOUT(15);
136
  }
137
138
  void init(std::istringstream &args) {
139
    sotDEBUGIN(15);
140
    std::stringbuf *pbuf = args.rdbuf();
141
    const unsigned int size = (unsigned int)(pbuf->in_avail());
142
    char *buffer = new char[size + 1];
143
    pbuf->sgetn(buffer, size);
144
145
    buffer[size] = '\0';
146
    cmd = buffer;
147
    sotDEBUGOUT(15);
148
    delete[] buffer;
149
  }
150
  const std::string &getEventCmd() const { return cmd; }
151
  virtual void display(std::ostream &os) const { os << "Run: " << cmd; }
152
  virtual void operator()(Sot * /*sotPtr*/) {
153
    std::ostringstream onull;
154
    onull.clear(std::ios::failbit);
155
    std::istringstream iss(cmd);
156
    std::string cmdName;
157
    iss >> cmdName;
158
    // Florent: remove reference to g_shell
159
    // g_shell.cmd( cmdName,iss,onull );
160
  };
161
};
162
163
/* --- TASK MANIP ----------------------------------------------------------- */
164
/* --- TASK MANIP ----------------------------------------------------------- */
165
/* --- TASK MANIP ----------------------------------------------------------- */
166
167
void Sequencer::addTask(sotEventAbstract *task, const unsigned int timeSpec) {
168
  TaskMap::iterator listKey = taskMap.find(timeSpec);
169
  if (taskMap.end() == listKey) {
170
    sotDEBUG(15) << "New element at " << timeSpec << std::endl;
171
    taskMap[timeSpec].push_back(task);
172
  } else {
173
    TaskList &tl = listKey->second;
174
    tl.push_back(task);
175
  }
176
}
177
178
// rmTask
179
void Sequencer::rmTask(int eventType, const std::string &name,
180
                       const unsigned int time) {
181
  TaskMap::iterator listKey = taskMap.find(time);
182
  if (taskMap.end() != listKey)  // the time exist
183
  {
184
    TaskList &tl = listKey->second;
185
    for (TaskList::iterator itL = tl.begin(); itL != tl.end(); ++itL) {
186
      if ((*itL)->getEventType() == eventType && (*itL)->getName() == name) {
187
        tl.remove(*itL);
188
        break;
189
      }
190
    }
191
192
    // remove the list if empty
193
    if (tl.empty()) taskMap.erase(listKey);
194
  }
195
}
196
197
// clearAll
198
void Sequencer::clearAll() {
199
  TaskMap::iterator itM;
200
  for (itM = taskMap.begin(); itM != taskMap.end(); ++itM) {
201
    TaskList::iterator itL;
202
    TaskList &currentMap = itM->second;
203
    for (itL = currentMap.begin(); itL != currentMap.end(); ++itL)
204
      delete (*itL);
205
    itM->second.clear();
206
  }
207
  // remove all the lists
208
  taskMap.clear();
209
}
210
/* --- SIGNALS -------------------------------------------------------------- */
211
/* --- SIGNALS -------------------------------------------------------------- */
212
/* --- SIGNALS -------------------------------------------------------------- */
213
214
int &Sequencer::trigger(int &dummy, const int &timeSpec) {
215
  sotDEBUGIN(15);
216
217
  if (!playMode) return dummy;
218
  if (-1 == timeInit) timeInit = timeSpec;
219
220
  sotDEBUG(15) << "Ref time: " << (timeSpec - timeInit) << std::endl;
221
  TaskMap::iterator listKey = taskMap.find(timeSpec - timeInit);
222
  if (taskMap.end() != listKey) {
223
    sotDEBUG(1) << "Time: " << (timeSpec - timeInit)
224
                << ": we've got a task to do!" << std::endl;
225
    TaskList &tl = listKey->second;
226
    for (TaskList::iterator iter = tl.begin(); iter != tl.end(); ++iter) {
227
      if (*iter) {
228
        (*iter)->operator()(sotPtr);
229
        if (NULL != outputStreamPtr) {
230
          (*outputStreamPtr) << "At time t=" << timeSpec << ": ";
231
          (*iter)->display(*outputStreamPtr);
232
          (*outputStreamPtr) << std::endl;
233
        }
234
      }
235
    }
236
  }
237
238
  sotDEBUGOUT(15);
239
  return dummy;
240
}
241
242
/* --- PARAMS --------------------------------------------------------------- */
243
/* --- PARAMS --------------------------------------------------------------- */
244
/* --- PARAMS --------------------------------------------------------------- */
245
246
void Sequencer::display(std::ostream &os) const {
247
  if (noOutput) return;
248
249
  os << "Sequencer " << getName() << "(t0=" << timeInit
250
     << ",mode=" << ((playMode) ? "play" : "pause") << "): " << std::endl;
251
  for (TaskMap::const_iterator iterMap = taskMap.begin();
252
       iterMap != taskMap.end(); iterMap++) {
253
    os << " - t=" << (iterMap->first) << ":\t";
254
    const TaskList &tl = iterMap->second;
255
    for (TaskList::const_iterator iterList = tl.begin(); iterList != tl.end();
256
         iterList++) {
257
      (*iterList)->display(os);
258
      os << " ";
259
    }
260
    os << std::endl;
261
  }
262
}