36#include "ompl/geometric/planners/informedtrees/aitstar/Vertex.h"
43#include "ompl/base/goals/GoalState.h"
44#include "ompl/base/goals/GoalStates.h"
46using namespace std::string_literals;
56 std::size_t generateId()
58 static std::atomic<std::size_t>
id{0u};
65 const std::size_t &batchId)
66 : spaceInformation_(spaceInformation)
67 , problemDefinition_(problemDefinition)
68 , objective_(problemDefinition->getOptimizationObjective())
71 , state_(spaceInformation->allocState())
72 , costToComeFromStart_(objective_->infiniteCost())
73 , edgeCostFromForwardParent_(objective_->infiniteCost())
74 , costToComeFromGoal_(objective_->infiniteCost())
75 , expandedCostToComeFromGoal_(objective_->infiniteCost())
76 , costToGoToGoal_(objective_->infiniteCost())
77 , vertexId_(generateId())
85 spaceInformation_->freeState(state_);
88 std::size_t Vertex::getId()
const
110 return costToComeFromStart_;
115 if (reverseSearchBatchId_ != batchId_)
117 costToComeFromGoal_ = objective_->infiniteCost();
119 return costToComeFromGoal_;
124 if (expandedReverseSearchId_ != batchId_)
126 expandedCostToComeFromGoal_ = objective_->infiniteCost();
128 return expandedCostToComeFromGoal_;
138 return edgeCostFromForwardParent_;
141 bool Vertex::hasForwardParent()
const
146 return static_cast<bool>(forwardParent_.lock());
149 std::shared_ptr<Vertex> Vertex::getForwardParent()
const
151 return forwardParent_.lock();
154 bool Vertex::hasReverseParent()
const
156 return static_cast<bool>(reverseParent_.lock());
159 std::shared_ptr<Vertex> Vertex::getReverseParent()
const
161 return reverseParent_.lock();
166 edgeCostFromForwardParent_ = cost;
171 costToComeFromStart_ = cost;
176 reverseSearchBatchId_ = batchId_;
177 costToComeFromGoal_ = cost;
182 expandedCostToComeFromGoal_ = cost;
187 costToGoToGoal_ = cost;
190 void Vertex::updateCostOfForwardBranch()
const
195 child->setCostToComeFromStart(objective_->combineCosts(
196 costToComeFromStart_, child->getEdgeCostFromForwardParent()));
197 child->updateCostOfForwardBranch();
201 std::vector<std::weak_ptr<aitstar::Vertex>> Vertex::invalidateReverseBranch()
203 std::vector<std::weak_ptr<aitstar::Vertex>> accumulatedChildren = reverseChildren_;
206 for (
const auto &child : reverseChildren_)
208 child.lock()->setCostToComeFromGoal(objective_->infiniteCost());
209 child.lock()->setExpandedCostToComeFromGoal(objective_->infiniteCost());
210 child.lock()->resetReverseParent();
211 auto childsAccumulatedChildren = child.lock()->invalidateReverseBranch();
212 accumulatedChildren.insert(accumulatedChildren.end(), childsAccumulatedChildren.begin(),
213 childsAccumulatedChildren.end());
215 reverseChildren_.clear();
218 return accumulatedChildren;
221 std::vector<std::weak_ptr<aitstar::Vertex>> Vertex::invalidateForwardBranch()
223 std::vector<std::weak_ptr<aitstar::Vertex>> accumulatedChildren = forwardChildren_;
226 for (
const auto &child : forwardChildren_)
228 child.lock()->setCostToComeFromGoal(objective_->infiniteCost());
229 child.lock()->resetForwardParent();
230 auto childsAccumulatedChildren = child.lock()->invalidateForwardBranch();
231 accumulatedChildren.insert(accumulatedChildren.end(), childsAccumulatedChildren.begin(),
232 childsAccumulatedChildren.end());
234 forwardChildren_.clear();
236 return accumulatedChildren;
239 void Vertex::setForwardParent(
const std::shared_ptr<Vertex> &vertex,
const ompl::base::Cost &edgeCost)
242 if (
static_cast<bool>(forwardParent_.lock()))
244 forwardParent_.lock()->removeFromForwardChildren(vertexId_);
248 edgeCostFromForwardParent_ = edgeCost;
251 forwardParent_ = std::weak_ptr<Vertex>(vertex);
254 costToComeFromStart_ = objective_->combineCosts(vertex->getCostToComeFromStart(), edgeCost);
257 void Vertex::resetForwardParent()
259 forwardParent_.reset();
262 void Vertex::setReverseParent(
const std::shared_ptr<Vertex> &vertex)
265 if (
static_cast<bool>(reverseParent_.lock()))
267 reverseParent_.lock()->removeFromReverseChildren(vertexId_);
271 reverseParent_ = std::weak_ptr<Vertex>(vertex);
274 void Vertex::resetReverseParent()
276 reverseParent_.reset();
279 void Vertex::addToForwardChildren(
const std::shared_ptr<Vertex> &vertex)
281 forwardChildren_.emplace_back(vertex);
284 void Vertex::removeFromForwardChildren(std::size_t vertexId)
287 auto it = std::find_if(
288 forwardChildren_.begin(), forwardChildren_.end(),
289 [vertexId](
const std::weak_ptr<Vertex> &child) { return vertexId == child.lock()->getId(); });
292 if (it == forwardChildren_.end())
294 auto msg =
"Asked to remove vertex from forward children that is currently not a child."s;
299 std::iter_swap(it, forwardChildren_.rbegin());
300 forwardChildren_.pop_back();
303 void Vertex::addToReverseChildren(
const std::shared_ptr<Vertex> &vertex)
305 reverseChildren_.push_back(vertex);
308 void Vertex::removeFromReverseChildren(std::size_t vertexId)
311 auto it = std::find_if(
312 reverseChildren_.begin(), reverseChildren_.end(),
313 [vertexId](
const std::weak_ptr<Vertex> &child) { return vertexId == child.lock()->getId(); });
316 if (it == reverseChildren_.end())
318 auto msg =
"Asked to remove vertex from reverse children that is currently not a child."s;
323 std::iter_swap(it, reverseChildren_.rbegin());
324 reverseChildren_.pop_back();
327 void Vertex::whitelistAsChild(
const std::shared_ptr<Vertex> &vertex)
const
329 whitelistedChildren_.emplace_back(vertex);
332 bool Vertex::isWhitelistedAsChild(
const std::shared_ptr<Vertex> &vertex)
const
334 for (
const auto &whitelistedChild : whitelistedChildren_)
336 if (whitelistedChild.lock()->getId() == vertex->getId())
344 void Vertex::blacklistAsChild(
const std::shared_ptr<Vertex> &vertex)
const
346 blacklistedChildren_.emplace_back(vertex);
349 bool Vertex::isBlacklistedAsChild(
const std::shared_ptr<Vertex> &vertex)
const
351 for (
const auto &blacklistedChild : blacklistedChildren_)
353 if (blacklistedChild.lock()->getId() == vertex->getId())
361 bool Vertex::hasCachedNeighbors()
const
363 return neighborBatchId_ == batchId_;
366 void Vertex::cacheNeighbors(
const std::vector<std::shared_ptr<Vertex>> &neighbors)
const
368 neighbors_ = neighbors;
369 neighborBatchId_ = batchId_;
372 const std::vector<std::shared_ptr<Vertex>> &Vertex::getNeighbors()
const
374 if (neighborBatchId_ != batchId_)
376 throw ompl::Exception(
"Requested neighbors from vertex of outdated approximation.");
382 std::vector<std::shared_ptr<Vertex>> Vertex::getForwardChildren()
const
384 std::vector<std::shared_ptr<Vertex>> children;
385 for (
const auto &child : forwardChildren_)
387 assert(!child.expired());
388 children.emplace_back(child.lock());
393 std::vector<std::shared_ptr<Vertex>> Vertex::getReverseChildren()
const
395 std::vector<std::shared_ptr<Vertex>> children;
396 children.reserve(reverseChildren_.size());
397 for (
const auto &child : reverseChildren_)
399 assert(!child.expired());
400 children.emplace_back(child.lock());
405 void Vertex::registerPoppedOutgoingEdgeDuringForwardSearch()
407 poppedOutgoingEdgeId_ = batchId_;
410 void Vertex::registerExpansionDuringReverseSearch()
412 expandedCostToComeFromGoal_ = costToComeFromGoal_;
413 expandedReverseSearchId_ = batchId_;
416 void Vertex::unregisterExpansionDuringReverseSearch()
418 expandedReverseSearchId_ = 0u;
421 void Vertex::registerInsertionIntoQueueDuringReverseSearch()
423 insertedIntoQueueId_ = batchId_;
426 bool Vertex::hasHadOutgoingEdgePoppedDuringCurrentForwardSearch()
const
428 return poppedOutgoingEdgeId_ == batchId_;
431 bool Vertex::hasBeenExpandedDuringCurrentReverseSearch()
const
433 return expandedReverseSearchId_ == batchId_;
436 bool Vertex::hasBeenInsertedIntoQueueDuringCurrentReverseSearch()
const
438 return insertedIntoQueueId_ == batchId_;
441 void Vertex::setReverseQueuePointer(
443 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>>,
444 std::function<
bool(
const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> &,
445 const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> &)>>::
448 reverseQueuePointerId_ = batchId_;
449 reverseQueuePointer_ = pointer;
453 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>>,
454 std::function<bool(
const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> &,
455 const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> &)>>::
457 Vertex::getReverseQueuePointer()
const
459 if (batchId_ != reverseQueuePointerId_)
461 reverseQueuePointer_ =
nullptr;
463 return reverseQueuePointer_;
466 void Vertex::resetReverseQueuePointer()
468 reverseQueuePointer_ =
nullptr;
471 void Vertex::addToForwardQueueIncomingLookup(
475 forwardQueueIncomingLookup_.emplace_back(pointer);
478 void Vertex::addToForwardQueueOutgoingLookup(
482 forwardQueueOutgoingLookup_.emplace_back(pointer);
487 Vertex::getForwardQueueIncomingLookup()
const
489 return forwardQueueIncomingLookup_;
494 Vertex::getForwardQueueOutgoingLookup()
const
496 return forwardQueueOutgoingLookup_;
499 void Vertex::removeFromForwardQueueIncomingLookup(
503 forwardQueueIncomingLookup_.erase(
504 std::remove(forwardQueueIncomingLookup_.begin(), forwardQueueIncomingLookup_.end(), element));
507 void Vertex::removeFromForwardQueueOutgoingLookup(
511 forwardQueueOutgoingLookup_.erase(
512 std::remove(forwardQueueOutgoingLookup_.begin(), forwardQueueOutgoingLookup_.end(), element));
515 void Vertex::resetForwardQueueIncomingLookup()
517 forwardQueueIncomingLookup_.clear();
520 void Vertex::resetForwardQueueOutgoingLookup()
522 forwardQueueOutgoingLookup_.clear();
This class provides an implementation of an updatable min-heap. Using it is a bit cumbersome,...
The exception type for ompl.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
A shared pointer wrapper for ompl::base::ProblemDefinition.
Definition of a scoped state.
Definition of an abstract state.
ompl::base::Cost getCostToComeFromGoal() const
Returns the cost to come to this vertex from the goal.
void unregisterExpansionDuringReverseSearch()
Unregisters the expansion of this vertex during the current reverse search, needed when a reverse bra...
std::vector< std::shared_ptr< Vertex > > getForwardChildren() const
Returns this vertex's children in the forward search tree.
Main namespace. Contains everything in this library.