Pass the robot_description as an arg in initialize() with fallback

ros-controls · Apr 9, 2024 · bbcea68 · bbcea68
1 parent c8e4791
commit bbcea68
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 10 deletions.
diff --git a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp
@@ -42,7 +42,8 @@ class KinematicsInterface
    */
   virtual bool initialize(
     std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface,
-    const std::string & end_effector_name) = 0;
+    const std::string & end_effector_name,
+    const std::string & robot_description = "") = 0;
 
   /**
    * \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian.

diff --git a/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp b/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp
@@ -41,7 +41,8 @@ class KinematicsInterfaceKDL : public kinematics_interface::KinematicsInterface
 public:
   bool initialize(
     std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface,
-    const std::string & end_effector_name) override;
+    const std::string & end_effector_name,
+    const std::string & robot_description = "") override;
 
   bool convert_cartesian_deltas_to_joint_deltas(
     const Eigen::VectorXd & joint_pos, const Eigen::Matrix<double, 6, 1> & delta_x,

diff --git a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp
@@ -22,19 +22,29 @@ rclcpp::Logger LOGGER = rclcpp::get_logger("kinematics_interface_kdl");
 
 bool KinematicsInterfaceKDL::initialize(
   std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface,
-  const std::string & end_effector_name)
+  const std::string & end_effector_name,
+  const std::string & robot_description)
 {
   // track initialization plugin
   initialized = true;
 
-  // get robot description
-  auto robot_param = rclcpp::Parameter();
-  if (!parameters_interface->get_parameter("robot_description", robot_param))
+  std::string robot_description_local;
+  if (robot_description.empty())
   {
-    RCLCPP_ERROR(LOGGER, "parameter robot_description not set");
-    return false;
+    // If the robot_description input argument is empty, try to get the
+    // robot_description from the node's parameters.
+    auto robot_param = rclcpp::Parameter();
+    if (!parameters_interface->get_parameter("robot_description", robot_param))
+    {
+      RCLCPP_ERROR(LOGGER, "parameter robot_description not set in kinematics_interface_kdl");
+      return false;
+    }
+    robot_description_local = robot_param.as_string();
+  }
+  else
+  {
+    robot_description_local = robot_description;
   }
-  auto robot_description = robot_param.as_string();
   // get alpha damping term
   auto alpha_param = rclcpp::Parameter("alpha", 0.000005);
   if (parameters_interface->has_parameter("alpha"))
@@ -44,7 +54,7 @@ bool KinematicsInterfaceKDL::initialize(
   alpha = alpha_param.as_double();
   // create kinematic chain
   KDL::Tree robot_tree;
-  kdl_parser::treeFromString(robot_description, robot_tree);
+  kdl_parser::treeFromString(robot_description_local, robot_tree);
   root_name_ = robot_tree.getRootSegment()->first;
   if (!robot_tree.getChain(root_name_, end_effector_name, chain_))
   {