Vertex.cpp
56 // Debug setting. The id number of a vertex to track. Requires BITSTAR_DEBUG to be defined in BITstar.h
158 throw ompl::Exception("Attempting to get the depth of a vertex that does not have a parent yet is not "
218 throw ompl::Exception("Attempting to add a parent to the root vertex, which cannot have a parent.");
245 throw ompl::Exception("Attempting to remove the parent of the root vertex, which cannot have a "
250 throw ompl::Exception("Attempting to remove the parent of a vertex that does not have a parent.");
280 throw ompl::Exception("A (weak) pointer to a child was found to have expired while collecting the "
302 throw ompl::Exception("A (weak) pointer to a child was found to have expired while collecting the "
364 // Iterate over the vector of children pointers until the child is found. Iterators make erase easier
399 throw ompl::Exception("Attempting to remove a child vertex not present in the vector of children "
419 throw ompl::Exception("Attempting to access the incoming-edge cost of a vertex without a parent.");
528 throw ompl::Exception("Attempting to access an iterator to the vertex queue before one is set.");
572 void BITstar::Vertex::addIncomingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr& newInPtr, unsigned int vertexQueueResetNum)
595 throw ompl::Exception("Attempted to add a second edge to the queue from a single source vertex.");
604 void BITstar::Vertex::rmIncomingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr &elemToDelete, unsigned int vertexQueueResetNum)
609 // Assert that the edge queue entries we have are of the same set as the one we're seeking to delete.
610 // 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.
613 throw ompl::Exception("Attempted to remove an incoming queue edge added under a different expansion id.");
622 for (auto iterToDelete = edgeQueueInPtrs_.begin(); iterToDelete != edgeQueueInPtrs_.end() && !found; ++iterToDelete)
644 void BITstar::Vertex::rmIncomingEdgeQueuePtrByIter(const SearchQueue::EdgeQueueElemPtrVector::const_iterator& constIterToDelete, unsigned int vertexQueueResetNum)
649 // Assert that the edge queue entries we have are of the same set as the one we're seeking to delete.
650 // 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.
653 throw ompl::Exception("Attempted to remove an incoming queue edge added under a different expansion id.");
669 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::incomingEdgeQueuePtrsBeginConst(unsigned int vertexQueueResetNum)
679 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::incomingEdgeQueuePtrsEndConst(unsigned int vertexQueueResetNum)
713 void BITstar::Vertex::addOutgoingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr& newOutPtr, unsigned int vertexQueueResetNum)
736 throw ompl::Exception("Attempted to add a second edge to the queue to a single target vertex.");
745 void BITstar::Vertex::rmOutgoingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr &elemToDelete, unsigned int vertexQueueResetNum)
750 // Assert that the edge queue entries we have are of the same set as the one we're seeking to delete.
751 // 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.
754 throw ompl::Exception("Attempted to remove an incoming queue edge added under a different expansion id.");
763 for (auto iterToDelete = edgeQueueOutPtrs_.begin(); iterToDelete != edgeQueueOutPtrs_.end() && !found; ++iterToDelete)
785 void BITstar::Vertex::rmOutgoingEdgeQueuePtrByIter(const SearchQueue::EdgeQueueElemPtrVector::const_iterator& constIterToDelete, unsigned int vertexQueueResetNum)
790 // Assert that the edge queue entries we have are of the same set as the one we're seeking to delete.
791 // 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.
794 throw ompl::Exception("Attempted to remove an outgoing queue edge added under a different expansion id.");
810 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::outgoingEdgeQueuePtrsBeginConst(unsigned int vertexQueueResetNum)
820 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::outgoingEdgeQueuePtrsEndConst(unsigned int vertexQueueResetNum)
879 throw ompl::Exception("Attempting to update descendants' costs and depths of a vertex that does "
920 void BITstar::Vertex::rmIncomingHelper(const SearchQueue::EdgeQueueElemPtrVector::iterator &iterToDelete)
938 throw ompl::Exception("Attempted to remove an incoming queue edge from a vertex with an empty list.");
942 for (SearchQueue::EdgeQueueElemPtrVector::iterator ptrIter = edgeQueueInPtrs_.begin(); ptrIter != edgeQueueInPtrs_.end() && !found; ++ptrIter)
970 void BITstar::Vertex::rmOutgoingHelper(const SearchQueue::EdgeQueueElemPtrVector::iterator &iterToDelete)
988 throw ompl::Exception("Attempted to remove an outgoing queue edge from a vertex with an empty list.");
void addIncomingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr &newInPtr, unsigned int vertexQueueResetNum)
Add to the list of the edge queue entries that point in to this vertex. Will clear existing in/out lo...
Definition: Vertex.cpp:572
void addParent(const VertexPtr &newParent, const ompl::base::Cost &edgeInCost, bool updateChildCosts)
Set the parent of this vertex, cannot be used to replace a previous parent. Will always update this v...
Definition: Vertex.cpp:208
A shared pointer wrapper for ompl::base::SpaceInformation.
void clearIncomingEdgeQueuePtrs()
Clear the pointers to all of the incoming edge queue entries.
Definition: Vertex.cpp:662
bool hasBeenExpandedToVertices() const
Returns true if the vertex has been expanded towards vertices.
Definition: Vertex.cpp:472
bool isNew() const
Returns true if the vertex is marked as new. Vertices are new until marked old.
Definition: Vertex.cpp:426
void rmIncomingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr &elemToDelete, unsigned int vertexQueueResetNum)
Remove an incoming edge queue entry by value to the member vector.
Definition: Vertex.cpp:604
bool hasIncomingEdgeQueueEntries(unsigned int vertexQueueResetNum)
Return true if the vertex has links to incoming entries in the Edge Queue. Will clear existing in/out...
Definition: Vertex.cpp:699
const ompl::base::State * stateConst() const
The state of a vertex as a constant pointer.
Definition: Vertex.cpp:113
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator incomingEdgeQueuePtrsEndConst(unsigned int vertexQueueResetNum)
Get an iterator to the end of the incoming edge queue entry vector. Will clear existing in/out lookup...
Definition: Vertex.cpp:679
bool isInTree() const
Get whether a vertex is "in the graph" or not. This returns true if the vertex is the graph root or i...
Definition: Vertex.cpp:144
unsigned int getDepth() const
Get the "depth" of the vertex from the root. A root vertex is at depth 0, a direct descendent of the ...
Definition: Vertex.cpp:151
void updateCostAndDepth(bool cascadeUpdates=true)
Calculates the updated cost and depth of the current state, as well as calling all children's updateC...
Definition: Vertex.cpp:856
void getChildrenConst(VertexConstPtrVector *children) const
Get the children of a vertex as constant pointers.
Definition: Vertex.cpp:268
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared const pointers.
Definition: BITstar.h:228
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator outgoingEdgeQueuePtrsEndConst(unsigned int vertexQueueResetNum)
Get an iterator to the end of the outgoing edge queue entry vector. Will clear existing in/out lookup...
Definition: Vertex.cpp:820
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator outgoingEdgeQueuePtrsBeginConst(unsigned int vertexQueueResetNum)
Get an iterator to the front of the outgoing edge queue entry vector. Will clear existing in/out look...
Definition: Vertex.cpp:810
ompl::base::Cost getCost() const
Get the cost-to-come of a vertex. Return infinity if the edge is disconnected.
Definition: Vertex.cpp:405
SearchQueue::VertexQueueIter getVertexQueueIter() const
Get an iterator to this vertex in the vertex queue.
Definition: Vertex.cpp:520
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator incomingEdgeQueuePtrsBeginConst(unsigned int vertexQueueResetNum)
Get an iterator to the front of the incoming edge queue entry vector. Will clear existing in/out look...
Definition: Vertex.cpp:669
void clearVertexQueueIter()
Clear the iterator to this vertex in the vertex queue.
Definition: Vertex.cpp:554
EdgeQueueAsPairBinHeap::Element * EdgeQueueElemPtr
An element pointer into the edge queue binary heap.
Definition: SearchQueue.h:216
A helper class to handle the various heuristic functions in one place.
Definition: CostHelper.h:133
bool hasBeenExpandedToSamples() const
Returns true if the vertex has been expanded towards samples.
Definition: Vertex.cpp:449
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:312
void markUnexpandedToVertices()
Mark the vertex as not expanded towards vertices.
Definition: Vertex.cpp:487
void rmIncomingEdgeQueuePtrByIter(const SearchQueue::EdgeQueueElemPtrVector::const_iterator &constIterToDelete, unsigned int vertexQueueResetNum)
Remove an incoming edge queue entry by iterator to the member vector.
Definition: Vertex.cpp:644
void clearOutgoingEdgeQueuePtrs()
Clear the pointers to all of the outgoing edge queue entries.
Definition: Vertex.cpp:803
unsigned int getNumIncomingEdgeQueuePtrs(unsigned int vertexQueueResetNum)
Get the number of edge queue entries incoming to this vertex. Will clear existing in/out lookups if t...
Definition: Vertex.cpp:689
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:339
void rmOutgoingEdgeQueuePtrByIter(const SearchQueue::EdgeQueueElemPtrVector::const_iterator &constIterToDelete, unsigned int vertexQueueResetNum)
Remove an outgoing edge queue entry by iterator to the member vector.
Definition: Vertex.cpp:785
void rmOutgoingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr &elemToDelete, unsigned int vertexQueueResetNum)
Remove an outgoing edge queue entry by value.
Definition: Vertex.cpp:745
bool hasVertexQueueEntry() const
Return true if the vertex has a link to it's entry in the Vertex Queue.
Definition: Vertex.cpp:562
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:236
void markUnexpandedToSamples()
Mark the vertex as not expanded towards samples.
Definition: Vertex.cpp:464
bool hasOutgoingEdgeQueueEntries(unsigned int vertexQueueResetNum)
Return true if the vertex has links to outgoing entries in the Edge Queue. Will clear existing in/out...
Definition: Vertex.cpp:840
void addOutgoingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr &newOutPtr, unsigned int vertexQueueResetNum)
Add to the list of the edge queue entries that point out of this vertex. Will clear existing in/out l...
Definition: Vertex.cpp:713
void markExpandedToVertices()
Mark the vertex as expanded towards vertices.
Definition: Vertex.cpp:479
std::shared_ptr< const Vertex > VertexConstPtr
A constant vertex shared pointer.
Definition: BITstar.h:222
VertexConstPtr getParentConst() const
Get the parent of a vertex as a constant pointer.
Definition: Vertex.cpp:166
void getChildren(VertexPtrVector *children)
Get the children of a vertex as mutable pointers.
Definition: Vertex.cpp:290
Vertex(ompl::base::SpaceInformationPtr si, const CostHelper *const costHelpPtr, bool root=false)
Constructor.
Definition: Vertex.cpp:79
ompl::base::Cost getEdgeInCost() const
Get the incremental cost-to-come of a vertex.
Definition: Vertex.cpp:412
unsigned int getNumOutgoingEdgeQueuePtrs(unsigned int vertexQueueResetNum)
Get the number of edge queue entries outgoing from this vertex. Will clear existing in/out lookups if...
Definition: Vertex.cpp:830
VertexQueueAsMMap::iterator VertexQueueIter
An iterator into the vertex queue multimap.
Definition: SearchQueue.h:205
void setVertexQueueIter(const SearchQueue::VertexQueueIter &newPtr)
Set the iterator to this vertex in the vertex queue.
Definition: Vertex.cpp:535
Main namespace. Contains everything in this library.
Definition: ConstrainedSpaceInformation.h:52