92 moveit_msgs::PickupResult& action_res)
94 pick_place::PickPlanPtr plan;
100 catch (std::exception& ex)
102 ROS_ERROR_NAMED(
"manipulation",
"Pick&place threw an exception: %s", ex.what());
107 const std::vector<pick_place::ManipulationPlanPtr>& success = plan->getSuccessfulManipulationPlans();
110 action_res.error_code = plan->getErrorCode();
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;
126 action_res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
131 moveit_msgs::PlaceResult& action_res)
133 pick_place::PlacePlanPtr plan;
139 catch (std::exception& ex)
141 ROS_ERROR_NAMED(
"manipulation",
"Pick&place threw an exception: %s", ex.what());
146 const std::vector<pick_place::ManipulationPlanPtr>& success = plan->getSuccessfulManipulationPlans();
149 action_res.error_code = plan->getErrorCode();
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;
165 action_res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
170 moveit_msgs::PickupResult* action_res,
177 pick_place::PickPlanPtr pick_plan;
182 catch (std::exception& ex)
184 ROS_ERROR_NAMED(
"manipulation",
"Pick&place threw an exception: %s", ex.what());
189 const std::vector<pick_place::ManipulationPlanPtr>& success = pick_plan->getSuccessfulManipulationPlans();
196 const pick_place::ManipulationPlanPtr& result = success.back();
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;
205 plan.
error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
208 return plan.
error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
212 moveit_msgs::PlaceResult* action_res,
219 pick_place::PlacePlanPtr place_plan;
224 catch (std::exception& ex)
226 ROS_ERROR_NAMED(
"manipulation",
"Pick&place threw an exception: %s", ex.what());
231 const std::vector<pick_place::ManipulationPlanPtr>& success = place_plan->getSuccessfulManipulationPlans();
238 const pick_place::ManipulationPlanPtr& result = success.back();
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;
247 plan.
error_code_.val = moveit_msgs::MoveItErrorCodes::FAILURE;
250 return plan.
error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS;
254 const moveit_msgs::PickupGoalConstPtr& goal, moveit_msgs::PickupResult& action_res)
258 opt.
replan_ = goal->planning_options.replan;
265 if (goal->planning_options.look_around &&
context_->plan_with_sensing_)
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(
275 context_->plan_execution_->planAndExecute(plan, goal->planning_options.planning_scene_diff, opt);
280 action_res.trajectory_descriptions[i] = plan.
plan_components_[i].description_;
285 const moveit_msgs::PlaceGoalConstPtr& goal, moveit_msgs::PlaceResult& action_res)
289 opt.
replan_ = goal->planning_options.replan;
295 if (goal->planning_options.look_around &&
context_->plan_with_sensing_)
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(
305 context_->plan_execution_->planAndExecute(plan, goal->planning_options.planning_scene_diff, opt);
310 action_res.trajectory_descriptions[i] = plan.
plan_components_[i].description_;
320 context_->planning_scene_monitor_->updateFrameTransforms();
322 moveit_msgs::PickupGoalConstPtr goal;
323 if (input_goal->possible_grasps.empty())
325 moveit_msgs::PickupGoal* copy(
new moveit_msgs::PickupGoal(*input_goal));
332 moveit_msgs::PickupResult action_res;
334 if (goal->planning_options.plan_only || !
context_->allow_trajectory_execution_)
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 " 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)
352 if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
367 context_->planning_scene_monitor_->updateFrameTransforms();
369 moveit_msgs::PlaceResult action_res;
371 if (goal->planning_options.plan_only || !
context_->allow_trajectory_execution_)
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 " 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)
389 if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
425 goal.minimize_object_distance =
true;
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;
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;
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;
449 if (lscene->getRobotModel()->hasEndEffector(goal.end_effector))
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());
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());
460 goal.possible_grasps.push_back(g);
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
moveit_msgs::PlaceFeedback place_feedback_
bool planUsingPickPlace_Place(const moveit_msgs::PlaceGoal &goal, moveit_msgs::PlaceResult *action_res, plan_execution::ExecutableMotionPlan &plan)
void setPickupState(MoveGroupState state)
unsigned int replan_attempts_
moveit_msgs::MoveItErrorCodes error_code_
void executePickupCallback_PlanOnly(const moveit_msgs::PickupGoalConstPtr &goal, moveit_msgs::PickupResult &action_res)
MoveGroupState pickup_state_
static const std::string PICKUP_ACTION
void preemptPickupCallback()
MoveGroupContextPtr context_
#define ROS_DEBUG_NAMED(name,...)
void executePlaceCallback_PlanAndExecute(const moveit_msgs::PlaceGoalConstPtr &goal, moveit_msgs::PlaceResult &action_res)
void startPickupExecutionCallback()
virtual void initialize()
planning_scene::PlanningSceneConstPtr planning_scene_
MoveGroupPickPlaceAction()
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)
void setPlaceState(MoveGroupState state)
ExecutableMotionPlanComputationFn plan_callback_
std::string getActionResultString(const moveit_msgs::MoveItErrorCodes &error_code, bool planned_trajectory_empty, bool plan_only)
moveit_msgs::PickupFeedback pickup_feedback_
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
MoveGroupState place_state_
void preemptPlaceCallback()
void startPickupLookCallback()
#define ROS_ERROR_NAMED(name,...)
std::unique_ptr< actionlib::SimpleActionServer< moveit_msgs::PlaceAction > > place_action_server_
boost::function< void()> before_execution_callback_
void fillGrasps(moveit_msgs::PickupGoal &goal)
ros::NodeHandle root_node_handle_
void startPlaceLookCallback()
std::vector< ExecutableTrajectory > plan_components_
pick_place::PickPlacePtr pick_place_
void startPlaceExecutionCallback()
CLASS_LOADER_REGISTER_CLASS(Dog, Base)