1 |
|
|
#include <pinocchio/fwd.hpp> |
2 |
|
|
|
3 |
|
|
// include pinocchio first |
4 |
|
|
// |
5 |
|
|
#include <ros/ros.h> |
6 |
|
|
#include <urdf_parser/urdf_parser.h> |
7 |
|
|
|
8 |
|
|
#include <boost/make_shared.hpp> |
9 |
|
|
#include <boost/shared_ptr.hpp> |
10 |
|
|
#include <sot/core/robot-utils.hh> |
11 |
|
|
#include <stdexcept> |
12 |
|
|
|
13 |
|
|
#include "dynamic_graph_bridge/ros_parameter.hh" |
14 |
|
|
#include "pinocchio/multibody/model.hpp" |
15 |
|
|
#include "pinocchio/parsers/urdf.hpp" |
16 |
|
|
|
17 |
|
|
namespace dynamicgraph { |
18 |
|
|
bool parameter_server_read_robot_description() { |
19 |
|
|
ros::NodeHandle nh; |
20 |
|
|
if (!nh.hasParam("/robot_description")) { |
21 |
|
|
ROS_ERROR("No /robot_description parameter"); |
22 |
|
|
return false; |
23 |
|
|
} |
24 |
|
|
|
25 |
|
|
std::string robot_description; |
26 |
|
|
std::string parameter_name("/robot_description"); |
27 |
|
|
nh.getParam(parameter_name, robot_description); |
28 |
|
|
|
29 |
|
|
std::string model_name("robot"); |
30 |
|
|
|
31 |
|
|
// Search for the robot util related to robot_name. |
32 |
|
|
sot::RobotUtilShrPtr aRobotUtil = sot::getRobotUtil(model_name); |
33 |
|
|
// If does not exist then it is created. |
34 |
|
|
if (aRobotUtil == sot::RefVoidRobotUtil()) |
35 |
|
|
aRobotUtil = sot::createRobotUtil(model_name); |
36 |
|
|
|
37 |
|
|
// If the creation is fine |
38 |
|
|
if (aRobotUtil != sot::RefVoidRobotUtil()) { |
39 |
|
|
// Then set the robot model. |
40 |
|
|
aRobotUtil->set_parameter(parameter_name, robot_description); |
41 |
|
|
ROS_INFO("Set parameter_name : %s.", parameter_name.c_str()); |
42 |
|
|
// Everything went fine. |
43 |
|
|
return true; |
44 |
|
|
} |
45 |
|
|
ROS_ERROR("Wrong initialization of parameter_name %s", |
46 |
|
|
parameter_name.c_str()); |
47 |
|
|
|
48 |
|
|
// Otherwise something went wrong. |
49 |
|
|
return false; |
50 |
|
|
} |
51 |
|
|
|
52 |
|
|
} // namespace dynamicgraph |