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_;
66 req.start_state.is_diff =
true;
69 plan->shared_data_->planning_group_));
70 unsigned int attempts = 0;
78 if (!plan->approach_posture_.joint_names.empty())
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.empty() &&
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.");
103 plan->trajectories_.insert(plan->trajectories_.begin(), et);
108 plan->trajectories_.insert(plan->trajectories_.begin(), et);
118 while (!
signal_stop_ && plan->error_code_.val == moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN && attempts < 2);