summaryrefslogtreecommitdiff
path: root/moveit-simple-controller-manager-remove-parallel-gripper-command.patch
blob: d875987f27c2f94269ea3d197678c36a5b48b73c (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
diff --git a/src/moveit_simple_controller_manager.cpp b/src/moveit_simple_controller_manager.cpp
--- a/src/moveit_simple_controller_manager.cpp
+++ b/src/moveit_simple_controller_manager.cpp
@@ -37,7 +37,6 @@
 
 #include <moveit_simple_controller_manager/action_based_controller_handle.hpp>
 #include <moveit_simple_controller_manager/gripper_command_controller_handle.hpp>
-#include <moveit_simple_controller_manager/parallel_gripper_command_controller_handle.hpp>
 #include <moveit_simple_controller_manager/follow_joint_trajectory_controller_handle.hpp>
 #include <boost/algorithm/string/join.hpp>
 #include <pluginlib/class_list_macros.hpp>
@@ -182,37 +181,6 @@
           RCLCPP_INFO_STREAM(getLogger(), "Added GripperCommand controller for " << controller_name);
           controllers_[controller_name] = new_handle;
         }
-        else if (type == "ParallelGripperCommand")
-        {
-          double max_effort;
-          node_->get_parameter_or(makeParameterName(PARAM_BASE_NAME, controller_name, "max_effort"), max_effort, 0.0);
-          if (max_effort > 0.0)
-          {
-            RCLCPP_INFO_STREAM(getLogger(), controller_name << " will command a max effort of: " << max_effort);
-          }
-
-          double max_velocity;
-          node_->get_parameter_or(makeParameterName(PARAM_BASE_NAME, controller_name, "max_velocity"), max_velocity,
-                                  0.0);
-          if (max_effort > 0.0)
-          {
-            RCLCPP_INFO_STREAM(getLogger(), controller_name << " will command a max velocity of: " << max_velocity);
-          }
-
-          new_handle = std::make_shared<ParallelGripperCommandControllerHandle>(node_, controller_name, action_ns,
-                                                                                max_effort, max_velocity);
-
-          if (controller_joints.size() > 1)
-          {
-            RCLCPP_WARN_STREAM(getLogger(), "ParallelGripperCommand controller only supports commanding a single joint "
-                                            "and multiple joint names were specified. Assuming control of joint: "
-                                                << controller_joints[0]);
-          }
-          static_cast<ParallelGripperCommandControllerHandle*>(new_handle.get())->setCommandJoint(controller_joints[0]);
-
-          RCLCPP_INFO_STREAM(getLogger(), "Added ParallelGripperCommand controller for " << controller_name);
-          controllers_[controller_name] = new_handle;
-        }
         else if (type == "FollowJointTrajectory")
         {
           new_handle = std::make_shared<FollowJointTrajectoryControllerHandle>(node_, controller_name, action_ns);