[tools] ParameterServer and RobotUtils are simplified and explained

better.
Thanks to the feedback of Joseph Mirabel.

Author: olivier-stasse

include/sot/core/parameter-server.hh

@@ -104,8 +104,9 @@ public:
                     const Type &ParameterValue)
   {
     if (!m_initSucceeded) {
-      DYNAMIC_GRAPH_ENTITY_WARNING(*this) << "Cannot set parameter " << ParameterName
-        << " to " << ParameterValue << " before initialization!\n";
+      DYNAMIC_GRAPH_ENTITY_WARNING(*this) << "Cannot set parameter " <<
+          ParameterName << " to " << ParameterValue
+          << " before initialization!\n";
       return;
     }
 
@@ -117,8 +118,8 @@ public:
   {
 
     if (!m_initSucceeded) {
-      SEND_WARNING_STREAM_MSG("Cannot get parameter " + ParameterName +
-                              " before initialization!");
+      DYNAMIC_GRAPH_ENTITY_WARNING(*this) << "Cannot get parameter " <<
+          ParameterName << " before initialization!\n";
       Type ParameterValue;
       return ParameterValue;
     }

include/sot/core/robot-utils.hh

@@ -11,26 +11,22 @@
 /* --------------------------------------------------------------------- */
 /* --- INCLUDE --------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
+
+/** pinocchio is forcing the BOOST_MPL_LIMIT_VECTOR_SIZE to a specific value.
+    This happen to be not working when including the boost property_tree library.
+    For this reason if defined, the current value of BOOST_MPL_LIMIT_VECTOR_SIZE
+    is saved in the preprocessor stack and unset.
+    Once the property_tree included the pinocchio value of this variable is
+    restored.
+ */
+
 #ifdef BOOST_MPL_LIMIT_VECTOR_SIZE
 #pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
 #undef BOOST_MPL_LIMIT_VECTOR_SIZE
-#define BOOST_MPL_LIMIT_VECTOR_SIZE_PUSH
-#endif
-
-#ifdef BOOST_MPL_LIMIT_LIST_SIZE
-#pragma push_macro("BOOST_MPL_LIMIT_LIST_SIZE")
-#undef BOOST_MPL_LIMIT_LIST_SIZE
-#define BOOST_MPL_LIMIT_LIST_SIZE_PUSH
-#endif
-
 #include <boost/property_tree/ptree.hpp>
-
-#ifdef BOOST_MPL_LIMIT_VECTOR_SIZE_PUSH
 #pragma pop_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
-#endif
-
-#ifdef BOOST_MPL_LIMIT_LIST_SIZE_PUSH
-#pragma pop_macro("BOOST_MPL_LIMIT_LIST_SIZE")
+#else
+#include <boost/property_tree/ptree.hpp>
 #endif
 
 #include "boost/assign.hpp"
@@ -271,10 +267,13 @@ public:
     }
     catch(const boost::property_tree::ptree_error &e)
     {
-      DYNAMIC_GRAPH_ENTITY_ERROR(*this) << "Robot utils: parameter path is invalid " << '\n'
+      std::ostringstream oss;
+      oss << "Robot utils: parameter path is invalid " << '\n'
           << " for set_parameter("
           << parameter_name << ")\n"
-          << e.what() << '\n';
+          << e.what() << std::endl;
+      sendMsg(oss.str(),
+              MSG_TYPE_ERROR);
       return;
 
     }
@@ -298,10 +297,13 @@ public:
     }
     catch(const boost::property_tree::ptree_error &e)
     {
-      DYNAMIC_GRAPH_ENTITY_ERROR(*this) << "Robot utils: parameter path is invalid " << '\n'
+      std::ostringstream oss;
+      oss << "Robot utils: parameter path is invalid " << '\n'
           << " for get_parameter("
           << parameter_name << ")\n"
-          << e.what() << '\n';
+          << e.what() << std::endl;
+      sendMsg(oss.str(),
+              MSG_TYPE_ERROR);
     }
   }
   /** @} */

src/tools/parameter-server.cpp

