46 :
ManipulationStage(
"plan"), planning_scene_(scene), planning_pipeline_(planning_pipeline)
61 req.group_name = plan->shared_data_->planning_group_->getName();
62 req.num_planning_attempts = 1;
63 req.allowed_planning_time = (plan->shared_data_->timeout_ -
ros::WallTime::now()).toSec();
64 req.path_constraints = plan->shared_data_->path_constraints_;
65 req.planner_id = plan->shared_data_->planner_id_;
66 req.start_state.is_diff =
true;
68 req.goal_constraints.resize(
70 unsigned int attempts = 0;
78 if (!plan->approach_posture_.joint_names.empty())
80 robot_state::RobotStatePtr pre_approach_state(
new robot_state::RobotState(res.
trajectory_->getLastWayPoint()));
82 pre_approach_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName()));
83 pre_approach_traj->setRobotTrajectoryMsg(*pre_approach_state, plan->approach_posture_);
88 if (plan->approach_posture_.points.size() > 0 &&
89 plan->approach_posture_.points.back().time_from_start >
ros::Duration(0.0))
91 pre_approach_traj->addPrefixWayPoint(pre_approach_state, 0.0);
96 <<
" seconds to the grasp closure time. Assign time_from_start " 97 <<
"to your trajectory to avoid this.");
98 pre_approach_traj->addPrefixWayPoint(pre_approach_state,
104 plan->trajectories_.insert(plan->trajectories_.begin(), et);
109 plan->trajectories_.insert(plan->trajectories_.begin(), et);
119 while (!
signal_stop_ && plan->error_code_.val == moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN && attempts < 2);
robot_trajectory::RobotTrajectoryPtr trajectory_
PlanStage(const planning_scene::PlanningSceneConstPtr &scene, const planning_pipeline::PlanningPipelinePtr &planning_pipeline)
planning_scene::PlanningSceneConstPtr planning_scene_
virtual void signalStop()
planning_pipeline::PlanningPipelinePtr planning_pipeline_
moveit_msgs::MoveItErrorCodes error_code_
moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState &state, const robot_model::JointModelGroup *jmg, double tolerance_below, double tolerance_above)
virtual bool evaluate(const ManipulationPlanPtr &plan) const
moveit_msgs::MotionPlanRequest MotionPlanRequest
virtual void signalStop()
#define ROS_INFO_STREAM(args)
static const double DEFAULT_GRASP_POSTURE_COMPLETION_DURATION