Vertex.cpp
56 // Debug setting. The id number of a vertex to track. Requires BITSTAR_DEBUG to be defined in BITstar.h
107 BITstar::Vertex::Vertex(ompl::base::SpaceInformationPtr spaceInformation, const CostHelper *const costHelpPtr, SearchQueue *const queuePtr,
190 throw ompl::Exception("Attempting to get the depth of a vertex that does not have a parent yet is not "
249 throw ompl::Exception("Attempting to add a parent to the root vertex, which cannot have a parent.");
276 throw ompl::Exception("Attempting to remove the parent of the root vertex, which cannot have a "
281 throw ompl::Exception("Attempting to remove the parent of a vertex that does not have a parent.");
311 throw ompl::Exception("A (weak) pointer to a child was found to have expired while collecting the "
333 throw ompl::Exception("A (weak) pointer to a child was found to have expired while collecting the "
395 // Iterate over the vector of children pointers until the child is found. Iterators make erase easier
427 throw ompl::Exception("Attempting to remove a child vertex not present in the vector of children "
477 throw ompl::Exception("Attempting to access the incoming-edge cost of a vertex without a parent.");
562 throw ompl::Exception("Attempted to add a second edge to the queue from a single source vertex.");
576 // Assert that the edge queue entries we have are of the same set as the one we're seeking to delete.
577 // If so, there's no point clearing them, as then we'd be trying to remove an edge that doesn't exist which would be an error.
580 throw ompl::Exception("Attempted to remove an incoming queue edge added under a different expansion id.");
611 void BITstar::Vertex::removeFromEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtrVector::const_iterator& element)
616 // Assert that the edge queue entries we have are of the same set as the one we're seeking to delete.
617 // If so, there's no point clearing them, as then we'd be trying to remove an edge that doesn't exist which would be an error.
620 throw ompl::Exception("Attempted to remove an incoming queue edge added under a different expansion id.");
636 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::edgeQueueInLookupConstBegin()
646 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::edgeQueueInLookupConstEnd()
689 throw ompl::Exception("Attempted to add a second edge to the queue to a single target vertex.");
698 void BITstar::Vertex::removeFromEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtr &element)
703 // Assert that the edge queue entries we have are of the same set as the one we're seeking to delete.
704 // If so, there's no point clearing them, as then we'd be trying to remove an edge that doesn't exist which would be an error.
707 throw ompl::Exception("Attempted to remove an incoming queue edge added under a different expansion id.");
738 void BITstar::Vertex::removeFromEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtrVector::const_iterator& element)
743 // Assert that the edge queue entries we have are of the same set as the one we're seeking to delete.
744 // If so, there's no point clearing them, as then we'd be trying to remove an edge that doesn't exist which would be an error.
747 throw ompl::Exception("Attempted to remove an outgoing queue edge added under a different expansion id.");
763 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::edgeQueueOutLookupConstBegin()
773 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::edgeQueueOutLookupConstEnd()
816 throw ompl::Exception("Attempting to update descendants' costs and depths of a vertex that does "
860 void BITstar::Vertex::removeFromEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtrVector::iterator &element)
881 throw ompl::Exception("Attempted to remove an incoming queue edge from a vertex with an empty list.");
914 void BITstar::Vertex::removeFromEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtrVector::iterator &element)
933 throw ompl::Exception("Attempted to remove an outgoing queue edge from a vertex with an empty list.");
VertexConstPtr getParent() const
Get a const pointer to the parent of this vertex.
Definition: Vertex.cpp:198
void clearEdgeQueueOutLookup()
Clear the pointers to all of the outgoing edge queue entries.
Definition: Vertex.cpp:756
A shared pointer wrapper for ompl::base::SpaceInformation.
std::shared_ptr< const Vertex > VertexConstPtr
A shared pointer to a const vertex.
Definition: BITstar.h:119
A queue of edges, sorted according to a sort key.
Definition: SearchQueue.h:65
bool isExpandedOnCurrentApproximation() const
Returns whether the vertex is expanded on current approximation.
Definition: Vertex.cpp:494
void blacklistChild(const VertexConstPtr &vertex)
Put the vertex on the blacklist of children.
Definition: Vertex.cpp:433
bool isRoot() const
Returns whether the vertex is the root of the search tree.
Definition: Vertex.cpp:162
bool isInTree() const
Get whether a vertex is in the search tree or a sample (i.e., a vertex of the RRG).
Definition: Vertex.cpp:176
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
ompl::base::Cost getCost() const
Get the cost-to-come of a vertex. Return infinity if the edge is disconnected.
Definition: Vertex.cpp:463
bool isBlacklistedAsChild(const VertexConstPtr &vertex) const
Returns true if the vertex is blacklisted as a child of this vertex.
Definition: Vertex.cpp:443
EdgeQueue::Element * EdgeQueueElemPtr
An element pointer into the edge queue binary heap.
Definition: SearchQueue.h:84
bool isExpandedOnCurrentSearch() const
Returns whether the vertex is expaned on current search.
Definition: Vertex.cpp:499
A helper class to handle the various heuristic functions in one place.
Definition: CostHelper.h:70
bool hasEverBeenExpandedAsRewiring() const
Returns whether the vertex has ever been expanded as a rewiring.
Definition: Vertex.cpp:504
void addChild(const VertexPtr &newChild)
Add a child to this vertex. Does not change this vertex's cost or those of its descendants....
Definition: Vertex.cpp:343
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueInLookupConstEnd()
Get an iterator to the end of the incoming edge queue entry vector. Will clear existing in/out lookup...
Definition: Vertex.cpp:646
An ID generator class for vertex IDs.
Definition: IdGenerator.h:59
void whitelistChild(const VertexConstPtr &vertex)
Put the vertex on the whitelist of children.
Definition: Vertex.cpp:438
void insertInEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtr &inEdge)
Add to the list of the edge queue entries that point in to this vertex. Will clear existing in/out lo...
Definition: Vertex.cpp:539
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared pointers to const vertices.
Definition: BITstar.h:128
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers to vertices.
Definition: BITstar.h:125
void clearEdgeQueueInLookup()
Clear the pointers to all of the incoming edge queue entries.
Definition: Vertex.cpp:629
bool isWhitelistedAsChild(const VertexConstPtr &vertex) const
Returns true if the vertex is blacklisted as a child of this vertex.
Definition: Vertex.cpp:448
void getChildren(VertexConstPtrVector *children) const
Get the children of a vertex as constant pointers.
Definition: Vertex.cpp:299
void removeChild(const VertexPtr &oldChild)
Remove a child from this vertex. Does not change this vertex's cost or those of its descendants....
Definition: Vertex.cpp:370
Vertex(ompl::base::SpaceInformationPtr spaceInformation, const CostHelper *const costHelpPtr, SearchQueue *const queuePtr, const std::shared_ptr< const unsigned int > &approximationId, bool root=false)
Construct a vertex using space information, and helpers to compute various costs.
Definition: Vertex.cpp:107
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueOutLookupConstBegin()
Get an iterator to the front of the outgoing edge queue entry vector. Will clear existing in/out look...
Definition: Vertex.cpp:763
void removeParent(bool updateChildCosts)
Remove the parent of this vertex. Will always update this vertex's cost, and can update the descenden...
Definition: Vertex.cpp:267
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueInLookupConstBegin()
Get an iterator to the front of the incoming edge queue entry vector. Will clear existing in/out look...
Definition: Vertex.cpp:636
unsigned int edgeQueueInLookupSize()
Get the number of edge queue entries incoming to this vertex. Will clear existing in/out lookups if t...
Definition: Vertex.cpp:656
void removeFromEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtr &inEdge)
Remove an incoming edge queue entry by value to the member vector.
Definition: Vertex.cpp:571
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueOutLookupConstEnd()
Get an iterator to the end of the outgoing edge queue entry vector. Will clear existing in/out lookup...
Definition: Vertex.cpp:773
void addParent(const VertexPtr &newParent, const ompl::base::Cost &edgeInCost)
Set the parent of this vertex, cannot be used to replace a previous parent. Will always update this v...
Definition: Vertex.cpp:240
void removeFromEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtr &outEdge)
Remove an outgoing edge queue entry by value.
Definition: Vertex.cpp:698
ompl::base::Cost getEdgeInCost() const
Get the incremental cost-to-come of a vertex.
Definition: Vertex.cpp:470
unsigned int edgeQueueOutLookupSize()
Get the number of edge queue entries outgoing from this vertex. Will clear existing in/out lookups if...
Definition: Vertex.cpp:783
void insertInEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtr &outEdge)
Add to the list of the edge queue entries that point out of this vertex. Will clear existing in/out l...
Definition: Vertex.cpp:666