From a4976a11958d72ddb9e2ed75fd1460b1d81b6f13 Mon Sep 17 00:00:00 2001
From: rickstaa <rick.staa@outlook.com>
Date: Tue, 7 Dec 2021 17:38:24 +0100
Subject: [PATCH] feat(simple_controller_manager): add `max_effort` parameter
 to GripperCommand action

This commit adds the `max_effort` parameter to the GripperCommand
declaration in the `controller_list` (see issue #2956). This value is
only used when effort is set in the requested gripper trajectory.

Co-authored-by: Jafar Abdi <cafer.abdi@gmail.com>
---
 .../gripper_controller_handle.h                  | 16 ++++++++++++++--
 .../src/moveit_simple_controller_manager.cpp     |  5 ++++-
 2 files changed, 18 insertions(+), 3 deletions(-)

diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h
index 212992de5bc..ecdce0fdeb5 100644
--- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h
+++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h
@@ -51,10 +51,11 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
 {
 public:
   /* Topics will map to name/ns/goal, name/ns/result, etc */
-  GripperControllerHandle(const std::string& name, const std::string& ns)
+  GripperControllerHandle(const std::string& name, const std::string& ns, const double max_effort = 0.0)
     : ActionBasedControllerHandle<control_msgs::GripperCommandAction>(name, ns)
     , allow_failure_(false)
     , parallel_jaw_gripper_(false)
+    , max_effort_(max_effort)
   {
   }
 
@@ -110,7 +111,6 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
     // goal to be sent
     control_msgs::GripperCommandGoal goal;
     goal.command.position = 0.0;
-    goal.command.max_effort = 0.0;
 
     // send last point
     int tpoint = trajectory.joint_trajectory.points.size() - 1;
@@ -130,7 +130,13 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
       goal.command.position += trajectory.joint_trajectory.points[tpoint].positions[idx];
 
       if (trajectory.joint_trajectory.points[tpoint].effort.size() > idx)
+      {
         goal.command.max_effort = trajectory.joint_trajectory.points[tpoint].effort[idx];
+      }
+      else
+      {
+        goal.command.max_effort = max_effort_;
+      }
     }
 
     controller_action_client_->sendGoal(
@@ -202,6 +208,12 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
    */
   bool parallel_jaw_gripper_;
 
+  /*
+   * The ``max_effort`` used in the GripperCommand message when no ``max_effort`` was
+   * specified in the requested trajectory. Defaults to ``0.0``.
+   */
+  double max_effort_;
+
   /*
    * A GripperCommand message has only a single float64 for the
    * "command", thus only a single joint angle can be sent -- however,
diff --git a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp
index 37172863b2e..e4e32753944 100644
--- a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp
+++ b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp
@@ -94,7 +94,10 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo
         ActionBasedControllerHandleBasePtr new_handle;
         if (type == "GripperCommand")
         {
-          new_handle = std::make_shared<GripperControllerHandle>(name, action_ns);
+          const double max_effort =
+              controller_list[i].hasMember("max_effort") ? double(controller_list[i]["max_effort"]) : 0.0;
+
+          new_handle = std::make_shared<GripperControllerHandle>(name, action_ns, max_effort);
           if (static_cast<GripperControllerHandle*>(new_handle.get())->isConnected())
           {
             if (controller_list[i].hasMember("parallel"))