GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/step-time-line.cpp Lines: 0 95 0.0 %
Date: 2023-06-05 08:59:09 Branches: 0 106 0.0 %

Line Branch Exec Source
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