Adding use of robot description parameter in the Admittance Controller (

#1247) --------- Co-authored-by: Kevin DeMarco <[email protected]> Co-authored-by: Nikola Banovic <[email protected]> Co-authored-by: Bence Magyar <[email protected]> Co-authored-by: Christoph Fröhlich <[email protected]>
ros-controls · Nov 6, 2024 · 9439764 · 9439764
1 parent f2da662
commit 9439764
Show file tree

Hide file tree

Showing 4 changed files with 12 additions and 10 deletions.
diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp
@@ -102,7 +102,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
@@ -20,6 +20,7 @@
 #include "admittance_controller/admittance_rule.hpp"
 
 #include <memory>
+#include <string>
 #include <vector>
 
 #include <control_toolbox/filters.hpp>
@@ -34,7 +35,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;
 
@@ -58,7 +60,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
@@ -280,7 +280,9 @@ 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_, this->get_robot_description()) ==
+    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
@@ -102,18 +102,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()
   {
@@ -163,7 +159,8 @@ class AdmittanceControllerTest : public ::testing::Test
   controller_interface::return_type SetUpControllerCommon(
     const std::string & controller_name, const rclcpp::NodeOptions & options)
   {
-    auto result = controller_->init(controller_name, "", 0, "", options);
+    auto result =
+      controller_->init(controller_name, controller_->robot_description_, 0, "", options);
 
     controller_->export_reference_interfaces();
     assign_interfaces();