Fixed implementation so that effort_controllers/GripperActionControll…

…er works. (#756) (#868) (cherry picked from commit 6352a70) Co-authored-by: chama1176 <kaede6120@gmail.com>
ros-controls · Nov 27, 2023 · c8f1b44 · c8f1b44
1 parent b4b465d
commit c8f1b44
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 13 deletions.
diff --git 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<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
-    joint_position_command_interface_;
+    joint_command_interface_;
   std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
     joint_position_state_interface_;
   std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>

diff --git 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 <const char * HardwareInterface>
 controller_interface::CallbackReturn GripperActionController<HardwareInterface>::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<HardwareInterface>:
     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 <const char * HardwareInterface>
 controller_interface::CallbackReturn GripperActionController<HardwareInterface>::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<HardwareInterface>::command_interface_configuration() co
 {
   return {
     controller_interface::interface_configuration_type::INDIVIDUAL,
-    {params_.joint + "/" + hardware_interface::HW_IF_POSITION}};
+    {params_.joint + "/" + HardwareInterface}};
 }
 
 template <const char * HardwareInterface>

diff --git 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<hardware_interface::HW_IF_EFFORT>
   {
     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<double>(node, prefix + ".p", 0.0);
     const auto k_i = auto_declare<double>(node, prefix + ".i", 0.0);
     const auto k_d = auto_declare<double>(node, prefix + ".d", 0.0);