Adding use of robot description parameter in the Admittance Controller (backport #1247) (#1354)

---------

Co-authored-by: Kevin DeMarco <kevin@kevindemarco.com>
Co-authored-by: Nikola Banovic <nibanovic@gmail.com>
Co-authored-by: Dr. Denis <denis@stoglrobotics.de>
Co-authored-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>

Author: 5 people

admittance_controller/include/admittance_controller/admittance_rule.hpp

@@ -109,7 +109,8 @@ class AdmittanceRule
 
   /// Configure admittance rule memory using number of joints.
   controller_interface::return_type configure(
-    const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const size_t num_joint);
+    const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const size_t num_joint,
+    const std::string & robot_description);
 
   /// Reset all values back to default
   controller_interface::return_type reset(const size_t num_joints);

admittance_controller/include/admittance_controller/admittance_rule_impl.hpp

@@ -20,6 +20,7 @@
 #include "admittance_controller/admittance_rule.hpp"
 
 #include <memory>
+#include <string>
 #include <vector>
 
 #include "rclcpp/duration.hpp"
@@ -33,7 +34,8 @@ constexpr auto NUM_CARTESIAN_DOF = 6;  // (3 translation + 3 rotation)
 
 /// Configure admittance rule memory for num joints and load kinematics interface
 controller_interface::return_type AdmittanceRule::configure(
-  const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const size_t num_joints)
+  const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const size_t num_joints,
+  const std::string & robot_description)
 {
   num_joints_ = num_joints;
 
@@ -57,7 +59,7 @@ controller_interface::return_type AdmittanceRule::configure(
         kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name));
 
       if (!kinematics_->initialize(
-            node->get_node_parameters_interface(), parameters_.kinematics.tip))
+            robot_description, node->get_node_parameters_interface(), "kinematics"))
       {
         return controller_interface::return_type::ERROR;
       }

admittance_controller/src/admittance_controller.cpp

@@ -284,7 +284,8 @@ controller_interface::CallbackReturn AdmittanceController::on_configure(
     semantic_components::ForceTorqueSensor(admittance_->parameters_.ft_sensor.name));
 
   // configure admittance rule
-  if (admittance_->configure(get_node(), num_joints_) == controller_interface::return_type::ERROR)
+  if (
+    admittance_->configure(get_node(), num_joints_, "") == controller_interface::return_type::ERROR)
   {
     return controller_interface::CallbackReturn::ERROR;
   }

admittance_controller/test/test_admittance_controller.hpp

@@ -103,18 +103,14 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon
     }
   }
 
-private:
   const std::string robot_description_ = ros2_control_test_assets::valid_6d_robot_urdf;
   const std::string robot_description_semantic_ = ros2_control_test_assets::valid_6d_robot_srdf;
 };
 
 class AdmittanceControllerTest : public ::testing::Test
 {
 public:
-  static void SetUpTestCase()
-  {
-    //    rclcpp::init(0, nullptr);
-  }
+  static void SetUpTestCase() {}
 
   void SetUp()
   {
@@ -164,8 +160,8 @@ class AdmittanceControllerTest : public ::testing::Test
   controller_interface::return_type SetUpControllerCommon(
     const std::string & controller_name, const rclcpp::NodeOptions & options)
   {
+    // robot_description is received from node's parameter
     auto result = controller_->init(controller_name, "", options);
-
     controller_->export_reference_interfaces();
     assign_interfaces();