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 bool PlanStage::evaluate(const ManipulationPlanPtr &plan) const
00060 {
00061 planning_interface::MotionPlanRequest req;
00062 planning_interface::MotionPlanResponse res;
00063 req.group_name = plan->shared_data_->planning_group_;
00064 req.num_planning_attempts = 1;
00065 req.allowed_planning_time = (plan->shared_data_->timeout_ - ros::WallTime::now()).toSec();
00066 req.path_constraints = plan->shared_data_->path_constraints_;
00067 req.planner_id = plan->shared_data_->planner_id_;
00068
00069 req.goal_constraints.resize(1, kinematic_constraints::constructGoalConstraints(plan->approach_state_->getJointStateGroup(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 &&
00076 res.trajectory_ && !res.trajectory_->empty())
00077 {
00078 if (!plan->approach_posture_.name.empty())
00079 {
00080 robot_state::RobotStatePtr state(new robot_state::RobotState(res.trajectory_->getLastWayPoint()));
00081 state->setStateValues(plan->approach_posture_);
00082 robot_trajectory::RobotTrajectoryPtr traj(new robot_trajectory::RobotTrajectory(state->getRobotModel(), plan->shared_data_->end_effector_group_));
00083 traj->addSuffixWayPoint(state, PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION);
00084 plan_execution::ExecutableTrajectory et(traj, "pre_grasp");
00085 plan->trajectories_.insert(plan->trajectories_.begin(), et);
00086 }
00087 plan_execution::ExecutableTrajectory et(res.trajectory_, name_);
00088 plan->trajectories_.insert(plan->trajectories_.begin(), et);
00089 plan->error_code_ = res.error_code_;
00090
00091 return true;
00092 }
00093 else
00094 plan->error_code_ = res.error_code_;
00095 }
00096
00097 while (!signal_stop_ && plan->error_code_.val == moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN && attempts < 2);
00098
00099 return false;
00100 }
00101
00102 }