diff --git a/CMakeLists.txt b/CMakeLists.txt index 961a478886cfbda5dceb6a027d7b22b02c40d524..59d757e7494268a1a6da8df0bb865235aaf67f74 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 0000000000000000000000000000000000000000..2c47ab05d4e3d790c83f4b990610afbec49699e8 --- /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 0000000000000000000000000000000000000000..bbf355434d3e3a27c498ad1ac062108b3450f8f6 --- /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 ea76f0a21b5d6d0f2296757728501f51004f379c..16a95a4fd20c021ca55350ab76f98bc963be9e5c 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; }