Adding use of robot description parameter in the Admittance Controller (

ros-controls#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]>
quasi-robotics · Dec 12, 2024 · 7707387 · 7707387
1 parent 6284228
commit 7707387
Show file tree

Hide file tree

Showing 4 changed files with 12 additions and 12 deletions.
diff --git 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
@@ -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;
 
@@ -51,7 +53,7 @@ controller_interface::return_type AdmittanceRule::configure(
       kinematics_ = std::unique_ptr<kinematics_interface::KinematicsInterface>(
         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
@@ -284,7 +284,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
@@ -122,20 +122,14 @@ class TestableAdmittanceController : public admittance_controller::AdmittanceCon
     return success;
   }
 
-private:
-  rclcpp::WaitSet input_wrench_command_subscriber_wait_set_;
-  rclcpp::WaitSet input_pose_command_subscriber_wait_set_;
   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()
   {
@@ -185,7 +179,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();