Skip to content

Commit 4f5d48d

Browse files
Abishalinibmagyar
authored andcommitted
Read base parameter in initialize function
1 parent 24174a4 commit 4f5d48d

File tree

1 file changed

+13
-1
lines changed

1 file changed

+13
-1
lines changed

kinematics_interface_kdl/src/kinematics_interface_kdl.cpp

+13-1
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,19 @@ bool KinematicsInterfaceKDL::initialize(
4545
// create kinematic chain
4646
KDL::Tree robot_tree;
4747
kdl_parser::treeFromString(robot_description, robot_tree);
48-
root_name_ = robot_tree.getRootSegment()->first;
48+
49+
// get root name
50+
auto base_param = rclcpp::Parameter();
51+
if (parameters_interface->has_parameter("base"))
52+
{
53+
parameters_interface->get_parameter("base", base_param);
54+
root_name_ = base_param.as_string();
55+
}
56+
else
57+
{
58+
root_name_ = robot_tree.getRootSegment()->first;
59+
}
60+
4961
if (!robot_tree.getChain(root_name_, end_effector_name, chain_))
5062
{
5163
RCLCPP_ERROR(

0 commit comments

Comments
 (0)