38#include "ompl/geometric/planners/informedtrees/bitstar/Vertex.h"
44#include "ompl/util/Exception.h"
48#include "ompl/geometric/planners/informedtrees/bitstar/HelperFunctions.h"
50#include "ompl/geometric/planners/informedtrees/bitstar/IdGenerator.h"
52#include "ompl/geometric/planners/informedtrees/bitstar/CostHelper.h"
57 #define TRACK_VERTEX_ID 0
60 #define PRINT_VERTEX_CHANGE \
61 if (id_ == TRACK_VERTEX_ID) \
63 std::cout << "Vertex " << id_ << ": " << __func__ << "" << std::endl; \
67 #define ASSERT_NOT_PRUNED \
68 if (this->isPruned_) \
70 std::cout << "Vertex " << id_ << ": " << __func__ << std::endl; \
71 throw ompl::Exception("Attempting to access a pruned vertex."); \
74 #define PRINT_VERTEX_CHANGE
75 #define ASSERT_NOT_PRUNED
83 std::once_flag g_IdInited;
85 boost::scoped_ptr<ompl::geometric::BITstar::IdGenerator> g_IdGenerator;
88 void initIdGenerator()
96 std::call_once(g_IdInited, &initIdGenerator);
97 return *g_IdGenerator;
108 const std::shared_ptr<const unsigned int> &approximationId,
bool root)
109 : id_(getIdGenerator().getNewId())
110 , si_(
std::move(spaceInformation))
111 , costHelpPtr_(
std::move(costHelpPtr))
112 , queuePtr_(queuePtr)
113 , state_(si_->allocState())
115 , edgeCost_(costHelpPtr_->infiniteCost())
116 , cost_(costHelpPtr_->infiniteCost())
117 , costAtExpansion_(costHelpPtr_->infiniteCost())
118 , currentSearchId_(queuePtr->getSearchId())
119 , currentApproximationId_(approximationId)
125 cost_ = costHelpPtr_->identityCost();
130 BITstar::Vertex::~Vertex()
135 si_->freeState(state_);
162 bool BITstar::Vertex::isRoot()
const
169 bool BITstar::Vertex::hasParent()
const
173 return static_cast<bool>(parentPtr_);
176 bool BITstar::Vertex::isInTree()
const
180 return this->isRoot() || this->hasParent();
183 unsigned int BITstar::Vertex::getDepth()
const
188 if (!this->isRoot() && !this->hasParent())
190 throw ompl::Exception(
"Attempting to get the depth of a vertex that does not have a parent yet is not "
203 if (!this->hasParent())
207 throw ompl::Exception(
"Attempting to access the parent of the root vertex.");
211 throw ompl::Exception(
"Attempting to access the parent of a vertex that does not have one.");
224 if (!this->hasParent())
228 throw ompl::Exception(
"Attempting to access the parent of the root vertex.");
232 throw ompl::Exception(
"Attempting to access the parent of a vertex that does not have one.");
249 throw ompl::Exception(
"Attempting to add a parent to the root vertex, which cannot have a parent.");
251 if (this->hasParent())
253 throw ompl::Exception(
"Attempting to add a parent to a vertex that already has one.");
258 parentPtr_ = newParent;
261 edgeCost_ = edgeInCost;
264 this->updateCostAndDepth(
true);
267 void BITstar::Vertex::removeParent(
bool updateChildCosts)
276 throw ompl::Exception(
"Attempting to remove the parent of the root vertex, which cannot have a "
279 if (!this->hasParent())
281 throw ompl::Exception(
"Attempting to remove the parent of a vertex that does not have a parent.");
289 this->updateCostAndDepth(updateChildCosts);
292 bool BITstar::Vertex::hasChildren()
const
296 return !children_.empty();
305 for (
const auto &child : children_)
311 throw ompl::Exception(
"A (weak) pointer to a child was found to have expired while collecting the "
312 "children of a vertex.");
317 children->push_back(child.lock());
327 for (
const auto &child : children_)
333 throw ompl::Exception(
"A (weak) pointer to a child was found to have expired while collecting the "
334 "children of a vertex.");
339 children->push_back(child.lock());
356 throw ompl::Exception(
"Attempted to add child that does not have a listed parent.");
360 throw ompl::Exception(
"Attempted to add someone else's child as mine.");
365 children_.push_back(child);
370 void BITstar::Vertex::removeChild(
const VertexPtr &child)
379 throw ompl::Exception(
"Attempted to remove a root vertex as a child.");
383 throw ompl::Exception(
"Attempted to remove a child that does not have a listed parent.");
387 throw ompl::Exception(
"Attempted to remove a child vertex from the wrong parent.");
397 for (
auto it = children_.begin(); it != children_.end(); ++it)
403 throw ompl::Exception(
"A (weak) pointer to a child was found to have expired while removing a "
404 "child from a vertex.");
409 if (it->lock()->getId() == child->
getId())
420 swapPopBack(it, &children_);
430 throw ompl::Exception(
"Attempting to remove a child vertex not present in the vector of children "
431 "stored in the (supposed) parent vertex.");
438 childIdBlacklist_.emplace(vertex->
getId());
443 childIdWhitelist_.emplace(vertex->
getId());
448 return childIdBlacklist_.find(vertex->
getId()) != childIdBlacklist_.end();
453 return childIdWhitelist_.find(vertex->
getId()) != childIdWhitelist_.end();
456 void BITstar::Vertex::clearBlacklist()
458 childIdBlacklist_.clear();
461 void BITstar::Vertex::clearWhitelist()
463 childIdWhitelist_.clear();
478 if (!this->hasParent())
480 throw ompl::Exception(
"Attempting to access the incoming-edge cost of a vertex without a parent.");
487 bool BITstar::Vertex::isConsistent()
const
489 return cost_.value() == costAtExpansion_.value();
492 bool BITstar::Vertex::isPruned()
const
497 bool BITstar::Vertex::isExpandedOnCurrentApproximation()
const
499 return expansionApproximationId_ == *currentApproximationId_;
502 bool BITstar::Vertex::isExpandedOnCurrentSearch()
const
504 return expansionSearchId_ == *currentSearchId_;
507 bool BITstar::Vertex::hasEverBeenExpandedAsRewiring()
const
509 return hasEverBeenExpandedAsRewiring_;
512 void BITstar::Vertex::registerExpansion()
515 costAtExpansion_ = cost_;
518 expansionApproximationId_ = *currentApproximationId_;
519 expansionSearchId_ = *currentSearchId_;
522 void BITstar::Vertex::registerRewiringExpansion()
524 hasEverBeenExpandedAsRewiring_ =
true;
527 void BITstar::Vertex::markPruned()
535 void BITstar::Vertex::markUnpruned()
547 this->clearLookupsIfOutdated();
551 if (element->data.second.first->getId() == id_)
553 throw ompl::Exception(
"Attempted to add a cyclic incoming queue edge.");
556 if (element->data.second.second->getId() != id_)
558 throw ompl::Exception(
"Attempted to add an incoming queue edge to the wrong vertex.");
561 for (
const auto &inEdge : edgeQueueInLookup_)
563 if (element->data.second.first->getId() == inEdge->data.second.first->getId())
565 throw ompl::Exception(
"Attempted to add a second edge to the queue from a single source vertex.");
571 edgeQueueInLookup_.push_back(element);
581 if (*currentApproximationId_ != lookupApproximationId_)
583 throw ompl::Exception(
"Attempted to remove an incoming queue edge added under a different expansion id.");
592 for (
auto it = edgeQueueInLookup_.begin(); it != edgeQueueInLookup_.end(); ++it)
595 if ((*it)->data.second.first->getId() == element->data.second.first->getId())
598 this->removeFromEdgeQueueInLookup(it);
612 throw ompl::Exception(
"Attempted to remove an edge not in the incoming lookup.");
617 void BITstar::Vertex::removeFromEdgeQueueInLookup(
const SearchQueue::EdgeQueueElemPtrVector::const_iterator& element)
624 if (*currentApproximationId_ != lookupApproximationId_)
626 throw ompl::Exception(
"Attempted to remove an incoming queue edge added under a different expansion id.");
632 this->removeFromEdgeQueueInLookup(edgeQueueInLookup_.erase(element, element));
635 void BITstar::Vertex::clearEdgeQueueInLookup()
639 edgeQueueInLookup_.clear();
642 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::edgeQueueInLookupConstBegin()
647 this->clearLookupsIfOutdated();
649 return edgeQueueInLookup_.cbegin();
652 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::edgeQueueInLookupConstEnd()
657 this->clearLookupsIfOutdated();
659 return edgeQueueInLookup_.cend();
662 unsigned int BITstar::Vertex::edgeQueueInLookupSize()
667 this->clearLookupsIfOutdated();
669 return edgeQueueInLookup_.size();
677 this->clearLookupsIfOutdated();
681 if (element->data.second.first->getId() != id_)
683 throw ompl::Exception(
"Attempted to add an outgoing queue edge to the wrong vertex.");
686 if (element->data.second.second->getId() == id_)
688 throw ompl::Exception(
"Attempted to add a cyclic outgoing queue edge.");
691 for (
const auto &outEdge : edgeQueueOutLookup_)
693 if (element->data.second.second->getId() == outEdge->data.second.second->getId())
695 throw ompl::Exception(
"Attempted to add a second edge to the queue to a single target vertex.");
701 edgeQueueOutLookup_.push_back(element);
711 if (*currentApproximationId_ != lookupApproximationId_)
713 throw ompl::Exception(
"Attempted to remove an incoming queue edge added under a different expansion id.");
722 for (
auto it = edgeQueueOutLookup_.begin(); it != edgeQueueOutLookup_.end(); ++it)
725 if ((*it)->data.second.second->getId() == element->data.second.second->getId())
728 this->removeFromEdgeQueueOutLookup(it);
742 throw ompl::Exception(
"Attempted to remove an edge not in the outgoing lookup.");
747 void BITstar::Vertex::removeFromEdgeQueueOutLookup(
const SearchQueue::EdgeQueueElemPtrVector::const_iterator& element)
754 if (*currentApproximationId_ != lookupApproximationId_)
756 throw ompl::Exception(
"Attempted to remove an outgoing queue edge added under a different expansion id.");
762 this->removeFromEdgeQueueOutLookup(edgeQueueOutLookup_.erase(element, element));
765 void BITstar::Vertex::clearEdgeQueueOutLookup()
769 edgeQueueOutLookup_.clear();
772 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::edgeQueueOutLookupConstBegin()
777 this->clearLookupsIfOutdated();
779 return edgeQueueOutLookup_.cbegin();
782 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::edgeQueueOutLookupConstEnd()
787 this->clearLookupsIfOutdated();
789 return edgeQueueOutLookup_.cend();
792 unsigned int BITstar::Vertex::edgeQueueOutLookupSize()
797 this->clearLookupsIfOutdated();
799 return edgeQueueOutLookup_.size();
802 void BITstar::Vertex::updateCostAndDepth(
bool cascadeUpdates )
810 cost_ = costHelpPtr_->identityCost();
813 else if (!this->hasParent())
816 cost_ = costHelpPtr_->infiniteCost();
823 if (this->hasChildren() && cascadeUpdates)
825 throw ompl::Exception(
"Attempting to update descendants' costs and depths of a vertex that does "
826 "not have a parent and is not root. This information would therefore be "
834 cost_ = costHelpPtr_->combineCosts(parentPtr_->getCost(), edgeCost_);
837 for (
const auto& edge : edgeQueueOutLookup_) {
838 if (lookupApproximationId_ == *currentApproximationId_) {
839 queuePtr_->update(edge);
844 depth_ = (parentPtr_->getDepth() + 1u);
851 for (
auto &child : children_)
857 throw ompl::Exception(
"A (weak) pointer to a child has was found to have expired while "
858 "updating the costs and depths of descendant vertices.");
863 child.lock()->updateCostAndDepth(
true);
869 void BITstar::Vertex::removeFromEdgeQueueInLookup(
const SearchQueue::EdgeQueueElemPtrVector::iterator &element)
873 VertexId parentId = (*element)->data.second.first->getId();
878 throw ompl::Exception(
"Attempted to remove a cyclic incoming queue edge.");
882 if ((*element)->data.second.second->getId() != id_)
884 throw ompl::Exception(
"Attempted to remove an incoming queue edge from the wrong vertex.");
888 if (edgeQueueInLookup_.empty())
890 throw ompl::Exception(
"Attempted to remove an incoming queue edge from a vertex with an empty list.");
895 for (
auto it = edgeQueueInLookup_.begin(); it != edgeQueueInLookup_.end() && !found; ++it)
897 found = ((*it)->data.second.first->getId() == parentId);
901 throw ompl::Exception(
"Attempted to remove an edge not in the incoming lookup.");
909 swapPopBack(element, &edgeQueueInLookup_);
913 for (
const auto &edgePtr : edgeQueueInLookup_)
915 if (edgePtr->data.second.first->getId() == parentId)
917 throw ompl::Exception(
"Failed to remove the designated edge in the incoming lookup.");
923 void BITstar::Vertex::removeFromEdgeQueueOutLookup(
const SearchQueue::EdgeQueueElemPtrVector::iterator &element)
927 VertexId childId = (*element)->data.second.second->getId();
930 if ((*element)->data.second.first->getId() != id_)
932 throw ompl::Exception(
"Attempted to remove an outgoing queue edge from the wrong vertex.");
937 throw ompl::Exception(
"Attempted to remove a cyclic outgoing queue edge.");
940 if (edgeQueueOutLookup_.empty())
942 throw ompl::Exception(
"Attempted to remove an outgoing queue edge from a vertex with an empty list.");
946 for (
auto it = edgeQueueOutLookup_.begin(); it != edgeQueueOutLookup_.end() && !found; ++it)
948 found = ((*it)->data.second.second->getId() == childId);
952 throw ompl::Exception(
"Attempted to remove an edge not in the outgoing lookup.");
960 swapPopBack(element, &edgeQueueOutLookup_);
964 for (
const auto &edgePtr : edgeQueueOutLookup_)
966 if (edgePtr->data.second.second->getId() == childId)
968 throw ompl::Exception(
"Failed to remove the designated edge in the outgoing lookup.");
974 void BITstar::Vertex::clearLookupsIfOutdated()
977 if (lookupApproximationId_ != *currentApproximationId_)
980 this->clearEdgeQueueInLookup();
981 this->clearEdgeQueueOutLookup();
984 lookupApproximationId_ = *currentApproximationId_;
The exception type for ompl.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
SpaceInformationPtr si_
The space information for which planning is done.
Definition of an abstract state.
A helper class to handle the various heuristic functions in one place.
An ID generator class for vertex IDs.
A queue of edges, sorted according to a sort key.
EdgeQueue::Element * EdgeQueueElemPtr
An element pointer into the edge queue binary heap.
bool hasParent() const
Returns whether this vertex has a parent.
VertexConstPtr getParent() const
Get a const pointer to the parent of this vertex.
bool isRoot() const
Returns whether the vertex is the root of the search tree.
BITstar::VertexId getId() const
The (unique) vertex ID.
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared pointers to const vertices.
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers to vertices.
std::shared_ptr< const Vertex > VertexConstPtr
A shared pointer to a const vertex.
std::shared_ptr< Vertex > VertexPtr
A shared pointer to a vertex.
unsigned int VertexId
The vertex id type.
Main namespace. Contains everything in this library.