本文整理汇总了C++中planning_scene::PlanningSceneConstPtr::isStateValid方法的典型用法代码示例。如果您正苦于以下问题：C++ PlanningSceneConstPtr::isStateValid方法的具体用法？C++ PlanningSceneConstPtr::isStateValid怎么用？C++ PlanningSceneConstPtr::isStateValid使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类planning_scene::PlanningSceneConstPtr的用法示例。
在下文中一共展示了PlanningSceneConstPtr::isStateValid方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: adaptAndPlan
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
{
ROS_DEBUG("Running '%s'", getDescription().c_str());
// get the specified start state
robot_state::RobotState start_state = planning_scene->getCurrentState();
robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state);
// if the start state is otherwise valid but does not meet path constraints
if (planning_scene->isStateValid(start_state, req.group_name) &&
!planning_scene->isStateValid(start_state, req.path_constraints, req.group_name))
{
ROS_INFO("Path constraints not satisfied for start state...");
planning_scene->isStateValid(start_state, req.path_constraints, req.group_name, true);
ROS_INFO("Planning to path constraints...");
planning_interface::MotionPlanRequest req2 = req;
req2.goal_constraints.resize(1);
req2.goal_constraints[0] = req.path_constraints;
req2.path_constraints = moveit_msgs::Constraints();
planning_interface::MotionPlanResponse res2;
// we call the planner for this additional request, but we do not want to include potential
// index information from that call
std::vector<std::size_t> added_path_index_temp;
added_path_index_temp.swap(added_path_index);
bool solved1 = planner(planning_scene, req2, res2);
added_path_index_temp.swap(added_path_index);
if (solved1)
{
planning_interface::MotionPlanRequest req3 = req;
ROS_INFO("Planned to path constraints. Resuming original planning request.");
// extract the last state of the computed motion plan and set it as the new start state
robot_state::robotStateToRobotStateMsg(res2.trajectory_->getLastWayPoint(), req3.start_state);
bool solved2 = planner(planning_scene, req3, res);
res.planning_time_ += res2.planning_time_;
if (solved2)
{
// since we add a prefix, we need to correct any existing index positions
for (std::size_t i = 0 ; i < added_path_index.size() ; ++i)
added_path_index[i] += res2.trajectory_->getWayPointCount();
// we mark the fact we insert a prefix path (we specify the index position we just added)
for (std::size_t i = 0 ; i < res2.trajectory_->getWayPointCount() ; ++i)
added_path_index.push_back(i);
// we need to append the solution paths.
res2.trajectory_->append(*res.trajectory_, 0.0);
res2.trajectory_->swap(*res.trajectory_);
return true;
}
else
return false;
}
else
{
ROS_WARN("Unable to plan to path constraints. Running usual motion plan.");
bool result = planner(planning_scene, req, res);
res.planning_time_ += res2.planning_time_;
return result;
}
}
else
{
ROS_DEBUG("Path constraints are OK. Running usual motion plan.");
return planner(planning_scene, req, res);
}
}