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> 29 namespace talos_balance
31 namespace dg = ::dynamicgraph;
36 #define PROFILE_EXAMPLE_SUM_COMPUTATION "Example: sum computation " 37 #define PROFILE_EXAMPLE_NBJOINTS_COMPUTATION "Example: nbJoints extraction " 39 #define INPUT_SIGNALS m_firstAddendSIN << m_secondAddendSIN 41 #define OUTPUT_SIGNALS m_sumSOUT << m_nbJointsSOUT 56 , CONSTRUCT_SIGNAL_IN(firstAddend, double)
57 , CONSTRUCT_SIGNAL_IN(secondAddend, double)
60 , m_initSucceeded(false)
65 addCommand(
"init", makeCommandVoid1(*
this, &
Example::init, docCommandVoid1(
"Initialize the entity.",
"Robot name")));
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);
76 std::string robotName_nonconst(robotName);
78 if (!isNameInRobotUtil(robotName_nonconst))
80 SEND_MSG(
"You should have a robotUtil pointer initialized before",MSG_TYPE_ERROR);
85 std::cerr <<
"m_robot_util:" <<
m_robot_util << std::endl;
87 for (
unsigned int i = 0; i<4; i++)
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);
107 SEND_WARNING_STREAM_MSG(
"Cannot compute signal sum before initialization!");
113 double firstAddend = m_firstAddendSIN(iter);
114 double secondAddend = m_secondAddendSIN(iter);
116 s = firstAddend + secondAddend;
127 SEND_WARNING_STREAM_MSG(
"Cannot compute signal nbJoints before initialization!");
150 os <<
"Example " << getName();
153 getProfiler().report_all(3, os);
155 catch (ExceptionSignal e) {}
void init(const std::string &robotName)
#define PROFILE_EXAMPLE_NBJOINTS_COMPUTATION
AdmittanceControllerEndEffector EntityClassName
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Example(const std::string &name)
RobotUtilShrPtr m_robot_util
true if the entity has been successfully initialized
virtual void display(std::ostream &os) const
#define PROFILE_EXAMPLE_SUM_COMPUTATION
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector)