At initialization read /robot_description model

and save it in the internal sot based parameter-server.
stack-of-tasks · Jun 30, 2020 · c239120 · c239120
1 parent eafb019
commit c239120
Show file tree

Hide file tree

Showing 4 changed files with 57 additions and 0 deletions.
diff --git a/include/sot/core/parameter-server.hh b/include/sot/core/parameter-server.hh
@@ -66,6 +66,11 @@ public:
   void init(const double &dt, const std::string &urdfFile,
             const std::string &robotRef);
 
+  /// Initialize
+  /// The dtparam is found from ros_param
+  /// The urdf model is found by reading /robot_description
+  /// The robot name is found using the name inside robot_description
+  void init_simple();
   /* --- SIGNALS --- */
 
   /* --- COMMANDS --- */

diff --git a/include/sot/core/robot-utils.hh b/include/sot/core/robot-utils.hh
@@ -246,6 +246,7 @@ RobotUtilShrPtr RefVoidRobotUtil();
 RobotUtilShrPtr getRobotUtil(std::string &robotName);
 bool isNameInRobotUtil(std::string &robotName);
 RobotUtilShrPtr createRobotUtil(std::string &robotName);
+std::shared_ptr< std::vector<std::string> > getListOfRobots();
 
 bool base_se3_to_sot(ConstRefVector pos, ConstRefMatrix R, RefVector q_sot);
 

diff --git a/src/tools/parameter-server.cpp b/src/tools/parameter-server.cpp
@@ -60,6 +60,9 @@ ParameterServer::ParameterServer(const std::string &name)
                                               "Time period in seconds (double)",
                                               "URDF file path (string)",
                                               "Robot reference (string)")));
+  addCommand("init_simple",
+             makeCommandVoid0(*this, &ParameterServer::init_simple,
+                              docCommandVoid0("Initialize the entity.")));
 
   addCommand("setNameToId",
              makeCommandVoid2(*this, &ParameterServer::setNameToId,
@@ -138,6 +141,39 @@ ParameterServer::ParameterServer(const std::string &name)
                                      "(string) ParameterName")));
 }
 
+void ParameterServer::init_simple() {
+
+  m_emergency_stop_triggered = false;
+  m_initSucceeded = true;
+
+  std::string localName;
+  std::shared_ptr< std::vector<std::string> >
+      listOfRobots = sot::getListOfRobots();
+
+  std::cerr << "listOfRobots.size()="
+            << listOfRobots->size()
+            << std::endl;
+
+  if (listOfRobots->size()==1)
+    localName = (*listOfRobots)[0];
+
+  std::cerr << "localName" << localName.c_str()
+            << std::endl;
+
+  if (!isNameInRobotUtil(localName)) {
+    m_robot_util = createRobotUtil(localName);
+  } else {
+    m_robot_util = getRobotUtil(localName);
+  }
+
+  addCommand(
+      "getJointsUrdfToSot",
+      makeDirectGetter(*this, &m_robot_util->m_dgv_urdf_to_sot,
+                       docDirectSetter("Display map Joints From URDF to SoT.",
+                                       "Vector of integer for mapping")));
+
+}
+
 void ParameterServer::init(const double &dt, const std::string &urdfFile,
                            const std::string &robotRef) {
   if (dt <= 0.0)

diff --git a/src/tools/robot-utils.cpp b/src/tools/robot-utils.cpp
@@ -438,6 +438,21 @@ bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) {
 
 std::map<std::string, RobotUtilShrPtr> sgl_map_name_to_robot_util;
 
+std::shared_ptr<std::vector<std::string> >  getListOfRobots() {
+  std::shared_ptr<std::vector<std::string> > res =
+      std::make_shared<std::vector<std::string> >();
+
+  std::map<std::string, RobotUtilShrPtr>::iterator it =
+      sgl_map_name_to_robot_util.begin();
+  while (it != sgl_map_name_to_robot_util.end())
+  {
+    res->push_back(it->first);
+    it++;
+  }
+
+  return res;
+}
+
 RobotUtilShrPtr getRobotUtil(std::string &robotName) {
   std::map<std::string, RobotUtilShrPtr>::iterator it =
       sgl_map_name_to_robot_util.find(robotName);