From cc92dff3be42caebfe356267be3695f946ad201a 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.
---
 .../gripper_controller_handle.h                   | 15 ++++++++++++---
 .../src/moveit_simple_controller_manager.cpp      |  9 ++++++++-
 2 files changed, 20 insertions(+), 4 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..7e278fafac3 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;
@@ -129,8 +129,11 @@ 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)
+      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 +205,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..ac7b6927b7b 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
@@ -43,6 +43,7 @@
 #include <pluginlib/class_list_macros.hpp>
 #include <algorithm>
 #include <map>
+#include <boost/thread/thread.hpp>
 
 using namespace moveit::core;
 
@@ -55,6 +56,10 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo
 public:
   MoveItSimpleControllerManager() : node_handle_("~")
   {
+    int t = 0;
+    while(t == 0)
+      boost::this_thread::sleep( boost::posix_time::seconds(1) );
+
     if (!node_handle_.hasParam("controller_list"))
     {
       ROS_ERROR_STREAM_NAMED(LOGNAME, "No controller_list specified.");
@@ -94,7 +99,9 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo
         ActionBasedControllerHandleBasePtr new_handle;
         if (type == "GripperCommand")
         {
-          new_handle = std::make_shared<GripperControllerHandle>(name, action_ns);
+          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"))