59 return "Fix Start State Path Constraints";
64 std::vector<std::size_t>& added_path_index)
const override
74 !
planning_scene->isStateValid(start_state, req.path_constraints, req.group_name))
76 ROS_INFO(
"Path constraints not satisfied for start state...");
77 planning_scene->isStateValid(start_state, req.path_constraints, req.group_name,
true);
78 ROS_INFO(
"Planning to path constraints...");
81 req2.goal_constraints.resize(1);
82 req2.goal_constraints[0] = req.path_constraints;
83 req2.path_constraints = moveit_msgs::Constraints();
87 std::vector<std::size_t> added_path_index_temp;
88 added_path_index_temp.swap(added_path_index);
90 added_path_index_temp.swap(added_path_index);
95 ROS_INFO(
"Planned to path constraints. Resuming original planning request.");
105 for (std::size_t& added_index : added_path_index)
106 added_index += res2.
trajectory_->getWayPointCount();
109 for (std::size_t i = 0; i < res2.
trajectory_->getWayPointCount(); ++i)
110 added_path_index.push_back(i);
122 ROS_WARN(
"Unable to plan to path constraints.");
123 res.
error_code_.val = moveit_msgs::MoveItErrorCodes::START_STATE_VIOLATES_PATH_CONSTRAINTS;
130 ROS_DEBUG(
"Path constraints are OK. Running usual motion plan.");