Implement MaxEffortGripperActionController for Dynamixel grippers

hector_ros_controllers/CMakeLists.txt

@@ -18,6 +18,7 @@ endif()
 set(THIS_PACKAGE_INCLUDE_DEPENDS
     ament_cmake
     backward_ros
+    control_msgs
     controller_interface
     srdfdom
     visualization_msgs
@@ -58,6 +59,10 @@ generate_parameter_library(safety_forward_controller_parameters
 
 generate_parameter_library(safety_position_controller_parameters
                            params/safety_position_controller_parameters.yaml)
+
+generate_parameter_library(
+  gripper_position_effort_controller_parameters
+  params/gripper_position_effort_controller_parameters.yaml)
 # Add controllers here Define Controllers as Shared Libraries
 
 add_library(
@@ -67,6 +72,7 @@ add_library(
   src/sync_group_velocity_to_position_controller_actions.cpp
   src/self_collision_avoidance_controller.cpp
   src/safety_position_controller.cpp
+  src/gripper_position_effort_controller.cpp
   src/collision_checker.cpp)
 
 target_compile_features(controllers PUBLIC cxx_std_17)
@@ -83,7 +89,8 @@ target_link_libraries(
          sync_group_velocity_to_position_controller_parameters
          self_collision_avoidance_controller_parameters
          safety_forward_controller_parameters
-         safety_position_controller_parameters)
+         safety_position_controller_parameters
+         gripper_position_effort_controller_parameters)
 
 target_link_libraries(controllers PUBLIC ad_kinematics::ad_kinematics)
 ament_target_dependencies(controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
@@ -100,6 +107,7 @@ install(
           self_collision_avoidance_controller_parameters
           safety_forward_controller_parameters
           safety_position_controller_parameters
+          gripper_position_effort_controller_parameters
   EXPORT export_hector_ros_controllers
   RUNTIME DESTINATION bin
   ARCHIVE DESTINATION lib
@@ -126,6 +134,7 @@ if(BUILD_TESTING)
     ${sensor_msgs_TARGETS}
     ${hector_ros_controllers_msgs_TARGETS}
     ${visualization_msgs_TARGETS}
+    ${control_msgs_TARGETS}
     rclcpp::rclcpp
     rclcpp_action::rclcpp_action
     rclcpp_lifecycle::rclcpp_lifecycle
@@ -141,7 +150,8 @@ if(BUILD_TESTING)
     sync_group_velocity_to_position_controller_parameters
     self_collision_avoidance_controller_parameters
     safety_forward_controller_parameters
-    safety_position_controller_parameters)
+    safety_position_controller_parameters
+    gripper_position_effort_controller_parameters)
 
   ament_add_gmock(test_collision_checker test/test_collision_checker.cpp)
   target_include_directories(
@@ -185,8 +195,6 @@ if(BUILD_TESTING)
     target_compile_definitions(benchmark_collision_checker PRIVATE ${DEFS})
   endif()
 
-  ament_add_gmock(test_velocity_to_position_command_controller
-                  test/test_velocity_to_position_command_controller.cpp)
   ament_add_gmock(test_sync_group_velocity_to_position_controller
                   test/test_sync_group_velocity_to_position_controller.cpp)
   target_include_directories(
@@ -198,6 +206,16 @@ if(BUILD_TESTING)
   target_compile_definitions(test_sync_group_velocity_to_position_controller
                              PRIVATE ${DEFS})
 
+  ament_add_gmock(test_gripper_position_effort_controller
+                  test/test_gripper_position_effort_controller.cpp)
+  target_include_directories(
+    test_gripper_position_effort_controller
+    PRIVATE ${PROJECT_SOURCE_DIR}/include ${PROJECT_SOURCE_DIR}/test)
+  target_link_libraries(
+    test_gripper_position_effort_controller
+    ${PROJECT_NAME}::controllers_test_doubles ament_index_cpp::ament_index_cpp)
+  target_compile_definitions(test_gripper_position_effort_controller
+                             PRIVATE ${DEFS})
   ament_add_gmock(
     test_sync_group_velocity_to_position_controller_actions
     test/test_sync_group_velocity_to_position_controller_actions.cpp)

hector_ros_controllers/hector_ros_controller_plugin.xml

@@ -23,4 +23,13 @@
             A chainable position controller ensuring that revolute joints are unwrapped and joint limits are satisfied.
         </description>
   </class>
+  <class name="gripper_position_effort_controller/GripperPositionEffortController" type="gripper_position_effort_controller::GripperPositionEffortController" base_class_type="controller_interface::ControllerInterface">
+    <description>
+            Gripper controller that simultaneously commands position and a max-effort (current/torque limit)
+            on a single joint. Implements control_msgs/action/GripperCommand and additionally accepts position and
+            velocity goals via topics. Publishes is_grasped (std_msgs/Bool) at action_monitor_rate. Designed for
+            Dynamixel actuators in current-based position control mode; the effort command interface can be
+            disabled via the effort_command_interface parameter for position-only setups (e.g. simulation).
+        </description>
+  </class>
 </library>

hector_ros_controllers/include/gripper_position_effort_controller/gripper_position_effort_controller.hpp

@@ -0,0 +1,166 @@
+#ifndef GRIPPER_POSITION_EFFORT_CONTROLLER__GRIPPER_POSITION_EFFORT_CONTROLLER_HPP_
+#define GRIPPER_POSITION_EFFORT_CONTROLLER__GRIPPER_POSITION_EFFORT_CONTROLLER_HPP_
+
+#include <atomic>
+#include <memory>
+#include <optional>
+#include <string>
+
+#include "control_msgs/action/gripper_command.hpp"
+#include "controller_interface/controller_interface.hpp"
+#include "hardware_interface/loaned_command_interface.hpp"
+#include "hardware_interface/loaned_state_interface.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/create_server.hpp"
+#include "realtime_tools/realtime_buffer.hpp"
+#include "realtime_tools/realtime_publisher.hpp"
+#include "realtime_tools/realtime_server_goal_handle.hpp"
+#include "std_msgs/msg/bool.hpp"
+#include "std_msgs/msg/float64.hpp"
+
+#include <hector_ros_controllers/gripper_position_effort_controller_parameters.hpp>
+
+namespace gripper_position_effort_controller
+{
+
+/**
+ * Gripper action controller that simultaneously commands a position and a max-effort
+ * (current/torque limit) on a single joint. Designed for Dynamixel actuators in
+ * current-based position control mode where writing the effort interface sets the
+ * goal-current register.
+ *
+ * Three goal sources, all writing to a single internal target. Last writer wins;
+ * any topic command preempts an active action goal.
+ *   - control_msgs/action/GripperCommand on ~/gripper_cmd
+ *   - std_msgs/msg/Float64 on ~/position_command  (uses default_max_effort)
+ *   - std_msgs/msg/Float64 on ~/velocity_command  (integrated, uses default_max_effort)
+ *
+ * Publishes std_msgs/msg/Bool on ~/is_grasped at action_monitor_rate.
+ */
+class GripperPositionEffortController : public controller_interface::ControllerInterface
+{
+public:
+  using GripperCommandAction = control_msgs::action::GripperCommand;
+  using GoalHandle = rclcpp_action::ServerGoalHandle<GripperCommandAction>;
+  using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle<GripperCommandAction>;
+  using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
+
+  struct Command {
+    double position;
+    double max_effort;
+  };
+
+  GripperPositionEffortController();
+  ~GripperPositionEffortController() override = default;
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::CallbackReturn on_init() override;
+  controller_interface::CallbackReturn
+  on_configure( const rclcpp_lifecycle::State &previous_state ) override;
+  controller_interface::CallbackReturn
+  on_activate( const rclcpp_lifecycle::State &previous_state ) override;
+  controller_interface::CallbackReturn
+  on_deactivate( const rclcpp_lifecycle::State &previous_state ) override;
+
+  controller_interface::return_type update( const rclcpp::Time &time,
+                                            const rclcpp::Duration &period ) override;
+
+private:
+  // Which input source won arbitration for the current update() cycle.
+  enum class InputSource { None, Action, PositionTopic, VelocityTopic };
+
+  // Lifecycle helpers
+  void parse_joint_limits_from_urdf();
+
+  // Action callbacks
+  rclcpp_action::GoalResponse goal_callback( const rclcpp_action::GoalUUID &uuid,
+                                             std::shared_ptr<const GripperCommandAction::Goal> goal );
+  rclcpp_action::CancelResponse cancel_callback( std::shared_ptr<GoalHandle> goal_handle );
+  void accepted_callback( std::shared_ptr<GoalHandle> goal_handle );
+
+  // Goal management
+  void preempt_active_goal( const std::string &reason );
+  void clear_active_goal();
+  void clear_pending_action_command();
+  void flush_previous_goal_if_any();
+  void set_hold_position();
+  void check_for_success( const rclcpp::Time &time, double error_position, double current_position,
+                          double current_velocity, double current_effort );
+
+  // Lock-free accessors for goal-handle slots (touched from both RT and non-RT).
+  // std::atomic_store/load/exchange on shared_ptr (correct in C++17, deprecated in C++20).
+  static void store_goal_slot( RealtimeGoalHandlePtr &slot, RealtimeGoalHandlePtr handle );
+  static RealtimeGoalHandlePtr load_goal_slot( const RealtimeGoalHandlePtr &slot );
+  static RealtimeGoalHandlePtr exchange_goal_slot( RealtimeGoalHandlePtr &slot,
+                                                   RealtimeGoalHandlePtr handle );
+
+  // Continues integrating the cached velocity message if it's still within the watchdog
+  // window. Used by the NONE arbitration branch.
+  bool continue_velocity_integration_if_within_watchdog( const rclcpp::Time &time,
+                                                         const rclcpp::Duration &period );
+
+  // Parameters
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+
+  // Joint limits (NaN if no limit, e.g. continuous joint)
+  double joint_lower_limit_;
+  double joint_upper_limit_;
+
+  // Hardware interface handles
+  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> position_command_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> effort_command_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> position_state_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> velocity_state_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> effort_state_interface_;
+
+  // Active target written to hardware each cycle
+  Command target_;
+
+  // Action server
+  rclcpp_action::Server<GripperCommandAction>::SharedPtr action_server_;
+  // Currently-active goal. Access ONLY via {store,load,exchange}_goal_slot.
+  RealtimeGoalHandlePtr rt_active_goal_;
+  // Most-recently-cleared goal, kept alive so the wall_timer can flush its deferred
+  // terminal flag (succeed/abort/canceled set in RT) before the next accept replaces
+  // the timer. Access ONLY via {store,load,exchange}_goal_slot.
+  RealtimeGoalHandlePtr previous_rt_goal_;
+  GripperCommandAction::Result::SharedPtr pre_alloc_result_;
+  rclcpp::TimerBase::SharedPtr goal_handle_timer_;
+  rclcpp::Duration action_monitor_period_;
+  rclcpp::Time last_movement_time_;
+
+  // Per-source pending input. Every writer bumps input_seq_counter_ and stores the
+  // returned sequence in its source-specific atomic. update() picks the highest
+  // unconsumed seq, giving last-writer-wins arbitration regardless of dispatch order.
+  std::atomic<uint64_t> input_seq_counter_;
+
+  realtime_tools::RealtimeBuffer<Command> rt_action_command_;
+  std::atomic<uint64_t> action_cmd_seq_;
+  uint64_t last_consumed_action_seq_;
+
+  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr position_cmd_sub_;
+  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr velocity_cmd_sub_;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64>> rt_position_cmd_;
+  realtime_tools::RealtimeBuffer<std::shared_ptr<std_msgs::msg::Float64>> rt_velocity_cmd_;
+  std::atomic<uint64_t> position_cmd_seq_;
+  std::atomic<uint64_t> velocity_cmd_seq_;
+  uint64_t last_consumed_position_seq_;
+  uint64_t last_consumed_velocity_seq_;
+  rclcpp::Time last_velocity_msg_time_;
+  // RT-only flag gating cached-velocity integration. Cleared by watchdog OR when a
+  // non-velocity source wins.
+  bool velocity_cached_valid_;
+
+  // Effort hold-mode reference for is_grasped publisher (heartbeat throttling)
+  rclcpp::Time last_is_grasped_publish_time_;
+  bool is_grasped_;
+  int is_grasped_dwell_counter_;
+  realtime_tools::RealtimePublisherSharedPtr<std_msgs::msg::Bool> rt_is_grasped_pub_;
+};
+
+} // namespace gripper_position_effort_controller
+
+#endif // GRIPPER_POSITION_EFFORT_CONTROLLER__GRIPPER_POSITION_EFFORT_CONTROLLER_HPP_

hector_ros_controllers/package.xml

@@ -13,6 +13,7 @@
 
   <depend>ad_kinematics</depend>
   <depend>backward_ros</depend>
+  <depend>control_msgs</depend>
   <depend>controller_interface</depend>
   <depend>generate_parameter_library</depend>
   <depend>hardware_interface</depend>

hector_ros_controllers/params/gripper_position_effort_controller_parameters.yaml

@@ -0,0 +1,15 @@
+gripper_position_effort_controller:
+  joint: {type: string, default_value: '', description: "Joint to control. Must expose 'position' command interface (and 'effort' command interface when effort_command_interface is 'required') and 'position', 'velocity', and 'effort' state interfaces."}
+  effort_command_interface: {type: string, default_value: required, description: "'required' (default) claims and writes the effort command interface — use on real Dynamixel hardware. 'disabled' runs in position-only mode without claiming an effort command interface — use in simulation when no effort command interface is exposed.", validation: {one_of<>: [[required, disabled]]}}
+  effort_command_interface_name: {type: string, default_value: effort, description: Name of the effort command interface exposed by the hardware. Defaults to 'effort'. Set to e.g. 'current' when the hardware exposes the effort/torque command under a different interface name (some Dynamixel drivers expose it as 'current').}
+  action_monitor_rate: {type: double, default_value: 20.0, description: Rate (Hz) at which the action goal status is monitored and the is_grasped topic is published., validation: {gt_eq: [0.1]}}
+  goal_tolerance: {type: double, default_value: 0.01, description: Position error below which an action goal is considered reached., validation: {gt_eq: [0.0]}}
+  default_max_effort: {type: double, default_value: 0.0, description: Effort written to the joint's effort command interface for topic-driven goals and for action goals where max_effort == 0. Used by Dynamixel actuators as a current/torque limit in current-based position mode., validation: {gt_eq: [0.0]}}
+  max_effort_limit: {type: double, default_value: 0.0, description: Hard upper bound applied to the effort value written to hardware (after action goal max_effort or default_max_effort selection). 0 disables the limit. URDF effort limits are not used because the URDF effort field rarely matches the actual current/torque scaling of Dynamixel actuators., validation: {gt_eq: [0.0]}}
+  allow_stalling: {type: bool, default_value: false, description: 'If true, an action goal that stalls before reaching the goal returns SUCCEEDED with stalled=true. If false, it is ABORTED.'}
+  stall_velocity_threshold: {type: double, default_value: 0.001, description: Velocity below which the joint is considered to be potentially stalled (action mode)., validation: {gt_eq: [0.0]}}
+  stall_timeout: {type: double, default_value: 1.0, description: Time (seconds) the joint must remain below stall_velocity_threshold without reaching the goal before stall is declared., validation: {gt_eq: [0.0]}}
+  velocity_command_timeout: {type: double, default_value: 0.2, description: Time (seconds) without a velocity command message after which the integrated velocity is treated as zero (target position held)., validation: {gt_eq: [0.0]}}
+  is_grasped_velocity_threshold: {type: double, default_value: 0.005, description: Velocity must remain below this value for is_grasped to become true., validation: {gt_eq: [0.0]}}
+  is_grasped_effort_threshold: {type: double, default_value: 0.5, description: Effort must remain above this value (absolute) for is_grasped to become true., validation: {gt_eq: [0.0]}}
+  is_grasped_dwell_cycles: {type: int, default_value: 5, description: Number of consecutive update cycles for which the velocity and effort conditions must hold before is_grasped latches true., validation: {gt_eq: [1]}}