From 45f5315d85d16c6b3e028ad99280a3d94525d056 Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Thu, 21 Nov 2024 08:55:43 +0100 Subject: [PATCH] Revert "Compiles again" This reverts commit 06ff4150676d3490cec05a5ddda426078bab170e. --- .../admittance_controller/admittance_rule.hpp | 3 ++- .../admittance_controller/admittance_rule_impl.hpp | 6 ++++-- admittance_controller/src/admittance_controller.cpp | 12 +++++------- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 7223dbe9d1..a326b663d0 100644 --- 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 & node, const size_t num_joint); + const std::shared_ptr & 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 689d13a93f..13d4e67fbc 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -35,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 & node, const size_t num_joints) + const std::shared_ptr & node, const size_t num_joints, + const std::string & robot_description) { num_joints_ = num_joints; @@ -58,7 +59,8 @@ controller_interface::return_type AdmittanceRule::configure( kinematics_ = std::unique_ptr( kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); - if (!kinematics_->initialize(node->get_node_parameters_interface(), "kinematics")) + if (!kinematics_->initialize( + 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 5649754dcd..d992707101 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -289,14 +289,10 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( "~/wrench_reference", rclcpp::SystemDefaultsQoS(), [&](const geometry_msgs::msg::WrenchStamped & msg) { - if ( - msg.header.frame_id != admittance_->parameters_.ft_sensor.frame.id && - !msg.header.frame_id.empty()) + if (msg.header.frame_id != admittance_->parameters_.ft_sensor.frame.id && !msg.header.frame_id.empty()) { RCLCPP_ERROR_STREAM( - get_node()->get_logger(), "Ignoring wrench reference as it is on the wrong frame: " - << msg.header.frame_id << ". Expected reference frame: " - << admittance_->parameters_.ft_sensor.frame.id); + get_node()->get_logger(), "Ignoring wrench reference as it is on the wrong frame: " << msg.header.frame_id << ". Expected reference frame: " << admittance_->parameters_.ft_sensor.frame_id); return; } input_wrench_command_.writeFromNonRT(msg); @@ -316,7 +312,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; }