pick_place_action_capability.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
41 
43 
45  : MoveGroupCapability("PickPlaceAction"), pickup_state_(IDLE)
46 {
47 }
48 
50 {
51  pick_place_.reset(new pick_place::PickPlace(context_->planning_pipeline_));
52  pick_place_->displayComputedMotionPlans(true);
53 
54  if (context_->debug_)
55  pick_place_->displayProcessedGrasps(true);
56 
57  // start the pickup action server
60  false));
61  pickup_action_server_->registerPreemptCallback(boost::bind(&MoveGroupPickPlaceAction::preemptPickupCallback, this));
62  pickup_action_server_->start();
63 
64  // start the place action server
67  place_action_server_->registerPreemptCallback(boost::bind(&MoveGroupPickPlaceAction::preemptPlaceCallback, this));
68  place_action_server_->start();
69 }
70 
72 {
74 }
75 
77 {
79 }
80 
82 {
84 }
85 
87 {
89 }
90 
91 void move_group::MoveGroupPickPlaceAction::executePickupCallback_PlanOnly(const moveit_msgs::PickupGoalConstPtr& goal,
92  moveit_msgs::PickupResult& action_res)
93 {
94  pick_place::PickPlanPtr plan;
95  try
96  {
97  planning_scene_monitor::LockedPlanningSceneRO ps(context_->planning_scene_monitor_);
98  plan = pick_place_->planPick(ps, *goal);
99  }
100  catch (std::exception& ex)
101  {
102  ROS_ERROR_NAMED("manipulation", "Pick&place threw an exception: %s", ex.what());
103  }
104 
105  if (plan)
106  {
107  const std::vector<pick_place::ManipulationPlanPtr>& success = plan->getSuccessfulManipulationPlans();
108  if (success.empty())
109  {
110  action_res.error_code = plan->getErrorCode();
111  }
112  else
113  {
114  const pick_place::ManipulationPlanPtr& result = success.back();
115  convertToMsg(result->trajectories_, action_res.trajectory_start, action_res.trajectory_stages);
116  action_res.trajectory_descriptions.resize(result->trajectories_.size());
117  for (std::size_t i = 0; i < result->trajectories_.size(); ++i)
118  action_res.trajectory_descriptions[i] = result->trajectories_[i].description_;
119  if (result->id_ < goal->possible_grasps.size())
120  action_res.grasp = goal->possible_grasps[result->id_];
121  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
122  }
123  }
124  else
125  {
126  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
127  }
128 }
129 
130 void move_group::MoveGroupPickPlaceAction::executePlaceCallback_PlanOnly(const moveit_msgs::PlaceGoalConstPtr& goal,
131  moveit_msgs::PlaceResult& action_res)
132 {
133  pick_place::PlacePlanPtr plan;
134  try
135  {
136  planning_scene_monitor::LockedPlanningSceneRO ps(context_->planning_scene_monitor_);
137  plan = pick_place_->planPlace(ps, *goal);
138  }
139  catch (std::exception& ex)
140  {
141  ROS_ERROR_NAMED("manipulation", "Pick&place threw an exception: %s", ex.what());
142  }
143 
144  if (plan)
145  {
146  const std::vector<pick_place::ManipulationPlanPtr>& success = plan->getSuccessfulManipulationPlans();
147  if (success.empty())
148  {
149  action_res.error_code = plan->getErrorCode();
150  }
151  else
152  {
153  const pick_place::ManipulationPlanPtr& result = success.back();
154  convertToMsg(result->trajectories_, action_res.trajectory_start, action_res.trajectory_stages);
155  action_res.trajectory_descriptions.resize(result->trajectories_.size());
156  for (std::size_t i = 0; i < result->trajectories_.size(); ++i)
157  action_res.trajectory_descriptions[i] = result->trajectories_[i].description_;
158  if (result->id_ < goal->place_locations.size())
159  action_res.place_location = goal->place_locations[result->id_];
160  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
161  }
162  }
163  else
164  {
165  action_res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
166  }
167 }
168 
170  moveit_msgs::PickupResult* action_res,
172 {
174 
176 
177  pick_place::PickPlanPtr pick_plan;
178  try
179  {
180  pick_plan = pick_place_->planPick(plan.planning_scene_, goal);
181  }
182  catch (std::exception& ex)
183  {
184  ROS_ERROR_NAMED("manipulation", "Pick&place threw an exception: %s", ex.what());
185  }
186 
187  if (pick_plan)
188  {
189  const std::vector<pick_place::ManipulationPlanPtr>& success = pick_plan->getSuccessfulManipulationPlans();
190  if (success.empty())
191  {
192  plan.error_code_ = pick_plan->getErrorCode();
193  }
194  else
195  {
196  const pick_place::ManipulationPlanPtr& result = success.back();
197  plan.plan_components_ = result->trajectories_;
198  if (result->id_ < goal.possible_grasps.size())
199  action_res->grasp = goal.possible_grasps[result->id_];
200  plan.error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
201  }
202  }
203  else
204  {
205  plan.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
206  }
207 
208  return plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
209 }
210 
212  moveit_msgs::PlaceResult* action_res,
214 {
216 
218 
219  pick_place::PlacePlanPtr place_plan;
220  try
221  {
222  place_plan = pick_place_->planPlace(plan.planning_scene_, goal);
223  }
224  catch (std::exception& ex)
225  {
226  ROS_ERROR_NAMED("manipulation", "Pick&place threw an exception: %s", ex.what());
227  }
228 
229  if (place_plan)
230  {
231  const std::vector<pick_place::ManipulationPlanPtr>& success = place_plan->getSuccessfulManipulationPlans();
232  if (success.empty())
233  {
234  plan.error_code_ = place_plan->getErrorCode();
235  }
236  else
237  {
238  const pick_place::ManipulationPlanPtr& result = success.back();
239  plan.plan_components_ = result->trajectories_;
240  if (result->id_ < goal.place_locations.size())
241  action_res->place_location = goal.place_locations[result->id_];
242  plan.error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
243  }
244  }
245  else
246  {
247  plan.error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
248  }
249 
250  return plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
251 }
252 
254  const moveit_msgs::PickupGoalConstPtr& goal, moveit_msgs::PickupResult& action_res)
255 {
257 
258  opt.replan_ = goal->planning_options.replan;
259  opt.replan_attempts_ = goal->planning_options.replan_attempts;
260  opt.replan_delay_ = goal->planning_options.replan_delay;
262 
263  opt.plan_callback_ =
264  boost::bind(&MoveGroupPickPlaceAction::planUsingPickPlace_Pickup, this, boost::cref(*goal), &action_res, _1);
265  if (goal->planning_options.look_around && context_->plan_with_sensing_)
266  {
267  opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(),
268  _1, opt.plan_callback_, goal->planning_options.look_around_attempts,
269  goal->planning_options.max_safe_execution_cost);
270  context_->plan_with_sensing_->setBeforeLookCallback(
272  }
273 
275  context_->plan_execution_->planAndExecute(plan, goal->planning_options.planning_scene_diff, opt);
276 
277  convertToMsg(plan.plan_components_, action_res.trajectory_start, action_res.trajectory_stages);
278  action_res.trajectory_descriptions.resize(plan.plan_components_.size());
279  for (std::size_t i = 0; i < plan.plan_components_.size(); ++i)
280  action_res.trajectory_descriptions[i] = plan.plan_components_[i].description_;
281  action_res.error_code = plan.error_code_;
282 }
283 
285  const moveit_msgs::PlaceGoalConstPtr& goal, moveit_msgs::PlaceResult& action_res)
286 {
288 
289  opt.replan_ = goal->planning_options.replan;
290  opt.replan_attempts_ = goal->planning_options.replan_attempts;
291  opt.replan_delay_ = goal->planning_options.replan_delay;
293  opt.plan_callback_ =
294  boost::bind(&MoveGroupPickPlaceAction::planUsingPickPlace_Place, this, boost::cref(*goal), &action_res, _1);
295  if (goal->planning_options.look_around && context_->plan_with_sensing_)
296  {
297  opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(),
298  _1, opt.plan_callback_, goal->planning_options.look_around_attempts,
299  goal->planning_options.max_safe_execution_cost);
300  context_->plan_with_sensing_->setBeforeLookCallback(
302  }
303 
305  context_->plan_execution_->planAndExecute(plan, goal->planning_options.planning_scene_diff, opt);
306 
307  convertToMsg(plan.plan_components_, action_res.trajectory_start, action_res.trajectory_stages);
308  action_res.trajectory_descriptions.resize(plan.plan_components_.size());
309  for (std::size_t i = 0; i < plan.plan_components_.size(); ++i)
310  action_res.trajectory_descriptions[i] = plan.plan_components_[i].description_;
311  action_res.error_code = plan.error_code_;
312 }
313 
314 void move_group::MoveGroupPickPlaceAction::executePickupCallback(const moveit_msgs::PickupGoalConstPtr& input_goal)
315 {
317 
318  // before we start planning, ensure that we have the latest robot state received...
319  context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now());
320  context_->planning_scene_monitor_->updateFrameTransforms();
321 
322  moveit_msgs::PickupGoalConstPtr goal;
323  if (input_goal->possible_grasps.empty())
324  {
325  moveit_msgs::PickupGoal* copy(new moveit_msgs::PickupGoal(*input_goal));
326  goal.reset(copy);
327  fillGrasps(*copy);
328  }
329  else
330  goal = input_goal;
331 
332  moveit_msgs::PickupResult action_res;
333 
334  if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_)
335  {
336  if (!goal->planning_options.plan_only)
337  ROS_WARN_NAMED("manipulation", "This instance of MoveGroup is not allowed to execute trajectories but the pick "
338  "goal request has plan_only set to false. Only a motion plan will be computed "
339  "anyway.");
340  executePickupCallback_PlanOnly(goal, action_res);
341  }
342  else
343  executePickupCallback_PlanAndExecute(goal, action_res);
344 
345  bool planned_trajectory_empty = action_res.trajectory_stages.empty();
346  std::string response =
347  getActionResultString(action_res.error_code, planned_trajectory_empty, goal->planning_options.plan_only);
348  if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
349  pickup_action_server_->setSucceeded(action_res, response);
350  else
351  {
352  if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
353  pickup_action_server_->setPreempted(action_res, response);
354  else
355  pickup_action_server_->setAborted(action_res, response);
356  }
357 
359 }
360 
361 void move_group::MoveGroupPickPlaceAction::executePlaceCallback(const moveit_msgs::PlaceGoalConstPtr& goal)
362 {
364 
365  // before we start planning, ensure that we have the latest robot state received...
366  context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now());
367  context_->planning_scene_monitor_->updateFrameTransforms();
368 
369  moveit_msgs::PlaceResult action_res;
370 
371  if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_)
372  {
373  if (!goal->planning_options.plan_only)
374  ROS_WARN_NAMED("manipulation", "This instance of MoveGroup is not allowed to execute trajectories but the place "
375  "goal request has plan_only set to false. Only a motion plan will be computed "
376  "anyway.");
377  executePlaceCallback_PlanOnly(goal, action_res);
378  }
379  else
380  executePlaceCallback_PlanAndExecute(goal, action_res);
381 
382  bool planned_trajectory_empty = action_res.trajectory_stages.empty();
383  std::string response =
384  getActionResultString(action_res.error_code, planned_trajectory_empty, goal->planning_options.plan_only);
385  if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
386  place_action_server_->setSucceeded(action_res, response);
387  else
388  {
389  if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
390  place_action_server_->setPreempted(action_res, response);
391  else
392  place_action_server_->setAborted(action_res, response);
393  }
394 
396 }
397 
399 {
400 }
401 
403 {
404 }
405 
407 {
408  pickup_state_ = state;
409  pickup_feedback_.state = stateToStr(state);
410  pickup_action_server_->publishFeedback(pickup_feedback_);
411 }
412 
414 {
415  place_state_ = state;
416  place_feedback_.state = stateToStr(state);
417  place_action_server_->publishFeedback(place_feedback_);
418 }
419 
420 void move_group::MoveGroupPickPlaceAction::fillGrasps(moveit_msgs::PickupGoal& goal)
421 {
422  planning_scene_monitor::LockedPlanningSceneRO lscene(context_->planning_scene_monitor_);
423 
424  ROS_DEBUG_NAMED("manipulation", "Using default grasp poses");
425  goal.minimize_object_distance = true;
426 
427  // add a number of default grasp points
428  // \todo add more!
429  moveit_msgs::Grasp g;
430  g.grasp_pose.header.frame_id = goal.target_name;
431  g.grasp_pose.pose.position.x = -0.2;
432  g.grasp_pose.pose.position.y = 0.0;
433  g.grasp_pose.pose.position.z = 0.0;
434  g.grasp_pose.pose.orientation.x = 0.0;
435  g.grasp_pose.pose.orientation.y = 0.0;
436  g.grasp_pose.pose.orientation.z = 0.0;
437  g.grasp_pose.pose.orientation.w = 1.0;
438 
439  g.pre_grasp_approach.direction.header.frame_id = lscene->getPlanningFrame();
440  g.pre_grasp_approach.direction.vector.x = 1.0;
441  g.pre_grasp_approach.min_distance = 0.1;
442  g.pre_grasp_approach.desired_distance = 0.2;
443 
444  g.post_grasp_retreat.direction.header.frame_id = lscene->getPlanningFrame();
445  g.post_grasp_retreat.direction.vector.z = 1.0;
446  g.post_grasp_retreat.min_distance = 0.1;
447  g.post_grasp_retreat.desired_distance = 0.2;
448 
449  if (lscene->getRobotModel()->hasEndEffector(goal.end_effector))
450  {
451  g.pre_grasp_posture.joint_names = lscene->getRobotModel()->getEndEffector(goal.end_effector)->getJointModelNames();
452  g.pre_grasp_posture.points.resize(1);
453  g.pre_grasp_posture.points[0].positions.resize(g.pre_grasp_posture.joint_names.size(),
454  std::numeric_limits<double>::max());
455 
456  g.grasp_posture.joint_names = g.pre_grasp_posture.joint_names;
457  g.grasp_posture.points.resize(1);
458  g.grasp_posture.points[0].positions.resize(g.grasp_posture.joint_names.size(), -std::numeric_limits<double>::max());
459  }
460  goal.possible_grasps.push_back(g);
461 }
462 
std::unique_ptr< actionlib::SimpleActionServer< moveit_msgs::PickupAction > > pickup_action_server_
void executePlaceCallback(const moveit_msgs::PlaceGoalConstPtr &goal)
void executePickupCallback_PlanAndExecute(const moveit_msgs::PickupGoalConstPtr &goal, moveit_msgs::PickupResult &action_res)
#define ROS_WARN_NAMED(name,...)
std::string stateToStr(MoveGroupState state) const
bool planUsingPickPlace_Place(const moveit_msgs::PlaceGoal &goal, moveit_msgs::PlaceResult *action_res, plan_execution::ExecutableMotionPlan &plan)
moveit_msgs::MoveItErrorCodes error_code_
void executePickupCallback_PlanOnly(const moveit_msgs::PickupGoalConstPtr &goal, moveit_msgs::PickupResult &action_res)
static const std::string PICKUP_ACTION
MoveGroupContextPtr context_
#define ROS_DEBUG_NAMED(name,...)
void executePlaceCallback_PlanAndExecute(const moveit_msgs::PlaceGoalConstPtr &goal, moveit_msgs::PlaceResult &action_res)
planning_scene::PlanningSceneConstPtr planning_scene_
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_
bool planUsingPickPlace_Pickup(const moveit_msgs::PickupGoal &goal, moveit_msgs::PickupResult *action_res, plan_execution::ExecutableMotionPlan &plan)
static const std::string PLACE_ACTION
bool computePlan(ExecutableMotionPlan &plan, const ExecutableMotionPlanComputationFn &motion_planner, unsigned int max_look_attempts, double max_safe_path_cost)
ExecutableMotionPlanComputationFn plan_callback_
std::string getActionResultString(const moveit_msgs::MoveItErrorCodes &error_code, bool planned_trajectory_empty, bool plan_only)
void executePlaceCallback_PlanOnly(const moveit_msgs::PlaceGoalConstPtr &goal, moveit_msgs::PlaceResult &action_res)
void executePickupCallback(const moveit_msgs::PickupGoalConstPtr &goal)
void convertToMsg(const std::vector< plan_execution::ExecutableTrajectory > &trajectory, moveit_msgs::RobotState &first_state_msg, std::vector< moveit_msgs::RobotTrajectory > &trajectory_msg) const
#define ROS_ERROR_NAMED(name,...)
std::unique_ptr< actionlib::SimpleActionServer< moveit_msgs::PlaceAction > > place_action_server_
static Time now()
boost::function< void()> before_execution_callback_
void fillGrasps(moveit_msgs::PickupGoal &goal)
std::vector< ExecutableTrajectory > plan_components_
IDLE
const std::string response
CLASS_LOADER_REGISTER_CLASS(Dog, Base)


manipulation
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jul 10 2019 04:03:57