Parallel gripper controller

moveit_plugins/moveit_ros_control_interface/CMakeLists.txt

@@ -38,7 +38,7 @@ ament_target_dependencies(moveit_ros_control_interface_trajectory_plugin
                           ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
 
 add_library(moveit_ros_control_interface_gripper_plugin SHARED
-            src/gripper_controller_plugin.cpp)
+            src/gripper_command_controller_plugin.cpp)
 set_target_properties(
   moveit_ros_control_interface_gripper_plugin
   PROPERTIES VERSION "${moveit_ros_control_interface_VERSION}")
@@ -47,6 +47,16 @@ target_include_directories(moveit_ros_control_interface_gripper_plugin
 ament_target_dependencies(moveit_ros_control_interface_gripper_plugin
                           ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
 
+add_library(moveit_ros_control_interface_parallel_gripper_plugin SHARED
+            src/parallel_gripper_command_controller_plugin.cpp)
+set_target_properties(
+  moveit_ros_control_interface_parallel_gripper_plugin
+  PROPERTIES VERSION "${moveit_ros_control_interface_VERSION}")
+target_include_directories(moveit_ros_control_interface_parallel_gripper_plugin
+                           PRIVATE include)
+ament_target_dependencies(moveit_ros_control_interface_parallel_gripper_plugin
+                          ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
+
 add_library(moveit_ros_control_interface_empty_plugin SHARED
             src/empty_controller_plugin.cpp)
 set_target_properties(
@@ -65,6 +75,7 @@ set(TARGET_LIBRARIES
     moveit_ros_control_interface_plugin
     moveit_ros_control_interface_trajectory_plugin
     moveit_ros_control_interface_gripper_plugin
+    moveit_ros_control_interface_parallel_gripper_plugin
     moveit_ros_control_interface_empty_plugin)
 
 # Mark executables and/or libraries for installation

moveit_plugins/moveit_ros_control_interface/moveit_ros_control_interface_plugins.xml

@@ -10,10 +10,15 @@
     </class>
   </library>
   <library path="moveit_ros_control_interface_gripper_plugin">
-    <class name="position_controllers/GripperActionController" type="moveit_ros_control_interface::GripperControllerAllocator" base_class_type="moveit_ros_control_interface::ControllerHandleAllocator">
+    <class name="position_controllers/GripperActionController" type="moveit_ros_control_interface::GripperCommandControllerAllocator" base_class_type="moveit_ros_control_interface::ControllerHandleAllocator">
         <description></description>
     </class>
-    <class name="effort_controllers/GripperActionController" type="moveit_ros_control_interface::GripperControllerAllocator" base_class_type="moveit_ros_control_interface::ControllerHandleAllocator">
+    <class name="effort_controllers/GripperActionController" type="moveit_ros_control_interface::GripperCommandControllerAllocator" base_class_type="moveit_ros_control_interface::ControllerHandleAllocator">
+        <description></description>
+    </class>
+  </library>
+  <library path="moveit_ros_control_interface_parallel_gripper_plugin">
+    <class name="parallel_gripper_action_controller/GripperActionController" type="moveit_ros_control_interface::ParallelGripperCommandControllerAllocator" base_class_type="moveit_ros_control_interface::ControllerHandleAllocator">
         <description></description>
     </class>
   </library>

moveit_plugins/moveit_ros_control_interface/src/gripper_controller_plugin.cpp → moveit_plugins/moveit_ros_control_interface/src/gripper_command_controller_plugin.cpp

@@ -36,27 +36,27 @@
 
 #include <moveit_ros_control_interface/ControllerHandle.hpp>
 #include <pluginlib/class_list_macros.hpp>
-#include <moveit_simple_controller_manager/gripper_controller_handle.hpp>
+#include <moveit_simple_controller_manager/gripper_command_controller_handle.hpp>
 #include <rclcpp/node.hpp>
 #include <memory>
 
 namespace moveit_ros_control_interface
 {
 /**
- * \brief Simple allocator for moveit_simple_controller_manager::FollowJointTrajectoryControllerHandle instances.
+ * \brief Simple allocator for moveit_simple_controller_manager::GripperCommandControllerHandle instances.
  */
-class GripperControllerAllocator : public ControllerHandleAllocator
+class GripperCommandControllerAllocator : public ControllerHandleAllocator
 {
 public:
   moveit_controller_manager::MoveItControllerHandlePtr alloc(const rclcpp::Node::SharedPtr& node,
                                                              const std::string& name,
                                                              const std::vector<std::string>& /* resources */) override
   {
-    return std::make_shared<moveit_simple_controller_manager::GripperControllerHandle>(node, name, "gripper_cmd");
+    return std::make_shared<moveit_simple_controller_manager::GripperCommandControllerHandle>(node, name, "gripper_cmd");
   }
 };
 
 }  // namespace moveit_ros_control_interface
 
-PLUGINLIB_EXPORT_CLASS(moveit_ros_control_interface::GripperControllerAllocator,
+PLUGINLIB_EXPORT_CLASS(moveit_ros_control_interface::GripperCommandControllerAllocator,
                        moveit_ros_control_interface::ControllerHandleAllocator);

moveit_plugins/moveit_ros_control_interface/src/parallel_gripper_command_controller_plugin.cpp

@@ -0,0 +1,63 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2025, Marq Rasmussen
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Marq Rasmussen nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Marq Rasmussen */
+
+#include <moveit_ros_control_interface/ControllerHandle.hpp>
+#include <pluginlib/class_list_macros.hpp>
+#include <moveit_simple_controller_manager/parallel_gripper_command_controller_handle.hpp>
+#include <rclcpp/node.hpp>
+#include <memory>
+
+namespace moveit_ros_control_interface
+{
+/**
+ * \brief Simple allocator for moveit_simple_controller_manager::ParallelGripperCommandControllerHandle instances.
+ */
+class ParallelGripperCommandControllerAllocator : public ControllerHandleAllocator
+{
+public:
+  moveit_controller_manager::MoveItControllerHandlePtr alloc(const rclcpp::Node::SharedPtr& node,
+                                                             const std::string& name,
+                                                             const std::vector<std::string>& /* resources */) override
+  {
+    return std::make_shared<moveit_simple_controller_manager::ParallelGripperCommandControllerHandle>(node, name,
+                                                                                                      "gripper_cmd");
+  }
+};
+
+}  // namespace moveit_ros_control_interface
+
+PLUGINLIB_EXPORT_CLASS(moveit_ros_control_interface::ParallelGripperCommandControllerAllocator,
+                       moveit_ros_control_interface::ControllerHandleAllocator);

moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.hpp → moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_command_controller_handle.hpp

@@ -47,12 +47,12 @@ namespace moveit_simple_controller_manager
  * This is an interface for a gripper using control_msgs/GripperCommandAction
  * action interface (single DOF).
  */
-class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs::action::GripperCommand>
+class GripperCommandControllerHandle : public ActionBasedControllerHandle<control_msgs::action::GripperCommand>
 {
 public:
   /* Topics will map to name/ns/goal, name/ns/result, etc */
-  GripperControllerHandle(const rclcpp::Node::SharedPtr& node, const std::string& name, const std::string& ns,
-                          const double max_effort = 0.0)
+  GripperCommandControllerHandle(const rclcpp::Node::SharedPtr& node, const std::string& name, const std::string& ns,
+                                 const double max_effort = 0.0)
     : ActionBasedControllerHandle<control_msgs::action::GripperCommand>(
           node, name, ns, "moveit.simple_controller_manager.gripper_controller_handle")
     , allow_failure_(false)
@@ -98,23 +98,23 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
       return false;
     }
 
-    std::vector<std::size_t> gripper_joint_indexes;
+    std::vector<std::size_t> gripper_joint_indices;
     for (std::size_t i = 0; i < trajectory.joint_trajectory.joint_names.size(); ++i)
     {
       if (command_joints_.find(trajectory.joint_trajectory.joint_names[i]) != command_joints_.end())
       {
-        gripper_joint_indexes.push_back(i);
+        gripper_joint_indices.push_back(i);
         if (!parallel_jaw_gripper_)
           break;
       }
     }
 
-    if (gripper_joint_indexes.empty())
+    if (gripper_joint_indices.empty())
     {
       RCLCPP_WARN(logger_, "No command_joint was specified for the MoveIt controller gripper handle. \
-                      Please see GripperControllerHandle::addCommandJoint() and \
-                      GripperControllerHandle::setCommandJoint(). Assuming index 0.");
-      gripper_joint_indexes.push_back(0);
+                      Please see GripperCommandControllerHandle::addCommandJoint() and \
+                      GripperCommandControllerHandle::setCommandJoint(). Assuming index 0.");
+      gripper_joint_indices.push_back(0);
     }
 
     // goal to be sent
@@ -126,7 +126,7 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
     RCLCPP_DEBUG(logger_, "Sending command from trajectory point %d", tpoint);
 
     // fill in goal from last point
-    for (std::size_t idx : gripper_joint_indexes)
+    for (std::size_t idx : gripper_joint_indices)
     {
       if (trajectory.joint_trajectory.points[tpoint].positions.size() <= idx)
       {

moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h

@@ -48,5 +48,5 @@
 /* Author: Michael Ferguson, Ioan Sucan, E. Gil Jones */
 
 #pragma once
-#pragma message(".h header is obsolete. Please use the .hpp header instead.")
-#include <moveit_simple_controller_manager/gripper_controller_handle.hpp>
+#pragma message(".h header is obsolete. Please use the gripper_command_controller_handle.hpp header instead.")
+#include <moveit_simple_controller_manager/gripper_command_controller_handle.hpp>