gripper_controller_handle.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Unbounded Robotics Inc.
5  * Copyright (c) 2012, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the Willow Garage nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Michael Ferguson, Ioan Sucan, E. Gil Jones */
37 
38 #ifndef MOVEIT_PLUGINS_GRIPPER_CONTROLLER_HANDLE
39 #define MOVEIT_PLUGINS_GRIPPER_CONTROLLER_HANDLE
40 
42 #include <control_msgs/GripperCommandAction.h>
43 #include <set>
44 
46 {
47 /*
48  * This is an interface for a gripper using control_msgs/GripperCommandAction
49  * action interface (single DOF).
50  */
51 class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs::GripperCommandAction>
52 {
53 public:
54  /* Topics will map to name/ns/goal, name/ns/result, etc */
55  GripperControllerHandle(const std::string& name, const std::string& ns)
56  : ActionBasedControllerHandle<control_msgs::GripperCommandAction>(name, ns)
57  , allow_failure_(false)
58  , parallel_jaw_gripper_(false)
59  {
60  }
61 
62  virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory& trajectory)
63  {
64  ROS_DEBUG_STREAM_NAMED("GripperController", "Received new trajectory for " << name_);
65 
67  return false;
68 
69  if (!trajectory.multi_dof_joint_trajectory.points.empty())
70  {
71  ROS_ERROR_NAMED("GripperController", "Gripper cannot execute multi-dof trajectories.");
72  return false;
73  }
74 
75  if (trajectory.joint_trajectory.points.empty())
76  {
77  ROS_ERROR_NAMED("GripperController", "GripperController requires at least one joint trajectory point.");
78  return false;
79  }
80 
81  if (trajectory.joint_trajectory.points.size() > 1)
82  {
83  ROS_DEBUG_STREAM_NAMED("GripperController", "Trajectory: " << trajectory.joint_trajectory);
84  }
85 
86  if (trajectory.joint_trajectory.joint_names.empty())
87  {
88  ROS_ERROR_NAMED("GripperController", "No joint names specified");
89  return false;
90  }
91 
92  std::vector<std::size_t> gripper_joint_indexes;
93  for (std::size_t i = 0; i < trajectory.joint_trajectory.joint_names.size(); ++i)
94  {
95  if (command_joints_.find(trajectory.joint_trajectory.joint_names[i]) != command_joints_.end())
96  {
97  gripper_joint_indexes.push_back(i);
99  break;
100  }
101  }
102 
103  if (gripper_joint_indexes.empty())
104  {
105  ROS_WARN_NAMED("GripperController", "No command_joint was specified for the MoveIt! controller gripper handle. \
106  Please see GripperControllerHandle::addCommandJoint() and \
107  GripperControllerHandle::setCommandJoint(). Assuming index 0.");
108  gripper_joint_indexes.push_back(0);
109  }
110 
111  // goal to be sent
112  control_msgs::GripperCommandGoal goal;
113  goal.command.position = 0.0;
114  goal.command.max_effort = 0.0;
115 
116  // send last point
117  int tpoint = trajectory.joint_trajectory.points.size() - 1;
118  ROS_DEBUG_NAMED("GripperController", "Sending command from trajectory point %d", tpoint);
119 
120  // fill in goal from last point
121  for (std::size_t i = 0; i < gripper_joint_indexes.size(); ++i)
122  {
123  std::size_t idx = gripper_joint_indexes[i];
124 
125  if (trajectory.joint_trajectory.points[tpoint].positions.size() <= idx)
126  {
127  ROS_ERROR_NAMED("GripperController", "GripperController expects a joint trajectory with one \
128  point that specifies at least the position of joint \
129  '%s', but insufficient positions provided",
130  trajectory.joint_trajectory.joint_names[idx].c_str());
131  return false;
132  }
133  goal.command.position += trajectory.joint_trajectory.points[tpoint].positions[idx];
134 
135  if (trajectory.joint_trajectory.points[tpoint].effort.size() > idx)
136  goal.command.max_effort = trajectory.joint_trajectory.points[tpoint].effort[idx];
137  }
138 
139  controller_action_client_->sendGoal(goal,
140  boost::bind(&GripperControllerHandle::controllerDoneCallback, this, _1, _2),
143 
144  done_ = false;
146  return true;
147  }
148 
149  void setCommandJoint(const std::string& name)
150  {
151  command_joints_.clear();
152  addCommandJoint(name);
153  }
154 
155  void addCommandJoint(const std::string& name)
156  {
157  command_joints_.insert(name);
158  }
159 
160  void allowFailure(bool allow)
161  {
162  allow_failure_ = allow;
163  }
164 
165  void setParallelJawGripper(const std::string& left, const std::string& right)
166  {
167  addCommandJoint(left);
168  addCommandJoint(right);
169  parallel_jaw_gripper_ = true;
170  }
171 
172 private:
174  const control_msgs::GripperCommandResultConstPtr& result)
175  {
178  else
180  }
181 
183  {
184  ROS_DEBUG_STREAM_NAMED("GripperController", name_ << " started execution");
185  }
186 
187  void controllerFeedbackCallback(const control_msgs::GripperCommandFeedbackConstPtr& feedback)
188  {
189  }
190 
191  /*
192  * Some gripper drivers may indicate a failure if they do not close all the way when
193  * an object is in the gripper.
194  */
196 
197  /*
198  * A common setup is where there are two joints that each move
199  * half the overall distance. Thus, the command to the gripper
200  * should be the sum of the two joint distances.
201  *
202  * When this is set, command_joints_ should be of size 2,
203  * and the command will be the sum of the two joints.
204  */
206 
207  /*
208  * A GripperCommand message has only a single float64 for the
209  * "command", thus only a single joint angle can be sent -- however,
210  * due to the complexity of making grippers look correct in a URDF,
211  * they typically have >1 joints. The "command_joint" is the joint
212  * whose position value will be sent in the GripperCommand action. A
213  * set of names is provided for acceptable joint names. If any of
214  * the joints specified is found, the value corresponding to that
215  * joint is considered the command.
216  */
217  std::set<std::string> command_joints_;
218 };
219 
220 } // end namespace moveit_simple_controller_manager
221 
222 #endif // MOVEIT_PLUGINS_GRIPPER_CONTROLLER_HANDLE
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_WARN_NAMED(name,...)
void controllerDoneCallback(const actionlib::SimpleClientGoalState &state, const control_msgs::GripperCommandResultConstPtr &result)
#define ROS_DEBUG_NAMED(name,...)
GripperControllerHandle(const std::string &name, const std::string &ns)
std::shared_ptr< actionlib::SimpleActionClient< control_msgs::GripperCommandAction > > controller_action_client_
#define ROS_ERROR_NAMED(name,...)
void setParallelJawGripper(const std::string &left, const std::string &right)
virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory &trajectory)
void controllerFeedbackCallback(const control_msgs::GripperCommandFeedbackConstPtr &feedback)


moveit_simple_controller_manager
Author(s): Michael Ferguson
autogenerated on Wed Jul 10 2019 04:04:40