sot-talos-balance  1.7.0
example.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2018, Gepetto team, LAAS-CNRS
3  *
4  * This file is part of sot-talos-balance.
5  * sot-talos-balance is free software: you can redistribute it and/or
6  * modify it under the terms of the GNU Lesser General Public License
7  * as published by the Free Software Foundation, either version 3 of
8  * the License, or (at your option) any later version.
9  * sot-talos-balance is distributed in the hope that it will be
10  * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11  * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details. You should
13  * have received a copy of the GNU Lesser General Public License along
14  * with sot-talos-balance. If not, see <http://www.gnu.org/licenses/>.
15  */
16 
18 
19 #include <sot/core/debug.hh>
20 #include <dynamic-graph/factory.h>
21 #include <dynamic-graph/all-commands.h>
22 #include <sot/core/stop-watch.hh>
23 
24 
25 namespace dynamicgraph
26 {
27  namespace sot
28  {
29  namespace talos_balance
30  {
31  namespace dg = ::dynamicgraph;
32  using namespace dg;
33  using namespace dg::command;
34 
35 //Size to be aligned "-------------------------------------------------------"
36 #define PROFILE_EXAMPLE_SUM_COMPUTATION "Example: sum computation "
37 #define PROFILE_EXAMPLE_NBJOINTS_COMPUTATION "Example: nbJoints extraction "
38 
39 #define INPUT_SIGNALS m_firstAddendSIN << m_secondAddendSIN
40 
41 #define OUTPUT_SIGNALS m_sumSOUT << m_nbJointsSOUT
42 
45  typedef Example EntityClassName;
46 
47  /* --- DG FACTORY ---------------------------------------------------- */
49  "Example");
50 
51  /* ------------------------------------------------------------------- */
52  /* --- CONSTRUCTION -------------------------------------------------- */
53  /* ------------------------------------------------------------------- */
54  Example::Example(const std::string& name)
55  : Entity(name)
56  , CONSTRUCT_SIGNAL_IN(firstAddend, double)
57  , CONSTRUCT_SIGNAL_IN(secondAddend, double)
58  , CONSTRUCT_SIGNAL_OUT(sum, double, INPUT_SIGNALS)
59  , CONSTRUCT_SIGNAL_OUT(nbJoints, int, INPUT_SIGNALS)
60  , m_initSucceeded(false)
61  {
62  Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
63 
64  /* Commands. */
65  addCommand("init", makeCommandVoid1(*this, &Example::init, docCommandVoid1("Initialize the entity.","Robot name")));
66  }
67 
68  void Example::init(const std::string& robotName)
69  {
70  if(!m_firstAddendSIN.isPlugged())
71  return SEND_MSG("Init failed: signal firstAddend is not plugged", MSG_TYPE_ERROR);
72  if(!m_secondAddendSIN.isPlugged())
73  return SEND_MSG("Init failed: signal secondAddend is not plugged", MSG_TYPE_ERROR);
74 
75  /*std::string & robotName_nonconst = const_cast<std::string &>(robotName);*/
76  std::string robotName_nonconst(robotName);
77 
78  if (!isNameInRobotUtil(robotName_nonconst))
79  {
80  SEND_MSG("You should have a robotUtil pointer initialized before",MSG_TYPE_ERROR);
81  }
82  else
83  {
84  m_robot_util = getRobotUtil(robotName_nonconst);
85  std::cerr << "m_robot_util:" << m_robot_util << std::endl;
86  }
87  for (unsigned int i = 0; i<4; i++)
88  {
89  std::cout << "Verbosity Level :" << i << std::endl;
90  Example::setLoggerVerbosityLevel((dynamicgraph::LoggerVerbosity) i);
91  Example::sendMsg("TEST MSG ERROR",dynamicgraph::MSG_TYPE_ERROR);
92  Example::sendMsg("TEST MSG DEBUG",dynamicgraph::MSG_TYPE_DEBUG);
93  Example::sendMsg("TEST MSG INFO",dynamicgraph::MSG_TYPE_INFO);
94  Example::sendMsg("TEST MSG WARNING",dynamicgraph::MSG_TYPE_WARNING);
95  }
96  m_initSucceeded = true;
97  }
98 
99  /* ------------------------------------------------------------------- */
100  /* --- SIGNALS ------------------------------------------------------- */
101  /* ------------------------------------------------------------------- */
102 
104  {
105  if(!m_initSucceeded)
106  {
107  SEND_WARNING_STREAM_MSG("Cannot compute signal sum before initialization!");
108  return s;
109  }
110 
111  getProfiler().start(PROFILE_EXAMPLE_SUM_COMPUTATION);
112 
113  double firstAddend = m_firstAddendSIN(iter);
114  double secondAddend = m_secondAddendSIN(iter);
115 
116  s = firstAddend + secondAddend;
117 
118  getProfiler().stop(PROFILE_EXAMPLE_SUM_COMPUTATION);
119 
120  return s;
121  }
122 
124  {
125  if(!m_initSucceeded)
126  {
127  SEND_WARNING_STREAM_MSG("Cannot compute signal nbJoints before initialization!");
128  return s;
129  }
130  (void)iter;
131 
132  getProfiler().start(PROFILE_EXAMPLE_NBJOINTS_COMPUTATION);
133 
134  s = int(m_robot_util->m_nbJoints);
135 
136  getProfiler().stop(PROFILE_EXAMPLE_NBJOINTS_COMPUTATION);
137 
138  return s;
139  }
140 
141 
142  /* --- COMMANDS ---------------------------------------------------------- */
143 
144  /* ------------------------------------------------------------------- */
145  /* --- ENTITY -------------------------------------------------------- */
146  /* ------------------------------------------------------------------- */
147 
148  void Example::display(std::ostream& os) const
149  {
150  os << "Example " << getName();
151  try
152  {
153  getProfiler().report_all(3, os);
154  }
155  catch (ExceptionSignal e) {}
156  }
157 
158  } // namespace talos_balance
159  } // namespace sot
160 } // namespace dynamicgraph
161 
void init(const std::string &robotName)
Definition: example.cpp:68
#define PROFILE_EXAMPLE_NBJOINTS_COMPUTATION
Definition: example.cpp:37
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Example(const std::string &name)
Definition: example.cpp:54
RobotUtilShrPtr m_robot_util
true if the entity has been successfully initialized
Definition: example.hh:79
#define OUTPUT_SIGNALS
Definition: example.cpp:41
virtual void display(std::ostream &os) const
Definition: example.cpp:148
#define PROFILE_EXAMPLE_SUM_COMPUTATION
Definition: example.cpp:36
#define INPUT_SIGNALS
Definition: example.cpp:39