Read base parameter in initialize function (#73)

ros-controls · May 20, 2024 · 2f02ebd · 2f02ebd
1 parent 24174a4
commit 2f02ebd
Showing 1 changed file with 13 additions and 1 deletion.
diff --git a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp
@@ -45,7 +45,19 @@ bool KinematicsInterfaceKDL::initialize(
   // create kinematic chain
   KDL::Tree robot_tree;
   kdl_parser::treeFromString(robot_description, robot_tree);
-  root_name_ = robot_tree.getRootSegment()->first;
+
+  // get root name
+  auto base_param = rclcpp::Parameter();
+  if (parameters_interface->has_parameter("base"))
+  {
+    parameters_interface->get_parameter("base", base_param);
+    root_name_ = base_param.as_string();
+  }
+  else
+  {
+    root_name_ = robot_tree.getRootSegment()->first;
+  }
+
   if (!robot_tree.getChain(root_name_, end_effector_name, chain_))
   {
     RCLCPP_ERROR(