From bbcea681000073f3ffbd2d5cebed79fd7d5ba6ef Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Tue, 9 Apr 2024 13:51:27 -0400 Subject: [PATCH] Pass the robot_description as an arg in initialize() with fallback --- .../kinematics_interface.hpp | 3 ++- .../kinematics_interface_kdl.hpp | 3 ++- .../src/kinematics_interface_kdl.cpp | 26 +++++++++++++------ 3 files changed, 22 insertions(+), 10 deletions(-) diff --git a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp index b3d4cee..87d2b29 100644 --- 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 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 index 9ed343d..c568757 100644 --- 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 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 & delta_x, diff --git a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp index af60266..9a71389 100644 --- 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 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_)) {