add optional interfaces for force and velocity

Signed-off-by: Paul Gesel <[email protected]>

gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp

@@ -131,9 +131,9 @@ class GripperActionController : public controller_interface::ControllerInterface
     joint_command_interface_;
   std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
   std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
-    force_multiplier_interface_;
+    effort_interface_;
   std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
-    speed_multiplier_interface_;
+    speed_interface_;
   std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
 
     joint_position_state_interface_;

gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp

@@ -95,8 +95,22 @@ void GripperActionController<HardwareInterface>::accepted_callback(
   // This is the non-realtime command_struct
   // We use command_ for sharing
   command_struct_.position_ = goal_handle->get_goal()->command.position;
-  command_struct_.target_velocity_ = goal_handle->get_goal()->command.target_velocity;
-  command_struct_.max_effort_ = goal_handle->get_goal()->command.max_effort;
+  if (params_.use_speed_interface)
+  {
+    command_struct_.target_velocity_ = goal_handle->get_goal()->command.target_velocity;
+  }
+  else
+  {
+    command_struct_.target_velocity_ = 0.0;
+  }
+  if (params_.use_effort_interface)
+  {
+    command_struct_.max_effort_ = goal_handle->get_goal()->command.max_effort;
+  }
+  else
+  {
+    command_struct_.max_effort_ = params_.max_effort;
+  }
   command_.writeFromNonRT(command_struct_);
 
   pre_alloc_result_->reached_goal = false;
@@ -287,21 +301,19 @@ controller_interface::CallbackReturn GripperActionController<HardwareInterface>:
 
   for (auto & interface : command_interfaces_)
   {
-    if (interface.get_name() == "gripper_speed_multiplier")
+    if (interface.get_interface_name() == "gripper_effort")
     {
-      speed_multiplier_interface_ = interface;
+      effort_interface_ = interface;
     }
-    else if (interface.get_name() == "gripper_force_multiplier")
+    else if (interface.get_interface_name() == "gripper_speed")
     {
-      force_multiplier_interface_ = interface;
+      speed_interface_ = interface;
     }
   }
-  force_multiplier_interface_.value().get().set_value(params_.force_multiplier);
-  speed_multiplier_interface_.value().get().set_value(params_.speed_multiplier);
 
   // Hardware interface adapter
-  hw_iface_adapter_.init(
-    joint_position_command_interface_, force_multiplier_interface_, speed_multiplier_interface_, get_node());
+
+  hw_iface_adapter_.init(joint_position_command_interface_, speed_interface_, effort_interface_, get_node());
 
   // Command - non RT version
   command_struct_.position_ = joint_position_state_interface_->get().get_value();
@@ -341,9 +353,18 @@ template <const char * HardwareInterface>
 controller_interface::InterfaceConfiguration
 GripperActionController<HardwareInterface>::command_interface_configuration() const
 {
-  return {
-    controller_interface::interface_configuration_type::INDIVIDUAL,
-    {params_.joint + "/" + HardwareInterface}};
+  std::vector<std::string> names = {params_.joint + "/" + HardwareInterface};
+  if (params_.use_effort_interface)
+  {
+    names.push_back({params_.joint + "/gripper_effort"});
+  }
+  if (params_.use_speed_interface)
+  {
+    names.push_back({params_.joint + "/gripper_speed"});
+  }
+
+  return {controller_interface::interface_configuration_type::INDIVIDUAL, names};
+
 }
 
 template <const char * HardwareInterface>

gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp

@@ -70,14 +70,13 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_POSITION>
 public:
   bool init(
     std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> joint_handle,
-    std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> force_handle,
-    std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
-      target_velocity_handle,
+    std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> speed_handle,
+    std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> effort_handle,
     const rclcpp_lifecycle::LifecycleNode::SharedPtr & /* node */)
   {
     joint_handle_ = joint_handle;
-    force_handle_ = force_handle;
-    target_velocity_handle_ = target_velocity_handle;
+    effort_handle_ = effort_handle;
+    speed_handle_ = speed_handle;
     return true;
   }
 
@@ -90,22 +89,21 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_POSITION>
   {
     // Forward desired position to command
     joint_handle_->get().set_value(desired_position);
-    if (target_velocity_handle_.has_value())
+    if (speed_handle_.has_value())
     {
-      target_velocity_handle_->get().set_value(desired_velocity);
+      speed_handle_->get().set_value(desired_velocity);
     }
-    if (force_handle_.has_value())
+    if (effort_handle_.has_value())
     {
-      force_handle_->get().set_value(max_allowed_effort);
+      effort_handle_->get().set_value(max_allowed_effort);
     }
     return max_allowed_effort;
   }
 
 private:
   std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> joint_handle_;
-  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> force_handle_;
-  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
-    target_velocity_handle_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> effort_handle_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> speed_handle_;
 };
 
 /**

gripper_controllers/src/gripper_action_controller_parameters.yaml

@@ -41,21 +41,14 @@ gripper_action_controller:
     description: "stall timeout",
     default_value: 1.0,
   }
-
-  force_multiplier: {
-    type: double,
-    description: "Scales the maximum effort of the gripper defined in the URDF",
-    default_value: 1.0,
-    validation: {
-      bounds<>: [ 0.0, 1.0 ]
-    },
+  use_effort_interface: {
+    type: bool,
+    description: "Controller will claim the {joint}/gripper_effort interface if true.",
+    default_value: false,
   }
-  speed_multiplier: {
-    type: double,
-    description: "Scales the default velocity of the gripper defined in the URDF",
-    default_value: 1.0,
-    validation: {
-      bounds<>: [ 0.0, 1.0 ]
-    },
+  use_speed_interface: {
+    type: bool,
+    description: "Controller will claim the {joint}/gripper_speed interface if true.",
+    default_value: false,
   }