[tools] Add set_parameter and get_parameter to robot-utils

include/sot/core/robot-utils.hh

@@ -93,8 +93,10 @@ struct SOT_CORE_EXPORT ForceUtil {
 struct SOT_CORE_EXPORT FootUtil {
   /// Position of the foot soles w.r.t. the frame of the foot
   dynamicgraph::Vector m_Right_Foot_Sole_XYZ;
+
   /// Position of the force/torque sensors w.r.t. the frame of the hosting link
   dynamicgraph::Vector m_Right_Foot_Force_Sensor_XYZ;
+
   std::string m_Left_Foot_Frame_Name;
   std::string m_Right_Foot_Frame_Name;
   void display(std::ostream &os) const;
@@ -212,8 +214,29 @@ public:
 
   void display(std::ostream &os) const;
 
+  /**{ \name Handling general parameters */
+  /** \brief Set a parameter of type string.
+      If parameter_name already exists the value is overwritten.
+      If not it is inserted.
+   */
+  void set_parameter(const std::string &parameter_name,
+                     const std::string &parameter_value);
+
+  /** \brief Get a parameter of type string.
+      If parameter_name already exists the value is overwritten.
+      If not it is inserted.
+      @param parameter_name: Name of the parameter
+      Return false if the parameter is not found.
+   */
+  const std::string &get_parameter(const std::string &parameter_name);
+
+  /** @} */
 protected:
   Logger logger_;
+
+  /** \brief Map of the parameters: map of strings. */
+  std::map<std::string, std::string> parameters_strings_;
+
 }; // struct RobotUtil
 
 /// Accessors - This should be changed to RobotUtilPtrShared

src/tools/robot-utils.cpp

@@ -360,10 +360,21 @@ void RobotUtil::display(std::ostream &os) const {
   }
   os << std::endl;
 }
+
 void RobotUtil::sendMsg(const std::string &msg, MsgType t,
-    const std::string &lineId) {
+                        const std::string &lineId) {
   logger_.sendMsg("[RobotUtil]" + msg, t, lineId);
 }
+
+void RobotUtil::set_parameter(const std::string &parameter_name,
+                              const std::string &parameter_value) {
+  parameters_strings_[parameter_name] = parameter_value;
+}
+
+const std::string &RobotUtil::get_parameter(const std::string &parameter_name) {
+  return parameters_strings_[parameter_name];
+}
+
 bool base_se3_to_sot(ConstRefVector pos, ConstRefMatrix R, RefVector q_sot) {
   assert(q_sot.size() == 6);
   assert(pos.size() == 3);
@@ -442,6 +453,7 @@ bool isNameInRobotUtil(std::string &robotName) {
     return true;
   return false;
 }
+
 RobotUtilShrPtr createRobotUtil(std::string &robotName) {
   std::map<std::string, RobotUtilShrPtr>::iterator it =
       sgl_map_name_to_robot_util.find(robotName);
@@ -451,8 +463,6 @@ RobotUtilShrPtr createRobotUtil(std::string &robotName) {
     it = sgl_map_name_to_robot_util.find(robotName);
     return it->second;
   }
-  std::cout << "Another robot is already in the map for " << robotName
-            << std::endl;
   return RefVoidRobotUtil();
 }
 } // namespace sot