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 PlanStage::PlanStage(const planning_scene::PlanningSceneConstPtr& scene,
00045 const planning_pipeline::PlanningPipelinePtr& planning_pipeline)
00046 : ManipulationStage("plan"), planning_scene_(scene), planning_pipeline_(planning_pipeline)
00047 {
00048 }
00049
00050 void PlanStage::signalStop()
00051 {
00052 ManipulationStage::signalStop();
00053 planning_pipeline_->terminate();
00054 }
00055
00056
00057 bool PlanStage::evaluate(const ManipulationPlanPtr& plan) const
00058 {
00059 planning_interface::MotionPlanRequest req;
00060 planning_interface::MotionPlanResponse res;
00061 req.group_name = plan->shared_data_->planning_group_->getName();
00062 req.num_planning_attempts = 1;
00063 req.allowed_planning_time = (plan->shared_data_->timeout_ - ros::WallTime::now()).toSec();
00064 req.path_constraints = plan->shared_data_->path_constraints_;
00065 req.planner_id = plan->shared_data_->planner_id_;
00066 req.start_state.is_diff = true;
00067
00068 req.goal_constraints.resize(
00069 1, kinematic_constraints::constructGoalConstraints(*plan->approach_state_, plan->shared_data_->planning_group_));
00070 unsigned int attempts = 0;
00071 do
00072 {
00073 attempts++;
00074 if (!signal_stop_ && planning_pipeline_->generatePlan(planning_scene_, req, res) &&
00075 res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS && res.trajectory_ && !res.trajectory_->empty())
00076 {
00077
00078 if (!plan->approach_posture_.joint_names.empty())
00079 {
00080 robot_state::RobotStatePtr pre_approach_state(new robot_state::RobotState(res.trajectory_->getLastWayPoint()));
00081 robot_trajectory::RobotTrajectoryPtr pre_approach_traj(new robot_trajectory::RobotTrajectory(
00082 pre_approach_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName()));
00083 pre_approach_traj->setRobotTrajectoryMsg(*pre_approach_state, plan->approach_posture_);
00084
00085
00086
00087
00088 if (plan->approach_posture_.points.size() > 0 &&
00089 plan->approach_posture_.points.back().time_from_start > ros::Duration(0.0))
00090 {
00091 pre_approach_traj->addPrefixWayPoint(pre_approach_state, 0.0);
00092 }
00093 else
00094 {
00095 pre_approach_traj->addPrefixWayPoint(pre_approach_state,
00096 PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION);
00097 }
00098
00099
00100 plan_execution::ExecutableTrajectory et(pre_approach_traj, "pre_grasp");
00101 plan->trajectories_.insert(plan->trajectories_.begin(), et);
00102 }
00103
00104
00105 plan_execution::ExecutableTrajectory et(res.trajectory_, name_);
00106 plan->trajectories_.insert(plan->trajectories_.begin(), et);
00107
00108 plan->error_code_ = res.error_code_;
00109
00110 return true;
00111 }
00112 else
00113 plan->error_code_ = res.error_code_;
00114 }
00115
00116 while (!signal_stop_ && plan->error_code_.val == moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN && attempts < 2);
00117
00118 return false;
00119 }
00120 }