55 return "Fix Start State Path Constraints";
61 std::vector<std::size_t>& added_path_index)
const 66 robot_state::RobotState start_state = planning_scene->getCurrentState();
67 robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state);
70 if (planning_scene->isStateValid(start_state, req.group_name) &&
71 !planning_scene->isStateValid(start_state, req.path_constraints, req.group_name))
73 ROS_INFO(
"Path constraints not satisfied for start state...");
74 planning_scene->isStateValid(start_state, req.path_constraints, req.group_name,
true);
75 ROS_INFO(
"Planning to path constraints...");
78 req2.goal_constraints.resize(1);
79 req2.goal_constraints[0] = req.path_constraints;
80 req2.path_constraints = moveit_msgs::Constraints();
84 std::vector<std::size_t> added_path_index_temp;
85 added_path_index_temp.swap(added_path_index);
86 bool solved1 = planner(planning_scene, req2, res2);
87 added_path_index_temp.swap(added_path_index);
92 ROS_INFO(
"Planned to path constraints. Resuming original planning request.");
95 robot_state::robotStateToRobotStateMsg(res2.
trajectory_->getLastWayPoint(), req3.start_state);
96 bool solved2 = planner(planning_scene, req3, res);
102 for (std::size_t i = 0; i < added_path_index.size(); ++i)
103 added_path_index[i] += res2.
trajectory_->getWayPointCount();
106 for (std::size_t i = 0; i < res2.
trajectory_->getWayPointCount(); ++i)
107 added_path_index.push_back(i);
119 ROS_WARN(
"Unable to plan to path constraints. Running usual motion plan.");
120 bool result = planner(planning_scene, req, res);
127 ROS_DEBUG(
"Path constraints are OK. Running usual motion plan.");
128 return planner(planning_scene, req, res);
robot_trajectory::RobotTrajectoryPtr trajectory_
virtual std::string getDescription() const
FixStartStatePathConstraints()
virtual bool adaptAndPlan(const PlannerFn &planner, const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res, std::vector< std::size_t > &added_path_index) const
moveit_msgs::MotionPlanRequest MotionPlanRequest
boost::function< bool(const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res)> PlannerFn
CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixStartStatePathConstraints, planning_request_adapter::PlanningRequestAdapter)