SearchQueue.h
65 // I am member class of the BITstar class (i.e., I am in it's namespace), so I need to include it's definition to be
66 // aware of the class BITstar. It has a forward declaration to me and the other helper classes but I will need to
116 typedef std::function<bool(const CostTripleAndVertexPtrPair &, const CostTripleAndVertexPtrPair &)> EdgeQueueSortFunc;
331 void enqueueSamples(const VertexPtr &vertex, const VertexPtrVector& neighbourSamples, bool addAll);
360 void vertexInsertHelper(const VertexPtr &newVertex, bool expandIfBeforeToken, bool removeFromFree,
bool edgeInsertCondition(const VertexPtrPair &edge) const
The condition used to insert edges into the queue. Compares lowerBoundHeuristicEdge to the given thre...
Definition: SearchQueue.cpp:698
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition: BITstar.h:243
VertexPtr frontVertex()
Get the best vertex on the queue, leaving it at the front of the vertex queue.
Definition: SearchQueue.cpp:198
std::array< ompl::base::Cost, 2u > CostDouble
A pair of costs, i.e., the vertex queue sorting key. Done as an array instead of a pair for consisten...
Definition: SearchQueue.h:196
void getVertices(VertexConstPtrVector *vertexQueue)
Get a copy of the vertices in the vertex queue that are left to be expanded. This is expensive and is...
Definition: SearchQueue.cpp:894
unsigned int numEdgesPopped() const
The number of edges popped off the queue for processing (numEdgesPopped_).
Definition: SearchQueue.cpp:1532
bool isVertexExpanded(const VertexConstPtr &vertex) const
Returns whether a given vertex has been expanded or not.
Definition: SearchQueue.cpp:876
void finish()
Finish the queue if it is sorted, if not resort the queue. Finishing the queue clears all the edge co...
Definition: SearchQueue.cpp:635
unsigned int numEdgesFrom(const VertexPtr &pVertex)
Get the number of edges in the queue coming from a specific vertex. Will resort/expand the queue if n...
Definition: SearchQueue.cpp:819
void removeExtraEdgesFrom(const VertexPtr &pVertex)
Removes edges in the edge queue that leave from the given vertex that would not be added to the queue...
Definition: SearchQueue.cpp:425
VertexPtrPair popFrontEdge()
Pop the best edge off the queue, removing it from the front of the edge queue in the process.
Definition: SearchQueue.cpp:312
void enqueueEdge(const VertexPtrPair &newEdge)
Insert an edge into the edge processing queue. The source vertex of this edge must be in the expansio...
Definition: SearchQueue.cpp:150
std::array< ompl::base::Cost, 3u > CostTriple
A triplet of costs, i.e., the edge queue sorting key.
Definition: SearchQueue.h:208
void resort()
Resort the queue around the marked unsorted vertices. If allowed, will remove any vertices that need ...
Definition: SearchQueue.cpp:533
void setStrictQueueOrdering(bool beStrict)
Set the queue to stay strictly sorted with each rewiring.
Definition: SearchQueue.cpp:1502
std::pair< VertexPtr, VertexPtr > VertexPtrPair
A pair of vertices, i.e., an edge.
Definition: BITstar.h:232
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared const pointers.
Definition: BITstar.h:228
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
void getEdges(VertexConstPtrPairVector *edgeQueue)
Get a copy of the edge queue. This is expensive and is only meant for animations/debugging.
Definition: SearchQueue.cpp:912
void setDelayedRewiring(bool delayRewiring)
Delay considering rewiring edges until an initial solution is found.
Definition: SearchQueue.cpp:1512
bool vertexInsertCondition(const VertexPtr &state) const
The condition used to insert vertices into the queue. Compares lowerBoundHeuristicVertex to the given...
Definition: SearchQueue.cpp:685
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 edgePruneCondition(const VertexPtrPair &edge) const
The condition used to prune edge (i.e., vertex-pair) out of the queue. Compares currentHeuristicEdge ...
Definition: SearchQueue.cpp:747
unsigned int numVertices()
Returns the number of vertices left to expand. This has nontrivial cost, as the token must be moved t...
Definition: SearchQueue.cpp:783
std::pair< unsigned int, unsigned int > prune(const VertexConstPtr &vertex)
Prune the vertex queue of vertices whose their lower-bound heuristic is greater then the threshold....
Definition: SearchQueue.cpp:473
ompl::BinaryHeap< CostTripleAndVertexPtrPair, EdgeQueueSortFunc > EdgeQueueAsPairBinHeap
The underlying edge queue. Using static keys for the same reason as the Vertex Queue.
Definition: SearchQueue.h:214
std::vector< VertexConstPtrPair > VertexConstPtrPairVector
A vector of pairs of const vertices, i.e., a vector of edges.
Definition: BITstar.h:238
std::function< bool(const CostDouble &, const CostDouble &)> VertexQueueSortFunc
The function signature of the sorting function for the Vertex Queue.
Definition: SearchQueue.h:198
VertexPtrPair frontEdge()
Get the best edge on the queue, leaving it at the front of the edge queue.
Definition: SearchQueue.cpp:216
void markVertexUnsorted(const VertexPtr &vertex)
Mark the queue as requiring resorting downstream of the specified vertex.
Definition: SearchQueue.cpp:466
bool isEmpty()
Returns true if the queue is empty. In the case where the edge queue is empty but the vertex queue is...
Definition: SearchQueue.cpp:851
void removeEdgesTo(const VertexPtr &cVertex)
Erase all edges in the edge queue that lead to the given vertex.
Definition: SearchQueue.cpp:334
void removeExtraEdgesTo(const VertexPtr &cVertex)
Removes edges in the edge queue that lead to the given vertex that would not be added to the queue no...
Definition: SearchQueue.cpp:384
CostTriple frontEdgeValue()
Get the value of the best edge on the queue, leaving it at the front of the edge queue.
Definition: SearchQueue.cpp:252
SearchQueue(NameFunc nameFunc)
Construct a search queue. It must be setup before use.
Definition: SearchQueue.cpp:75
void enqueueVertex(const VertexPtr &newVertex, bool removeFromFree)
Insert a vertex into the vertex expansion queue. Vertices remain in the vertex queue until pruned or ...
Definition: SearchQueue.cpp:142
unsigned int numEdges()
Returns the number of edges in the queue. Will resort/expand the queue if necessary.
Definition: SearchQueue.cpp:773
std::vector< EdgeQueueElemPtr > EdgeQueueElemPtrVector
A vector of edge queue pointers.
Definition: SearchQueue.h:218
std::pair< CostTriple, VertexPtrPair > CostTripleAndVertexPtrPair
The data stored in the edge-queue binary heap.
Definition: SearchQueue.h:210
void reset()
Reset the queue, clearing all the edge containers and moving the vertex expansion token to the start....
Definition: SearchQueue.cpp:661
bool isReset() const
Returns true if the queue is reset. This means that no edges have been expanded and the vertex expans...
Definition: SearchQueue.cpp:844
std::function< bool(const CostTripleAndVertexPtrPair &, const CostTripleAndVertexPtrPair &)> EdgeQueueSortFunc
The function signature of the sorting function for the Edge Queue.
Definition: SearchQueue.h:212
unsigned int numEdgesTo(const VertexPtr &cVertex)
Get the number of edges in the queue pointing to a specific vertex. Will resort/expand the queue if n...
Definition: SearchQueue.cpp:808
void unqueueVertex(const VertexPtr &oldVertex)
Erase a vertex from the vertex expansion queue. Will disconnect the vertex from its parent and remove...
Definition: SearchQueue.cpp:184
bool getStrictQueueOrdering() const
Get whether the queue stays strictly sorted with each rewiring.
Definition: SearchQueue.cpp:1507
void setup(CostHelper *costHelpPtr, ImplicitGraph *graphPtr)
Setup the SearchQueue, must be called before use.
Definition: SearchQueue.cpp:89
std::shared_ptr< const Vertex > VertexConstPtr
A constant vertex shared pointer.
Definition: BITstar.h:222
bool vertexPruneCondition(const VertexPtr &state) const
The condition used to prune vertices out of the queue. Compares currentHeuristicVertex to the given t...
Definition: SearchQueue.cpp:724
bool getDelayedRewiring() const
Get whether BIT* is delaying rewiring until a solution is found.
Definition: SearchQueue.cpp:1517
CostDouble frontVertexValue()
Get the value of the best vertex on the queue, leaving it at the front of the vertex queue.
Definition: SearchQueue.cpp:234
void hasSolution(const ompl::base::Cost &solnCost)
Mark that a solution has been found.
Definition: SearchQueue.cpp:323
bool samplePruneCondition(const VertexPtr &state) const
The condition used to prune disconnected samples from the free set. Compares lowerBoundHeuristicVerte...
Definition: SearchQueue.cpp:736
unsigned int numUnsorted() const
Return the number of vertices marked as unsorted.
Definition: SearchQueue.cpp:830
VertexQueueAsMMap::iterator VertexQueueIter
An iterator into the vertex queue multimap.
Definition: SearchQueue.h:205
std::multimap< CostDouble, VertexPtr, VertexQueueSortFunc > VertexQueueAsMMap
The underlying vertex queue. The advantage of a multimap over a multiset is that a copy of the key is...
Definition: SearchQueue.h:203
void removeEdgesFrom(const VertexPtr &pVertex)
Erase all edges in the edge queue that leave from the given vertex.
Definition: SearchQueue.cpp:359
Main namespace. Contains everything in this library.
Definition: ConstrainedSpaceInformation.h:52