1 |
|
|
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 |
|
|
* Copyright Projet JRL-Japan, 2007 |
3 |
|
|
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
4 |
|
|
* |
5 |
|
|
* File: StepTimeLine.cpp |
6 |
|
|
* Project: SOT |
7 |
|
|
* Author: Nicolas Mansard, Olivier Stasse, Paul Evrard |
8 |
|
|
* |
9 |
|
|
* Version control |
10 |
|
|
* =============== |
11 |
|
|
* |
12 |
|
|
* $Id$ |
13 |
|
|
* |
14 |
|
|
* Description |
15 |
|
|
* ============ |
16 |
|
|
* |
17 |
|
|
* StepTimeLine entity: synchronizes a StepQueue, a StepComputer and a |
18 |
|
|
* PGManager to compute steps to send to the PG. Uses a StepChecker |
19 |
|
|
* to clip the steps. |
20 |
|
|
* Highest-level class for automatic step generation. |
21 |
|
|
* |
22 |
|
|
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ |
23 |
|
|
|
24 |
|
|
#include <dynamic-graph/factory.h> |
25 |
|
|
#include <dynamic-graph/pool.h> |
26 |
|
|
#include <sot/pattern-generator/step-time-line.h> |
27 |
|
|
|
28 |
|
|
#include <sot/core/debug.hh> |
29 |
|
|
|
30 |
|
|
namespace dynamicgraph { |
31 |
|
|
namespace sot { |
32 |
|
|
|
33 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(StepTimeLine, "TimeLine"); |
34 |
|
|
|
35 |
|
|
const unsigned int StepTimeLine::PERIOD_DEFAULT = 160; |
36 |
|
|
// 160iter=800ms |
37 |
|
|
const unsigned int StepTimeLine::FIRST_STEP_TO_MODIFY = 3; |
38 |
|
|
|
39 |
|
|
StepTimeLine::StepTimeLine(const std::string &name) |
40 |
|
|
: Entity(name), |
41 |
|
|
triggerSOUT("NextStep(" + name + ")::input(dummy)::trigger") |
42 |
|
|
|
43 |
|
|
, |
44 |
|
|
stepQueue(0x0), |
45 |
|
|
stepComputer(0x0), |
46 |
|
|
pgManager(0x0) |
47 |
|
|
|
48 |
|
|
, |
49 |
|
|
state(STATE_STOPPED), |
50 |
|
|
timeLastIntroduction(0), |
51 |
|
|
period(PERIOD_DEFAULT), |
52 |
|
|
nStartingSteps(0) { |
53 |
|
|
sotDEBUGIN(5); |
54 |
|
|
|
55 |
|
|
triggerSOUT.setFunction( |
56 |
|
|
boost::bind(&StepTimeLine::triggerCall, this, _1, _2)); |
57 |
|
|
signalRegistration(triggerSOUT); |
58 |
|
|
|
59 |
|
|
sotDEBUGOUT(5); |
60 |
|
|
} |
61 |
|
|
|
62 |
|
|
int &StepTimeLine::triggerCall(int &dummy, int timeCurrent) { |
63 |
|
|
sotDEBUGIN(45); |
64 |
|
|
|
65 |
|
|
if (state == STATE_STARTED) { |
66 |
|
|
int nextIntoductionTime = timeLastIntroduction + period; |
67 |
|
|
|
68 |
|
|
if (nextIntoductionTime <= timeCurrent) { // condition (A) |
69 |
|
|
if (nStartingSteps < FIRST_STEP_TO_MODIFY) { |
70 |
|
|
++nStartingSteps; |
71 |
|
|
} else { |
72 |
|
|
stepComputer->changeFirstStep(*stepQueue, timeCurrent); |
73 |
|
|
double stepTime = pgManager->changeNextStep(*stepQueue); |
74 |
|
|
if (stepTime < 0.0) { |
75 |
|
|
period = PERIOD_DEFAULT; |
76 |
|
|
} else { |
77 |
|
|
period = static_cast<unsigned int>((stepTime + 0.025) * 200.0); |
78 |
|
|
} |
79 |
|
|
} |
80 |
|
|
|
81 |
|
|
stepComputer->nextStep(*stepQueue, timeCurrent); |
82 |
|
|
pgManager->introduceStep(*stepQueue); |
83 |
|
|
timeLastIntroduction = timeCurrent; |
84 |
|
|
} |
85 |
|
|
} else if (state == STATE_STARTING) { |
86 |
|
|
stepQueue->startSequence(); |
87 |
|
|
pgManager->startSequence(*stepQueue); |
88 |
|
|
timeLastIntroduction = timeCurrent - period + 1; |
89 |
|
|
// trick to satisfy condition (A) at next call |
90 |
|
|
nStartingSteps = 0; |
91 |
|
|
state = STATE_STARTED; |
92 |
|
|
period = PERIOD_DEFAULT; |
93 |
|
|
} else if (state == STATE_STOPPING) { |
94 |
|
|
pgManager->stopSequence(*stepQueue); |
95 |
|
|
state = STATE_STOPPED; |
96 |
|
|
} else { |
97 |
|
|
// state == STATE_STOPPED: do nothing |
98 |
|
|
} |
99 |
|
|
|
100 |
|
|
sotDEBUGOUT(45); |
101 |
|
|
return dummy; |
102 |
|
|
} |
103 |
|
|
|
104 |
|
|
void StepTimeLine::display(std::ostream &os) const { |
105 |
|
|
os << "StepTimeLine <" << getName() << ">:" << std::endl; |
106 |
|
|
os << " - timeLastIntroduction: " << timeLastIntroduction << std::endl; |
107 |
|
|
os << " - period: " << period << std::endl; |
108 |
|
|
|
109 |
|
|
switch (state) { |
110 |
|
|
case STATE_STARTING: |
111 |
|
|
os << " - state: starting" << std::endl; |
112 |
|
|
case STATE_STARTED: |
113 |
|
|
os << " - state: started" << std::endl; |
114 |
|
|
case STATE_STOPPING: |
115 |
|
|
os << " - state: stopping" << std::endl; |
116 |
|
|
case STATE_STOPPED: |
117 |
|
|
os << " - state: stopped" << std::endl; |
118 |
|
|
} |
119 |
|
|
} |
120 |
|
|
|
121 |
|
|
void StepTimeLine::commandLine(const std::string &cmdLine, |
122 |
|
|
std::istringstream &cmdArgs, std::ostream &os) { |
123 |
|
|
if (cmdLine == "help") { |
124 |
|
|
os << "StepTimeLine: " << std::endl << std::endl; |
125 |
|
|
} else if (cmdLine == "setComputer") { |
126 |
|
|
std::string name = "stepcomp"; |
127 |
|
|
cmdArgs >> std::ws; |
128 |
|
|
if (cmdArgs.good()) { |
129 |
|
|
cmdArgs >> name; |
130 |
|
|
} |
131 |
|
|
Entity *entity = &(PoolStorage::getInstance()->getEntity(name)); |
132 |
|
|
stepComputer = dynamic_cast<StepComputer *>(entity); |
133 |
|
|
} else if (cmdLine == "setQueue") { |
134 |
|
|
std::string name = "stepqueue"; |
135 |
|
|
cmdArgs >> std::ws; |
136 |
|
|
if (cmdArgs.good()) { |
137 |
|
|
cmdArgs >> name; |
138 |
|
|
} |
139 |
|
|
Entity *entity = &(PoolStorage::getInstance()->getEntity(name)); |
140 |
|
|
stepQueue = dynamic_cast<StepQueue *>(entity); |
141 |
|
|
} else if (cmdLine == "setPGManager") { |
142 |
|
|
std::string name = "steppg"; |
143 |
|
|
cmdArgs >> std::ws; |
144 |
|
|
if (cmdArgs.good()) { |
145 |
|
|
cmdArgs >> name; |
146 |
|
|
} |
147 |
|
|
Entity *entity = &(PoolStorage::getInstance()->getEntity(name)); |
148 |
|
|
pgManager = dynamic_cast<PGManager *>(entity); |
149 |
|
|
} else if (cmdLine == "state") { |
150 |
|
|
cmdArgs >> std::ws; |
151 |
|
|
if (cmdArgs.good()) { |
152 |
|
|
std::string statearg; |
153 |
|
|
cmdArgs >> statearg; |
154 |
|
|
if (statearg == "start") { |
155 |
|
|
state = STATE_STARTING; |
156 |
|
|
} else if (statearg == "stop") { |
157 |
|
|
state = STATE_STOPPING; |
158 |
|
|
} |
159 |
|
|
} else { |
160 |
|
|
os << "state = "; |
161 |
|
|
switch (state) { |
162 |
|
|
case STATE_STARTING: |
163 |
|
|
os << "starting"; |
164 |
|
|
break; |
165 |
|
|
case STATE_STARTED: |
166 |
|
|
os << "started"; |
167 |
|
|
break; |
168 |
|
|
case STATE_STOPPING: |
169 |
|
|
os << "stopping"; |
170 |
|
|
break; |
171 |
|
|
case STATE_STOPPED: |
172 |
|
|
os << "stopped"; |
173 |
|
|
break; |
174 |
|
|
default: |
175 |
|
|
os << "should never happen"; |
176 |
|
|
break; |
177 |
|
|
} |
178 |
|
|
os << std::endl; |
179 |
|
|
} |
180 |
|
|
} else { |
181 |
|
|
} |
182 |
|
|
} |
183 |
|
|
|
184 |
|
|
} // namespace sot |
185 |
|
|
} // namespace dynamicgraph |