sot-core  4.11.2
Hierarchical task solver plug-in for dynamic-graph.
variadic-op.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2018,
3  * Mirabel Joseph
4  *
5  * CNRS/AIST
6  *
7  */
8 
9 #ifndef SOT_CORE_VARIADICOP_HH
10 #define SOT_CORE_VARIADICOP_HH
11 
12 /* --------------------------------------------------------------------- */
13 /* --- INCLUDE --------------------------------------------------------- */
14 /* --------------------------------------------------------------------- */
15 
16 /* Matrix */
17 #include <dynamic-graph/linear-algebra.h>
18 
19 /* SOT */
20 #include <dynamic-graph/all-signals.h>
21 #include <dynamic-graph/entity.h>
22 #include <sot/core/flags.hh>
24 #include <sot/core/pool.hh>
25 
26 /* STD */
27 #include <string>
28 
29 #include <boost/function.hpp>
30 
31 namespace dynamicgraph {
32 namespace sot {
33 
34 /* --------------------------------------------------------------------- */
35 /* --- CLASS ----------------------------------------------------------- */
36 /* --------------------------------------------------------------------- */
37 
38 template <typename Tin, typename Tout, typename Time>
39 class VariadicAbstract : public Entity {
40 public: /* --- CONSTRUCTION --- */
41  static std::string getTypeInName(void);
42  static std::string getTypeOutName(void);
43 
44  VariadicAbstract(const std::string &name, const std::string &className)
45  : Entity(name), SOUT(className + "(" + name + ")::output(" +
46  getTypeOutName() + ")::sout"),
47  baseSigname(className + "(" + name + ")::input(" + getTypeInName() +
48  ")::") {
49  signalRegistration(SOUT);
50  }
51 
52  virtual ~VariadicAbstract(void) {
53  for (std::size_t i = 0; i < signalsIN.size(); ++i) {
54  _removeSignal(i);
55  }
56  };
57 
58 public: /* --- SIGNAL --- */
59  typedef SignalPtr<Tin, int> signal_t;
60  SignalTimeDependent<Tout, int> SOUT;
61 
62  std::size_t addSignal() {
63  std::ostringstream oss;
64  oss << "sin" << signalsIN.size();
65  return addSignal(oss.str());
66  }
67 
68  std::size_t addSignal(const std::string &name) {
69  signal_t *sig = new signal_t(NULL, baseSigname + name);
70  try {
71  _declareSignal(sig);
72  signalsIN.push_back(sig);
73  // names.push_back (name);
74  return signalsIN.size() - 1;
75  } catch (const ExceptionAbstract &) {
76  delete sig;
77  throw;
78  }
79  }
80 
81  void removeSignal() {
82  assert(signalsIN.size() > 0);
83  _removeSignal(signalsIN.size() - 1);
84  // names.pop_back();
85  signalsIN.pop_back();
86  }
87 
88  void setSignalNumber(const int &n) {
89  assert(n >= 0);
90  const std::size_t oldSize = signalsIN.size();
91  for (std::size_t i = n; i < oldSize; ++i)
92  _removeSignal(i);
93  signalsIN.resize(n, NULL);
94  // names.resize(n);
95 
96  for (std::size_t i = oldSize; i < (std::size_t)n; ++i) {
97  assert(signalsIN[i] == NULL);
98  std::ostringstream oss;
99  oss << baseSigname << "sin" << i;
100  // names[i] = oss.str();
101  // signal_t* s = new signal_t (NULL,names[i]);
102  signal_t *s = new signal_t(NULL, oss.str());
103  signalsIN[i] = s;
104  _declareSignal(s);
105  }
107  }
108 
109  int getSignalNumber() const { return (int)signalsIN.size(); }
110 
111  signal_t *getSignalIn(int i) {
112  if (i < 0 || i >= (int)signalsIN.size())
113  throw std::out_of_range("Wrong signal index");
114  return signalsIN[i];
115  }
116 
117 protected:
118  std::vector<signal_t *> signalsIN;
119  // Use signal->shortName instead
120  // std::vector< std::string > names;
121 
122  virtual void updateSignalNumber(int n) { (void)n; };
123 
124 private:
125  void _removeSignal(const std::size_t i) {
126  // signalDeregistration(names[i]);
127  signalDeregistration(signalsIN[i]->shortName());
128  SOUT.removeDependency(*signalsIN[i]);
129  delete signalsIN[i];
130  }
131  void _declareSignal(signal_t *s) {
132  signalRegistration(*s);
133  SOUT.addDependency(*s);
134  }
135  const std::string baseSigname;
136 };
137 
138 template <typename Operator>
139 class VariadicOp : public VariadicAbstract<typename Operator::Tin,
140  typename Operator::Tout, int> {
141  typedef typename Operator::Tin Tin;
142  typedef typename Operator::Tout Tout;
143  typedef VariadicOp<Operator> Self;
144 
145 public: /* --- CONSTRUCTION --- */
146  Operator op;
147 
149 
150  // static std::string getTypeInName ( void ) { return Operator::nameTypeIn ();
151  // } static std::string getTypeOutName( void ) { return
152  // Operator::nameTypeOut(); }
153  static const std::string CLASS_NAME;
154  virtual const std::string &getClassName() const { return CLASS_NAME; }
155  std::string getDocString() const { return op.getDocString(); }
156 
157  VariadicOp(const std::string &name) : Base(name, CLASS_NAME) {
158  this->SOUT.setFunction(boost::bind(&Self::computeOperation, this, _1, _2));
159  op.initialize(this, this->commandMap);
160  }
161 
162  virtual ~VariadicOp(void){};
163 
164 protected:
165  Tout &computeOperation(Tout &res, int time) {
166  std::vector<const Tin *> in(this->signalsIN.size());
167  for (std::size_t i = 0; i < this->signalsIN.size(); ++i) {
168  const Tin &x = this->signalsIN[i]->access(time);
169  in[i] = &x;
170  }
171  op(in, res);
172  return res;
173  }
174 
175  inline void updateSignalNumber(int n) { op.updateSignalNumber(n); }
176 };
177 } // namespace sot
178 } // namespace dynamicgraph
179 
180 #endif // #ifndef SOT_CORE_VARIADICOP_HH
void setSignalNumber(const int &n)
Definition: variadic-op.hh:88
static std::string getTypeInName(void)
static std::string getTypeOutName(void)
VariadicAbstract< Tin, Tout, int > Base
Definition: variadic-op.hh:148
std::vector< signal_t * > signalsIN
Definition: variadic-op.hh:118
VariadicOp(const std::string &name)
Definition: variadic-op.hh:157
Tout & computeOperation(Tout &res, int time)
Definition: variadic-op.hh:165
std::size_t addSignal()
Definition: variadic-op.hh:62
Operator op
Definition: variadic-op.hh:146
Definition: exception-abstract.hh:35
Definition: variadic-op.hh:39
VariadicAbstract(const std::string &name, const std::string &className)
Definition: variadic-op.hh:44
SignalPtr< Tin, int > signal_t
Definition: variadic-op.hh:56
virtual ~VariadicAbstract(void)
Definition: variadic-op.hh:52
void updateSignalNumber(int n)
Definition: variadic-op.hh:175
SignalTimeDependent< Tout, int > SOUT
Definition: variadic-op.hh:60
void removeSignal()
Definition: variadic-op.hh:81
virtual ~VariadicOp(void)
Definition: variadic-op.hh:162
std::string getDocString() const
Definition: variadic-op.hh:155
std::size_t addSignal(const std::string &name)
Definition: variadic-op.hh:68
signal_t * getSignalIn(int i)
Definition: variadic-op.hh:111
virtual void updateSignalNumber(int n)
Definition: variadic-op.hh:122
Definition: variadic-op.hh:139
Definition: abstract-sot-external-interface.hh:17
static const std::string CLASS_NAME
Definition: variadic-op.hh:153
int getSignalNumber() const
Definition: variadic-op.hh:109
virtual const std::string & getClassName() const
Definition: variadic-op.hh:154