63 Vertex::Vertex(
const ompl::base::SpaceInformationPtr &spaceInformation,
64 const ompl::base::ProblemDefinitionPtr &problemDefinition,
const std::size_t &batchId)
65 : spaceInformation_(spaceInformation)
66 , problemDefinition_(problemDefinition)
67 , objective_(problemDefinition->getOptimizationObjective())
72 , state_(spaceInformation->allocState())
73 , costToComeFromStart_(objective_->infiniteCost())
74 , edgeCostFromForwardParent_(objective_->infiniteCost())
75 , costToComeFromGoal_(objective_->infiniteCost())
76 , expandedCostToComeFromGoal_(objective_->infiniteCost())
77 , costToGoToGoal_(objective_->infiniteCost())
78 , vertexId_(generateId())
83 Vertex::Vertex(
const std::shared_ptr<Vertex> &other)
84 : spaceInformation_(other->spaceInformation_)
85 , problemDefinition_(other->problemDefinition_)
86 , objective_(other->objective_)
87 , forwardChildren_(other->forwardChildren_)
88 , reverseChildren_(other->reverseChildren_)
89 , forwardParent_(other->forwardParent_)
90 , reverseParent_(other->reverseParent_)
91 , state_(spaceInformation_->allocState())
92 , costToComeFromStart_(other->costToComeFromStart_)
93 , edgeCostFromForwardParent_(other->edgeCostFromForwardParent_)
94 , costToComeFromGoal_(other->costToComeFromGoal_)
95 , expandedCostToComeFromGoal_(other->expandedCostToComeFromGoal_)
96 , costToGoToGoal_(other->costToGoToGoal_)
97 , vertexId_(other->vertexId_)
98 , batchId_(other->batchId_)
100 spaceInformation_->copyState(state_, other->getState());
223 std::vector<std::weak_ptr<aitstar::Vertex>> Vertex::invalidateReverseBranch()
225 std::vector<std::weak_ptr<aitstar::Vertex>> accumulatedChildren = reverseChildren_;
228 for (
const auto &child : reverseChildren_)
230 child.lock()->setCostToComeFromGoal(objective_->infiniteCost());
231 child.lock()->setExpandedCostToComeFromGoal(objective_->infiniteCost());
232 child.lock()->resetReverseParent();
233 auto childsAccumulatedChildren = child.lock()->invalidateReverseBranch();
234 accumulatedChildren.insert(accumulatedChildren.end(), childsAccumulatedChildren.begin(),
235 childsAccumulatedChildren.end());
237 reverseChildren_.clear();
239 return accumulatedChildren;
242 std::vector<std::weak_ptr<aitstar::Vertex>> Vertex::invalidateForwardBranch()
244 std::vector<std::weak_ptr<aitstar::Vertex>> accumulatedChildren = forwardChildren_;
247 for (
const auto &child : forwardChildren_)
249 child.lock()->setCostToComeFromGoal(objective_->infiniteCost());
250 child.lock()->resetForwardParent();
251 auto childsAccumulatedChildren = child.lock()->invalidateForwardBranch();
252 accumulatedChildren.insert(accumulatedChildren.end(), childsAccumulatedChildren.begin(),
253 childsAccumulatedChildren.end());
255 forwardChildren_.clear();
257 return accumulatedChildren;
260 void Vertex::setForwardParent(
const std::shared_ptr<Vertex> &vertex,
const ompl::base::Cost &edgeCost)
263 if (
static_cast<bool>(forwardParent_.lock()))
265 forwardParent_.lock()->removeFromForwardChildren(vertexId_);
269 edgeCostFromForwardParent_ = edgeCost;
272 forwardParent_ = std::weak_ptr<Vertex>(vertex);
275 costToComeFromStart_ = objective_->combineCosts(vertex->getCostToComeFromStart(), edgeCost);
305 void Vertex::removeFromForwardChildren(std::size_t vertexId)
308 auto it = std::find_if(
309 forwardChildren_.begin(), forwardChildren_.end(),
310 [vertexId](
const std::weak_ptr<Vertex> &child) { return vertexId == child.lock()->getId(); });
313 if (it == forwardChildren_.end())
315 auto msg =
"Asked to remove vertex from forward children that is currently not a child."s;
320 std::iter_swap(it, forwardChildren_.rbegin());
321 forwardChildren_.pop_back();
329 void Vertex::removeFromReverseChildren(std::size_t vertexId)
332 auto it = std::find_if(
333 reverseChildren_.begin(), reverseChildren_.end(),
334 [vertexId](
const std::weak_ptr<Vertex> &child) { return vertexId == child.lock()->getId(); });
337 if (it == reverseChildren_.end())
339 auto msg =
"Asked to remove vertex from reverse children that is currently not a child."s;
344 std::iter_swap(it, reverseChildren_.rbegin());
345 reverseChildren_.pop_back();
353 bool Vertex::isWhitelistedAsChild(
const std::shared_ptr<Vertex> &vertex)
const
358 auto it = whitelistedChildren_.begin();
359 while (it != whitelistedChildren_.end())
362 if (
const auto child = it->lock())
365 if (child->getId() == vertex->getId())
373 it = whitelistedChildren_.erase(it);
384 bool Vertex::isBlacklistedAsChild(
const std::shared_ptr<Vertex> &vertex)
const
386 auto it = blacklistedChildren_.begin();
387 while (it != blacklistedChildren_.end())
390 if (
const auto child = it->lock())
393 if (child->getId() == vertex->getId())
401 it = blacklistedChildren_.erase(it);
436 std::vector<std::shared_ptr<Vertex>> Vertex::getForwardChildren()
const
438 std::vector<std::shared_ptr<Vertex>> children;
439 for (
const auto &child : forwardChildren_)
441 assert(!child.expired());
442 children.emplace_back(child.lock());
447 std::vector<std::shared_ptr<Vertex>> Vertex::getReverseChildren()
const
449 std::vector<std::shared_ptr<Vertex>> children;
450 children.reserve(reverseChildren_.size());
451 for (
const auto &child : reverseChildren_)
453 assert(!child.expired());
454 children.emplace_back(child.lock());
465 void Vertex::setReverseQueuePointer(
467 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>>,
468 std::function<
bool(
const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> &,
469 const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> &)>>::
472 reverseQueuePointerId_ = batchId_;
473 reverseQueuePointer_ = pointer;
477 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>>,
478 std::function<bool(
const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> &,
549 void Vertex::callOnForwardBranch(
const std::function<
void(
const std::shared_ptr<Vertex> &)> &function)
552 function(shared_from_this());
555 for (
auto &child : forwardChildren_)
557 child.lock()->callOnForwardBranch(function);
561 void Vertex::callOnReverseBranch(
const std::function<
void(
const std::shared_ptr<Vertex> &)> &function)
564 function(shared_from_this());
567 for (
auto &child : reverseChildren_)
569 child.lock()->callOnReverseBranch(function);