Add max_effort parameter to GripperCommand action (#1162)

Backport of moveit/moveit#2984 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. (cherry picked from commit 38b84de) Co-authored-by: Rick Staa <[email protected]>
moveit · Apr 21, 2022 · c01a853 · c01a853
1 parent fa983f7
commit c01a853
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 3 deletions.
diff --git a/...e_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h b/...e_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h
@@ -51,11 +51,13 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
 {
 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)
+  GripperControllerHandle(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)
     , parallel_jaw_gripper_(false)
+    , max_effort_(max_effort)
   {
   }
 
@@ -112,7 +114,6 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
     // goal to be sent
     control_msgs::action::GripperCommand::Goal goal;
     goal.command.position = 0.0;
-    goal.command.max_effort = 0.0;
 
     // send last point
     int tpoint = trajectory.joint_trajectory.points.size() - 1;
@@ -132,7 +133,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_;
+      }
     }
     rclcpp_action::Client<control_msgs::action::GripperCommand>::SendGoalOptions send_goal_options;
     // Active callback
@@ -202,6 +209,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
@@ -102,7 +102,14 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo
         ActionBasedControllerHandleBasePtr new_handle;
         if (type == "GripperCommand")
         {
-          new_handle = std::make_shared<GripperControllerHandle>(node_, controller_name, action_ns);
+          double max_effort;
+          if (!node->get_parameter(PARAM_BASE_NAME + "." + controller_name + ".max_effort", max_effort))
+          {
+            RCLCPP_INFO_STREAM(LOGGER, "Max effort set to 0.0");
+            max_effort = 0.0;
+          }
+
+          new_handle = std::make_shared<GripperControllerHandle>(node_, controller_name, action_ns, max_effort);
           if (static_cast<GripperControllerHandle*>(new_handle.get())->isConnected())
           {
             bool parallel_gripper = false;