sequencer.cpp
Go to the documentation of this file.
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>
15 #include <sot/core/sequencer.hh>
16 #include <sot/core/sot.hh>
17 
18 using namespace dynamicgraph::sot;
19 using namespace dynamicgraph;
20 
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 
35 
36  sotDEBUGOUT(5);
37 }
38 
40  sotDEBUGIN(5);
41 
42  sotDEBUGOUT(5);
43  return;
44 }
45 
46 /* --- SPECIFIC EVENT ------------------------------------------------------- */
47 /* --- SPECIFIC EVENT ------------------------------------------------------- */
48 /* --- SPECIFIC EVENT ------------------------------------------------------- */
49 
51  protected:
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 *>(
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 
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<";
101  os << ">";
102  }
103 };
104 
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<";
123  os << ">";
124  }
125 };
126 
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
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 }
SignalArray< int > sotNOSIGNAL(0)
void signalRegistration(const SignalArray< int > &signals)
#define sotDEBUGOUT(level)
Definition: debug.hh:212
void setNeedUpdateFromAllChildren(const bool b=true)
const std::string defaultTaskName
Definition: sequencer.cpp:53
virtual const std::string & getName() const
Definition: sequencer.cpp:75
std::ostream * outputStreamPtr
Definition: sequencer.hh:86
void init(std::istringstream &cmdArgs)
Definition: sequencer.cpp:59
void operator()(Sot *sotptr)
Definition: sequencer.cpp:90
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePosture, "FeaturePosture")
#define sotDEBUGIN(level)
Definition: debug.hh:211
virtual void push(TaskAbstract &task)
Push the task in the stack. It has a lowest priority than the previous ones. If this is the first tas...
Definition: sot.cpp:210
sotEventAddATask(const std::string name="", TaskAbstract *task=NULL)
Definition: sequencer.cpp:85
int & trigger(int &dummy, const int &time)
Definition: sequencer.cpp:214
TaskAbstract * taskPtr
Definition: sequencer.cpp:52
const std::string & getEventCmd() const
Definition: sequencer.cpp:150
#define sotDEBUGINOUT(level)
Definition: debug.hh:213
void rmTask(int eventType, const std::string &name, const unsigned int time)
Definition: sequencer.cpp:179
virtual void remove(const TaskAbstract &task)
Remove a task regardless to its position in the stack. It removes also the signals connected to the o...
Definition: sot.cpp:236
Entity & getEntity(const std::string &name)
void init(std::istringstream &args)
Definition: sequencer.cpp:138
static PoolStorage * getInstance()
Definition: pool.cpp:147
virtual void display(std::ostream &os) const
Definition: sequencer.cpp:69
#define sotDEBUG(level)
Definition: debug.hh:165
virtual void display(std::ostream &os) const
Definition: sequencer.cpp:151
FCL_REAL size() const
Sequencer(const std::string &name)
Definition: sequencer.cpp:23
const std::string & getName() const
DummyClass dummy
Definition: test_signal.cpp:31
void addTask(sotEventAbstract *task, const unsigned int time)
Definition: sequencer.cpp:167
This class implements the Stack of Task. It allows to deal with the priority of the controllers throu...
Definition: sot.hh:57
virtual void display(std::ostream &os) const
Definition: sequencer.cpp:246
std::list< sotEventAbstract * > TaskList
Definition: sequencer.hh:78
void operator()(Sot *sotptr)
Definition: sequencer.cpp:112
dynamicgraph::SignalTimeDependent< int, int > triggerSOUT
Definition: sequencer.hh:100
sotEventRemoveATask(const std::string name="", TaskAbstract *task=NULL)
Definition: sequencer.cpp:107
virtual void display(std::ostream &os) const
Definition: sequencer.cpp:120
virtual void operator()(Sot *)
Definition: sequencer.cpp:152
virtual void display(std::ostream &os) const
Definition: sequencer.cpp:98
sotEventTaskBased(const std::string name="", TaskAbstract *task=NULL)
Definition: sequencer.cpp:56
std::string cmd
Definition: sequencer.cpp:129
sotEventCmd(const std::string cmdLine="")
Definition: sequencer.cpp:132


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Wed Jun 21 2023 02:51:26