API changes to support robot description (#83)

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<rclcpp::node_interfaces::NodeParametersInterface> 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.

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<rclcpp::node_interfaces::NodeParametersInterface> 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<double, 6, 1> & delta_x,

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<rclcpp::node_interfaces::NodeParametersInterface> 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

kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp

@@ -28,6 +28,8 @@ class TestKDLPlugin : public ::testing::Test
   std::shared_ptr<kinematics_interface::KinematicsInterface> ik_;
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> 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,16 +63,28 @@ 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)
 {
   // 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<double, Eigen::Dynamic, 1> pos = Eigen::Matrix<double, 2, 1>::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<double> 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<double, Eigen::Dynamic, 1> pos = Eigen::Matrix<double, 2, 1>::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(), ""));
 }