At initialization time read the ros param /robot_description

and record it in the sot parameter-server.

Author: olivier-stasse

CMakeLists.txt

@@ -66,13 +66,15 @@ set(${PROJECT_NAME}_HEADERS
 SET(${PROJECT_NAME}_SOURCES
   src/ros_init.cpp
   src/sot_to_ros.cpp
+  src/ros_parameter.cpp
   )
 
 ADD_LIBRARY(ros_bridge SHARED
   ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
 TARGET_INCLUDE_DIRECTORIES(ros_bridge SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
 TARGET_INCLUDE_DIRECTORIES(ros_bridge PUBLIC $<INSTALL_INTERFACE:include>)
-TARGET_LINK_LIBRARIES(ros_bridge ${catkin_LIBRARIES} sot-core::sot-core)
+TARGET_LINK_LIBRARIES(ros_bridge ${catkin_LIBRARIES}
+  sot-core::sot-core pinocchio::pinocchio)
 pkg_config_use_dependency(ros_bridge dynamic_graph_bridge_msgs)
 
 IF(SUFFIX_SO_VERSION)

include/dynamic_graph_bridge/ros_parameter.hh

@@ -0,0 +1,9 @@
+#ifndef _ROS_DYNAMIC_GRAPH_PARAMETER_
+#define _ROS_DYNAMIC_GRAPH_PARAMETER_
+
+namespace dynamicgraph {
+
+bool parameter_server_read_robot_description();
+
+};
+#endif /* _ROS_DYNAMIC_GRAPH_PARAMETER_ */

src/ros_parameter.cpp

@@ -0,0 +1,55 @@
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+
+#include <stdexcept>
+#include <boost/make_shared.hpp>
+#include <boost/shared_ptr.hpp>
+
+#include <urdf_parser/urdf_parser.h>
+
+#include <sot/core/robot-utils.hh>
+#include <ros/ros.h>
+#include "dynamic_graph_bridge/ros_parameter.hh"
+
+namespace dynamicgraph {
+bool parameter_server_read_robot_description()
+{
+  ros::NodeHandle nh;
+  if (!nh.hasParam("/robot_description"))
+  {
+    ROS_ERROR("No /robot_description parameter");
+    return false;
+  }
+
+  std::string robot_description;
+  std::string parameter_name("/robot_description");
+  nh.getParam(parameter_name,robot_description);
+
+  pinocchio::Model model;
+  pinocchio::urdf::buildModelFromXML(robot_description, model);
+
+  ROS_INFO("Robot name : %s.",model.name.c_str());
+  // Search for the robot util related to robot_name.
+  sot::RobotUtilShrPtr aRobotUtil = sot::getRobotUtil(model.name);
+  // If does not exist then it is created.
+  if (aRobotUtil != sot::RefVoidRobotUtil())
+    aRobotUtil = sot::createRobotUtil(model.name);
+
+  // If the creation is fine 
+  if (aRobotUtil != sot::RefVoidRobotUtil())
+  {
+    // Then set the robot model.
+    aRobotUtil->set_parameter(parameter_name,robot_description);
+    ROS_INFO("Set parameter_name : %s.",parameter_name.c_str());
+    // Everything went fine.
+    return true;
+  }
+  ROS_ERROR("Wrong initialization of parameter_name %s",
+            parameter_name.c_str());
+
+  // Otherwise something went wrong.
+  return false;
+  
+}
+
+};

src/sot_loader_basic.cpp

@@ -11,6 +11,7 @@
 
 #include <dynamic_graph_bridge/sot_loader.hh>
 #include "dynamic_graph_bridge/ros_init.hh"
+#include "dynamic_graph_bridge/ros_parameter.hh"
 
 #include <dynamic-graph/pool.h>
 
@@ -41,6 +42,8 @@ void SotLoaderBasic::initializeRosNode(int, char* []) {
   service_start_ = n.advertiseService("start_dynamic_graph", &SotLoaderBasic::start_dg, this);
 
   service_stop_ = n.advertiseService("stop_dynamic_graph", &SotLoaderBasic::stop_dg, this);
+
+  dynamicgraph::parameter_server_read_robot_description();
 }
 
 void SotLoaderBasic::setDynamicLibraryName(std::string& afilename) { dynamicLibraryName_ = afilename; }