diff --git a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp index b3d4cee..9e4f10c 100644 --- a/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp +++ b/kinematics_interface/include/kinematics_interface/kinematics_interface.hpp @@ -39,10 +39,15 @@ class KinematicsInterface /** * \brief Initialize plugin. This method must be called before any other. + * \param[in] robot_description robot URDF in string format + * \param[in] parameters_interface + * \param[in] param_namespace namespace for kinematics parameters - defaults to "kinematics" + * \return true if successful */ virtual bool initialize( + const std::string & robot_description, std::shared_ptr parameters_interface, - const std::string & end_effector_name) = 0; + const std::string & param_namespace) = 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..645a5fa 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 @@ -40,8 +40,9 @@ class KinematicsInterfaceKDL : public kinematics_interface::KinematicsInterface { public: bool initialize( + const std::string & robot_description, std::shared_ptr parameters_interface, - const std::string & end_effector_name) override; + const std::string & param_namespace) 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 729ed0f..62dca65 100644 --- a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp @@ -21,36 +21,62 @@ namespace kinematics_interface_kdl rclcpp::Logger LOGGER = rclcpp::get_logger("kinematics_interface_kdl"); bool KinematicsInterfaceKDL::initialize( + const std::string & robot_description, std::shared_ptr parameters_interface, - const std::string & end_effector_name) + const std::string & param_namespace) { // track initialization plugin initialized = true; - // get robot description - auto robot_param = rclcpp::Parameter(); - if (!parameters_interface->get_parameter("robot_description", robot_param)) + // get parameters + std::string ns = !param_namespace.empty() ? param_namespace + "." : ""; + + 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(ns + "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(); } - auto robot_description = robot_param.as_string(); + else + { + robot_description_local = robot_description; + } + // get alpha damping term auto alpha_param = rclcpp::Parameter("alpha", 0.000005); - if (parameters_interface->has_parameter("alpha")) + if (parameters_interface->has_parameter(ns + "alpha")) { - parameters_interface->get_parameter("alpha", alpha_param); + parameters_interface->get_parameter(ns + "alpha", alpha_param); } alpha = alpha_param.as_double(); + // get end-effector name + auto end_effector_name_param = rclcpp::Parameter("tip"); + if (parameters_interface->has_parameter(ns + "tip")) + { + parameters_interface->get_parameter(ns + "tip", end_effector_name_param); + } + else + { + RCLCPP_ERROR(LOGGER, "Failed to find end effector name parameter [tip]."); + return false; + } + std::string end_effector_name = end_effector_name_param.as_string(); + // create kinematic chain KDL::Tree robot_tree; - kdl_parser::treeFromString(robot_description, robot_tree); - + kdl_parser::treeFromString(robot_description_local, robot_tree); // get root name auto base_param = rclcpp::Parameter(); - if (parameters_interface->has_parameter("base")) + if (parameters_interface->has_parameter(ns + "base")) { - parameters_interface->get_parameter("base", base_param); + parameters_interface->get_parameter(ns + "base", base_param); root_name_ = base_param.as_string(); } else diff --git a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp index b78ae43..9176c3a 100644 --- a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp @@ -28,6 +28,8 @@ class TestKDLPlugin : public ::testing::Test std::shared_ptr ik_; std::shared_ptr node_; std::string end_effector_ = "link2"; + std::string urdf_ = std::string(ros2_control_test_assets::urdf_head) + + std::string(ros2_control_test_assets::urdf_tail); void SetUp() { @@ -50,9 +52,7 @@ class TestKDLPlugin : public ::testing::Test void loadURDFParameter() { - auto urdf = std::string(ros2_control_test_assets::urdf_head) + - std::string(ros2_control_test_assets::urdf_tail); - rclcpp::Parameter param("robot_description", urdf); + rclcpp::Parameter param("robot_description", urdf_); node_->declare_parameter("robot_description", ""); node_->set_parameter(param); } @@ -63,6 +63,17 @@ class TestKDLPlugin : public ::testing::Test node_->declare_parameter("alpha", 0.005); node_->set_parameter(param); } + + /** + * \brief Used for testing initialization from parameters. + * Elsewhere, `end_effector_` member is used. + */ + void loadTipParameter() + { + rclcpp::Parameter param("tip", "link2"); + node_->declare_parameter("tip", "link2"); + node_->set_parameter(param); + } }; TEST_F(TestKDLPlugin, KDL_plugin_function) @@ -70,9 +81,10 @@ TEST_F(TestKDLPlugin, KDL_plugin_function) // load robot description and alpha to parameter server loadURDFParameter(); loadAlphaParameter(); + loadTipParameter(); // initialize the plugin - ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); + ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); // calculate end effector transform Eigen::Matrix pos = Eigen::Matrix::Zero(); @@ -103,9 +115,10 @@ TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector) // load robot description and alpha to parameter server loadURDFParameter(); loadAlphaParameter(); + loadTipParameter(); // initialize the plugin - ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); + ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); // calculate end effector transform std::vector pos = {0, 0}; @@ -136,9 +149,10 @@ TEST_F(TestKDLPlugin, incorrect_input_sizes) // load robot description and alpha to parameter server loadURDFParameter(); loadAlphaParameter(); + loadTipParameter(); // initialize the plugin - ASSERT_TRUE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); + ASSERT_TRUE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); // define correct values Eigen::Matrix pos = Eigen::Matrix::Zero(); @@ -175,5 +189,11 @@ TEST_F(TestKDLPlugin, KDL_plugin_no_robot_description) { // load alpha to parameter server loadAlphaParameter(); - ASSERT_FALSE(ik_->initialize(node_->get_node_parameters_interface(), end_effector_)); + loadTipParameter(); + ASSERT_FALSE(ik_->initialize("", node_->get_node_parameters_interface(), "")); +} + +TEST_F(TestKDLPlugin, KDL_plugin_no_parameter_tip) +{ + ASSERT_FALSE(ik_->initialize(urdf_, node_->get_node_parameters_interface(), "")); }