@@ -27,14 +27,17 @@ bool KinematicsInterfaceKDL::initialize(
27
27
{
28
28
// track initialization plugin
29
29
initialized = true ;
30
+
31
+ // get parameters
32
+ std::string ns = !param_namespace.empty () ? param_namespace + " ." : " " ;
30
33
31
34
std::string robot_description_local;
32
35
if (robot_description.empty ())
33
36
{
34
37
// If the robot_description input argument is empty, try to get the
35
38
// robot_description from the node's parameters.
36
39
auto robot_param = rclcpp::Parameter ();
37
- if (!parameters_interface->get_parameter (param_namespace + " robot_description" , robot_param))
40
+ if (!parameters_interface->get_parameter (ns + " robot_description" , robot_param))
38
41
{
39
42
RCLCPP_ERROR (LOGGER, " parameter robot_description not set in kinematics_interface_kdl" );
40
43
return false ;
@@ -46,8 +49,6 @@ bool KinematicsInterfaceKDL::initialize(
46
49
robot_description_local = robot_description;
47
50
}
48
51
49
- // get parameters
50
- std::string ns = !param_namespace.empty () ? param_namespace + " ." : " " ;
51
52
// get alpha damping term
52
53
auto alpha_param = rclcpp::Parameter (" alpha" , 0.000005 );
53
54
if (parameters_interface->has_parameter (ns + " alpha" ))
@@ -57,16 +58,28 @@ bool KinematicsInterfaceKDL::initialize(
57
58
alpha = alpha_param.as_double ();
58
59
// get end-effector name
59
60
auto end_effector_name = rclcpp::Parameter (" tip" , " MISSING_END_EFFECTOR_NAME" );
60
- if (parameters_interface->has_parameter (ns + " tip" ))
61
+ if (parameters_interface->has_parameter (param_namespace + " tip" ))
61
62
{
62
63
parameters_interface->get_parameter (ns + " tip" , end_effector_name);
63
64
}
64
65
65
66
// create kinematic chain
66
67
KDL::Tree robot_tree;
67
68
kdl_parser::treeFromString (robot_description_local, robot_tree);
68
- root_name_ = robot_tree.getRootSegment ()->first ;
69
- if (!robot_tree.getChain (root_name_, end_effector_name.as_string (), chain_))
69
+
70
+ // get root name
71
+ auto base_param = rclcpp::Parameter ();
72
+ if (parameters_interface->has_parameter (ns + " base" ))
73
+ {
74
+ parameters_interface->get_parameter (ns + " base" , base_param);
75
+ root_name_ = base_param.as_string ();
76
+ }
77
+ else
78
+ {
79
+ root_name_ = robot_tree.getRootSegment ()->first ;
80
+ }
81
+
82
+ if (!robot_tree.getChain (root_name_, end_effector_name, chain_))
70
83
{
71
84
RCLCPP_ERROR (
72
85
LOGGER, " failed to find chain from robot root %s to end effector %s" , root_name_.c_str (),
0 commit comments