[debug] Fix sendMsg recursive call leading to dramatic failure.

include/sot/torque_control/admittance-controller.hh

@@ -108,7 +108,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("["+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("["+name+"] "+msg, t, file, line);
         }
 
       protected:

include/sot/torque_control/base-estimator.hh

@@ -174,7 +174,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("["+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("["+name+"] "+msg, t, file, line);
         }
 
       protected:

include/sot/torque_control/control-manager.hh

@@ -155,7 +155,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("[ControlManager-"+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("[ControlManager-"+name+"] "+msg, t, file, line);
         }
 
       protected:

include/sot/torque_control/current-controller.hh

@@ -115,7 +115,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("[CurrentController-"+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("[CurrentController-"+name+"] "+msg, t, file, line);
         }
 
       protected:

include/sot/torque_control/device-torque-ctrl.hh

@@ -98,7 +98,7 @@ namespace dynamicgraph
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("[DeviceTorqueCtrl] "+msg, t, file, line);
+	  Entity::sendMsg("[DeviceTorqueCtrl] "+msg, t, file, line);
         }
 
         /// \brief Current integration step.

include/sot/torque_control/filter-differentiator.hh

@@ -106,7 +106,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("["+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("["+name+"] "+msg, t, file, line);
         }
 
       public: /* --- ENTITY INHERITANCE --- */

include/sot/torque_control/free-flyer-locator.hh

@@ -97,7 +97,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("[FreeFlyerLocator-"+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("[FreeFlyerLocator-"+name+"] "+msg, t, file, line);
         }
 
       protected:

include/sot/torque_control/imu_offset_compensation.hh

@@ -84,7 +84,7 @@ namespace dynamicgraph {
         void update_offset_impl(int iter);
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("[ImuOffsetCompensation-"+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("[ImuOffsetCompensation-"+name+"] "+msg, t, file, line);
         }
 
       protected:

include/sot/torque_control/inverse-dynamics-balance-controller.hh

@@ -207,7 +207,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("["+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("["+name+"] "+msg, t, file, line);
         }
 
       protected:

include/sot/torque_control/joint-torque-controller.hh

@@ -128,7 +128,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("["+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("["+name+"] "+msg, t, file, line);
         }
 
       public:

include/sot/torque_control/joint-trajectory-generator.hh

@@ -163,7 +163,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("[JointTrajectoryGenerator-"+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("[JointTrajectoryGenerator-"+name+"] "+msg, t, file, line);
         }
 
       protected:

include/sot/torque_control/madgwickahrs.hh

@@ -97,7 +97,7 @@ namespace dynamicgraph {
         //void madgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("[MadgwickAHRS-"+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("[MadgwickAHRS-"+name+"] "+msg, t, file, line);
         }
 
       protected:

include/sot/torque_control/nd-trajectory-generator.hh

@@ -148,7 +148,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("[NdTrajectoryGenerator-"+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("[NdTrajectoryGenerator-"+name+"] "+msg, t, file, line);
         }
 
       protected:

include/sot/torque_control/numerical-difference.hh

@@ -117,7 +117,7 @@ namespace dynamicgraph {
       protected:
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("["+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("["+name+"] "+msg, t, file, line);
         }
 
       public: /* --- ENTITY INHERITANCE --- */

include/sot/torque_control/position-controller.hh

@@ -89,7 +89,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("[PositionController-"+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("[PositionController-"+name+"] "+msg, t, file, line);
         }
 
       protected:

include/sot/torque_control/se3-trajectory-generator.hh

@@ -140,7 +140,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("[SE3TrajectoryGenerator-"+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("[SE3TrajectoryGenerator-"+name+"] "+msg, t, file, line);
         }
 
       protected:

include/sot/torque_control/torque-offset-estimator.hh

@@ -114,7 +114,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("["+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("["+name+"] "+msg, t, file, line);
         }
 
       private:

include/sot/torque_control/trace-player.hh

@@ -94,7 +94,7 @@ namespace dynamicgraph {
 
         void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
         {
-          sendMsg("["+name+"] "+msg, t, file, line);
+	  Entity::sendMsg("["+name+"] "+msg, t, file, line);
         }
 
       protected:

src/control-manager.cpp

@@ -199,15 +199,15 @@ namespace dynamicgraph
         std::string localName(robotRef);
         if (!isNameInRobotUtil(localName))
         {
-            m_robot_util = createRobotUtil(localName);
-            std::cout << "createRobotUtil success" << std::endl;
+	  m_robot_util = createRobotUtil(localName);
+	  SEND_MSG("createRobotUtil success\n", MSG_TYPE_INFO);
         }
         else
         {
-            m_robot_util = getRobotUtil(localName);
-            std::cout << "getRobotUtil success" << std::endl;
+	  m_robot_util = getRobotUtil(localName);
+	  SEND_MSG("getRobotUtil success\n",MSG_TYPE_INFO);
         }
-        std::cout << m_robot_util->m_urdf_filename << std::endl;
+        SEND_MSG( m_robot_util->m_urdf_filename,MSG_TYPE_INFO);
         m_robot_util->m_urdf_filename = urdfFile;
         addCommand("getJointsUrdfToSot",
                    makeDirectGetter(*this, &m_robot_util->m_dgv_urdf_to_sot,