From c8f1b449d45b28efbf1d82d91305d5684af0b8ce Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 27 Nov 2023 20:25:14 +0000 Subject: [PATCH] Fixed implementation so that effort_controllers/GripperActionController works. (#756) (#868) (cherry picked from commit 6352a70d210a6bdec6228a24f41b6d542d0e4364) Co-authored-by: chama1176 --- .../gripper_action_controller.hpp | 2 +- .../gripper_action_controller_impl.hpp | 23 ++++++++++--------- .../hardware_interface_adapter.hpp | 2 +- 3 files changed, 14 insertions(+), 13 deletions(-) diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 89bf232fad..cf3ec193f3 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -124,7 +124,7 @@ class GripperActionController : public controller_interface::ControllerInterface bool verbose_ = false; ///< Hard coded verbose flag to help in debugging std::string name_; ///< Controller name. std::optional> - joint_position_command_interface_; + joint_command_interface_; std::optional> joint_position_state_interface_; std::optional> diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 666fced1c0..77598591ae 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -226,20 +226,21 @@ template controller_interface::CallbackReturn GripperActionController::on_activate( const rclcpp_lifecycle::State &) { - auto position_command_interface_it = std::find_if( + auto command_interface_it = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), [](const hardware_interface::LoanedCommandInterface & command_interface) - { return command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION; }); - if (position_command_interface_it == command_interfaces_.end()) + { return command_interface.get_interface_name() == HardwareInterface; }); + if (command_interface_it == command_interfaces_.end()) { - RCLCPP_ERROR(get_node()->get_logger(), "Expected 1 position command interface"); + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), "Expected 1 " << HardwareInterface << " command interface"); return controller_interface::CallbackReturn::ERROR; } - if (position_command_interface_it->get_prefix_name() != params_.joint) + if (command_interface_it->get_prefix_name() != params_.joint) { RCLCPP_ERROR_STREAM( - get_node()->get_logger(), "Position command interface is different than joint name `" - << position_command_interface_it->get_prefix_name() << "` != `" + get_node()->get_logger(), "Command interface is different than joint name `" + << command_interface_it->get_prefix_name() << "` != `" << params_.joint << "`"); return controller_interface::CallbackReturn::ERROR; } @@ -278,12 +279,12 @@ controller_interface::CallbackReturn GripperActionController: return controller_interface::CallbackReturn::ERROR; } - joint_position_command_interface_ = *position_command_interface_it; + joint_command_interface_ = *command_interface_it; joint_position_state_interface_ = *position_state_interface_it; joint_velocity_state_interface_ = *velocity_state_interface_it; // Hardware interface adapter - hw_iface_adapter_.init(joint_position_command_interface_, get_node()); + hw_iface_adapter_.init(joint_command_interface_, get_node()); // Command - non RT version command_struct_.position_ = joint_position_state_interface_->get().get_value(); @@ -311,7 +312,7 @@ template controller_interface::CallbackReturn GripperActionController::on_deactivate( const rclcpp_lifecycle::State &) { - joint_position_command_interface_ = std::nullopt; + joint_command_interface_ = std::nullopt; joint_position_state_interface_ = std::nullopt; joint_velocity_state_interface_ = std::nullopt; release_interfaces(); @@ -324,7 +325,7 @@ GripperActionController::command_interface_configuration() co { return { controller_interface::interface_configuration_type::INDIVIDUAL, - {params_.joint + "/" + hardware_interface::HW_IF_POSITION}}; + {params_.joint + "/" + HardwareInterface}}; } template diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index 97ebbb1f4b..b125ab12d0 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -131,7 +131,7 @@ class HardwareInterfaceAdapter { joint_handle_ = joint_handle; // Init PID gains from ROS parameter server - const std::string prefix = "gains." + joint_handle_->get().get_name(); + const std::string prefix = "gains." + joint_handle_->get().get_prefix_name(); const auto k_p = auto_declare(node, prefix + ".p", 0.0); const auto k_i = auto_declare(node, prefix + ".i", 0.0); const auto k_d = auto_declare(node, prefix + ".d", 0.0);