本文整理汇总了C++中Motion::updatePriority方法的典型用法代码示例。如果您正苦于以下问题：C++ Motion::updatePriority方法的具体用法？C++ Motion::updatePriority怎么用？C++ Motion::updatePriority使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Motion的用法示例。
在下文中一共展示了Motion::updatePriority方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: if
ompl::base::PlannerStatus ompl::control::PDST::solve(const base::PlannerTerminationCondition &ptc)
{
// Make sure the planner is configured correctly.
// ompl::base::Planner::checkValidity checks that there is at least one
// start state and an ompl::base::Goal object specified and throws an
// exception if this is not the case.
checkValidity();
// depending on how the planning problem is set up, this may be necessary
bsp_->bounds_ = projectionEvaluator_->getBounds();
base::Goal *goal = pdef_->getGoal().get();
goalSampler_ = dynamic_cast<ompl::base::GoalSampleableRegion *>(goal);
// Ensure that we have a state sampler AND a control sampler
if (!sampler_)
sampler_ = si_->allocStateSampler();
if (!controlSampler_)
controlSampler_ = siC_->allocDirectedControlSampler();
// Initialize to correct values depending on whether or not previous calls to solve
// generated an approximate or exact solution. If solve is being called for the first
// time then initializes hasSolution to false and isApproximate to true.
double distanceToGoal, closestDistanceToGoal = std::numeric_limits<double>::infinity();
bool hasSolution = lastGoalMotion_ != nullptr;
bool isApproximate = !hasSolution || !goal->isSatisfied(lastGoalMotion_->endState_, &closestDistanceToGoal);
unsigned int ndim = projectionEvaluator_->getDimension();
// If an exact solution has already been found, do not run for another iteration.
if (hasSolution && !isApproximate)
return ompl::base::PlannerStatus::EXACT_SOLUTION;
// Initialize tree with start state(s)
while (const base::State *st = pis_.nextStart())
{
auto *startMotion = new Motion(si_->cloneState(st));
bsp_->addMotion(startMotion);
startMotion->heapElement_ = priorityQueue_.insert(startMotion);
}
if (priorityQueue_.empty())
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(),
priorityQueue_.size());
base::State *tmpState1 = si_->allocState(), *tmpState2 = si_->allocState();
base::EuclideanProjection tmpProj1(ndim), tmpProj2(ndim);
while (!ptc)
{
// Get the top priority path.
Motion *motionSelected = priorityQueue_.top()->data;
motionSelected->updatePriority();
priorityQueue_.update(motionSelected->heapElement_);
Motion *newMotion = propagateFrom(motionSelected, tmpState1, tmpState2);
if (newMotion == nullptr)
continue;
addMotion(newMotion, bsp_, tmpState1, tmpState2, tmpProj1, tmpProj2);
// Check if the newMotion reached the goal.
hasSolution = goal->isSatisfied(newMotion->endState_, &distanceToGoal);
if (hasSolution)
{
closestDistanceToGoal = distanceToGoal;
lastGoalMotion_ = newMotion;
isApproximate = false;
break;
}
else if (distanceToGoal < closestDistanceToGoal)
{
closestDistanceToGoal = distanceToGoal;
lastGoalMotion_ = newMotion;
}
// subdivide cell that contained selected motion, put motions of that
// cell in subcells and split motions so that they contained within
// one subcell
Cell *cellSelected = motionSelected->cell_;
std::vector<Motion *> motions;
cellSelected->subdivide(ndim);
motions.swap(cellSelected->motions_);
for (auto &motion : motions)
addMotion(motion, cellSelected, tmpState1, tmpState2, tmpProj1, tmpProj2);
}
if (lastGoalMotion_ != nullptr)
hasSolution = true;
// If a solution path has been computed, save it in the problem definition object.
if (hasSolution)
{
Motion *m;
std::vector<unsigned int> durations(
1, findDurationAndAncestor(lastGoalMotion_, lastGoalMotion_->endState_, tmpState1, m));
std::vector<Motion *> mpath(1, m);
//.........这里部分代码省略.........