Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/pick_place/pick_place.h>
00038 #include <moveit/pick_place/plan_stage.h>
00039 #include <moveit/kinematic_constraints/utils.h>
00040 #include <ros/console.h>
00041
00042 namespace pick_place
00043 {
00044
00045 PlanStage::PlanStage(const planning_scene::PlanningSceneConstPtr &scene,
00046 const planning_pipeline::PlanningPipelinePtr &planning_pipeline) :
00047 ManipulationStage("plan"),
00048 planning_scene_(scene),
00049 planning_pipeline_(planning_pipeline)
00050 {
00051 }
00052
00053 void PlanStage::signalStop()
00054 {
00055 ManipulationStage::signalStop();
00056 planning_pipeline_->terminate();
00057 }
00058
00059
00060 bool PlanStage::evaluate(const ManipulationPlanPtr &plan) const
00061 {
00062 planning_interface::MotionPlanRequest req;
00063 planning_interface::MotionPlanResponse res;
00064 req.group_name = plan->shared_data_->planning_group_->getName();
00065 req.num_planning_attempts = 1;
00066 req.allowed_planning_time = (plan->shared_data_->timeout_ - ros::WallTime::now()).toSec();
00067 req.path_constraints = plan->shared_data_->path_constraints_;
00068 req.planner_id = plan->shared_data_->planner_id_;
00069 req.start_state.is_diff = true;
00070
00071 req.goal_constraints.resize(1, kinematic_constraints::constructGoalConstraints(*plan->approach_state_, plan->shared_data_->planning_group_));
00072 unsigned int attempts = 0;
00073 do
00074 {
00075 attempts++;
00076 if (!signal_stop_ && planning_pipeline_->generatePlan(planning_scene_, req, res) &&
00077 res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS &&
00078 res.trajectory_ && !res.trajectory_->empty())
00079 {
00080
00081 if (!plan->approach_posture_.joint_names.empty())
00082 {
00083 robot_state::RobotStatePtr pre_approach_state(new robot_state::RobotState(res.trajectory_->getLastWayPoint()));
00084 robot_trajectory::RobotTrajectoryPtr pre_approach_traj(new robot_trajectory::RobotTrajectory(
00085 pre_approach_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName()));
00086 pre_approach_traj->setRobotTrajectoryMsg(*pre_approach_state, plan->approach_posture_);
00087
00088
00089
00090 if (plan->approach_posture_.points.size() > 0 && plan->approach_posture_.points.back().time_from_start > ros::Duration(0.0)){
00091 pre_approach_traj->addPrefixWayPoint(pre_approach_state, 0.0);
00092 }
00093 else {
00094 pre_approach_traj->addPrefixWayPoint(pre_approach_state, PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION);
00095 }
00096
00097
00098 plan_execution::ExecutableTrajectory et(pre_approach_traj, "pre_grasp");
00099 plan->trajectories_.insert(plan->trajectories_.begin(), et);
00100
00101 }
00102
00103
00104 plan_execution::ExecutableTrajectory et(res.trajectory_, name_);
00105 plan->trajectories_.insert(plan->trajectories_.begin(), et);
00106
00107 plan->error_code_ = res.error_code_;
00108
00109 return true;
00110 }
00111 else
00112 plan->error_code_ = res.error_code_;
00113 }
00114
00115 while (!signal_stop_ && plan->error_code_.val == moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN && attempts < 2);
00116
00117 return false;
00118 }
00119
00120 }