From d2c32ca6b751ba83cf6db0237c240d10062e2a34 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Tue, 23 Jun 2020 19:17:13 +0200
Subject: [PATCH] At initialization time read the ros param /robot_description
and record it in the sot parameter-server.
---
CMakeLists.txt | 4 +-
include/dynamic_graph_bridge/ros_parameter.hh | 9 +++
src/ros_parameter.cpp | 55 +++++++++++++++++++
src/sot_loader_basic.cpp | 3 +
4 files changed, 70 insertions(+), 1 deletion(-)
create mode 100644 include/dynamic_graph_bridge/ros_parameter.hh
create mode 100644 src/ros_parameter.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 961a478..59d757e 100644
--- a/CMakeLists.txt
+++ b/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)
diff --git a/include/dynamic_graph_bridge/ros_parameter.hh b/include/dynamic_graph_bridge/ros_parameter.hh
new file mode 100644
index 0000000..2c47ab0
--- /dev/null
+++ b/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_ */
diff --git a/src/ros_parameter.cpp b/src/ros_parameter.cpp
new file mode 100644
index 0000000..bbf3554
--- /dev/null
+++ b/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;
+
+}
+
+};
diff --git a/src/sot_loader_basic.cpp b/src/sot_loader_basic.cpp
index ea76f0a..16a95a4 100644
--- a/src/sot_loader_basic.cpp
+++ b/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; }
--
GitLab