@@ -14,26 +14,21 @@
  * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+/** pinocchio is forcing the BOOST_MPL_LIMIT_VECTOR_SIZE to a specific value.
+    This happen to be not working when including the boost property_tree library.
+    For this reason if defined, the current value of BOOST_MPL_LIMIT_VECTOR_SIZE
+    is saved in the preprocessor stack and unset.
+    Once the property_tree included the pinocchio value of this variable is
+    restored.
+ */
+
 #ifdef BOOST_MPL_LIMIT_VECTOR_SIZE
 #pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
 #undef BOOST_MPL_LIMIT_VECTOR_SIZE
-#define BOOST_MPL_LIMIT_VECTOR_SIZE_PUSH
-#endif
-
-#ifdef BOOST_MPL_LIMIT_LIST_SIZE
-#pragma push_macro("BOOST_MPL_LIMIT_LIST_SIZE")
-#undef BOOST_MPL_LIMIT_LIST_SIZE
-#define BOOST_MPL_LIMIT_LIST_SIZE_PUSH
-#endif
-
 #include <boost/property_tree/ptree.hpp>
-
-#ifdef BOOST_MPL_LIMIT_VECTOR_SIZE_PUSH
 #pragma pop_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
-#endif
-
-#ifdef BOOST_MPL_LIMIT_LIST_SIZE_PUSH
-#pragma pop_macro("BOOST_MPL_LIMIT_LIST_SIZE")
+#else
+#include <boost/property_tree/ptree.hpp>
 #endif
 
 #include <dynamic-graph/all-commands.h>

src/tools/robot-utils.cpp

@@ -5,6 +5,13 @@
  *
  */
 
+/** pinocchio is forcing the BOOST_MPL_LIMIT_VECTOR_SIZE to a specific value.
+    This happen to be not working when including the boost property_tree library.
+    For this reason if defined, the current value of BOOST_MPL_LIMIT_VECTOR_SIZE
+    is saved in the preprocessor stack and unset.
+    Once the property_tree included the pinocchio value of this variable is
+    restored.
+ */
 #ifdef BOOST_MPL_LIMIT_VECTOR_SIZE
 #pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
 #undef BOOST_MPL_LIMIT_VECTOR_SIZE
@@ -40,7 +47,9 @@ RobotUtilShrPtr RefVoidRobotUtil() {
 ExtractJointMimics::ExtractJointMimics(std::string & robot_model) {
   // Parsing the model from a string.
   std::istringstream iss(robot_model);
+  /// Read the XML file in the property tree.
   boost::property_tree::read_xml(iss,tree_);
+  /// Start the recursive parsing.
   go_through_full();
 }
 
@@ -50,40 +59,56 @@ ExtractJointMimics::get_mimic_joints() {
 }
 
 void ExtractJointMimics::go_through_full()  {
+  /// Root of the recursive parsing.
   current_joint_name_="";
   go_through(tree_,0,0);
 }
 
 void ExtractJointMimics::go_through(pt::ptree &pt, int level, int stage)
 {
+  /// If pt is empty (i.e. this is a leaf)
   if (pt.empty()) {
+    /// and this is a name of a joint (stage == 3) update the
+    /// curret_joint_name_ variable.
     if (stage==3)
       current_joint_name_=pt.data();
   }
   else {
 
+    /// This is not a leaf
     for (auto pos : pt) {
       int new_stage = stage;
 
+      /// But this is joint
       if (pos.first=="joint")
+        /// the continue the exploration.
         new_stage=1;
       else if (pos.first=="<xmlattr>")
       {
+        /// we are exploring the xml attributes of a joint
+        /// -> continue the exploration
         if (stage==1)
           new_stage=2;
       }
+      /// The xml attribute of the joint is the name
+      /// next leaf is the name we are possibly looking for
       else if (pos.first=="name")
       {
         if (stage==2)
           new_stage=3;
       }
+      /// The exploration of the tree tracback on the joint
+      /// and find that this is a mimic joint.
       else if (pos.first=="mimic")
       {
         if (stage==1)
+          /// Save the current name of the joint
+          /// in mimic_joints.
           mimic_joints_.push_back(current_joint_name_);
       }
       else new_stage=0;
 
+      /// Explore the subtree of the XML robot description.
       go_through(pos.second, level + 1,new_stage);
 
     }