From 927f12314d4122765fd2c419b6ce0d54abc7ae81 Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Mon, 11 Nov 2024 15:50:06 +0100
Subject: [PATCH] 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>
---
 .../include/admittance_controller/admittance_rule.hpp     | 3 ++-
 .../admittance_controller/admittance_rule_impl.hpp        | 6 ++++--
 admittance_controller/src/admittance_controller.cpp       | 3 ++-
 admittance_controller/test/test_admittance_controller.hpp | 8 ++------
 4 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp
index 36c027491c..2a6d755104 100644
--- a/admittance_controller/include/admittance_controller/admittance_rule.hpp
+++ b/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);
diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp
index fa1dcfcc63..c330e34b46 100644
--- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp
+++ b/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;
       }
diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp
index 22d25bb8b1..7d45fe0adf 100644
--- a/admittance_controller/src/admittance_controller.cpp
+++ b/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;
   }
diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp
index ca12a38645..0f4e122328 100644
--- a/admittance_controller/test/test_admittance_controller.hpp
+++ b/admittance_controller/test/test_admittance_controller.hpp
@@ -103,7 +103,6 @@ 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;
 };
@@ -111,10 +110,7 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon
 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();