ompl::geometric::BITstar::SearchQueue Class Reference

A queue of edges to be processed that integrates both the expansion of Vertices and the ordering of the resulting edges. More...

#include <ompl/geometric/planners/bitstar/datastructures/SearchQueue.h>

## Public Types

typedef 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 consistency with the EdgeQueue.

typedef std::function< bool(const CostDouble &, const CostDouble &)> VertexQueueSortFunc
The function signature of the sorting function for the Vertex Queue.

typedef std::multimap< CostDouble, VertexPtr, VertexQueueSortFuncVertexQueueAsMMap
The underlying vertex queue. The advantage of a multimap over a multiset is that a copy of the key is stored with the value, which guarantees that the ordering remains sane even if the data inherently behind the queue value changes. In such a case the queue will remain sorted by the old key until manually updated.

typedef VertexQueueAsMMap::iterator VertexQueueIter
An iterator into the vertex queue multimap.

typedef std::array< ompl::base::Cost, 3u > CostTriple
A triplet of costs, i.e., the edge queue sorting key.

typedef std::pair< CostTriple, VertexPtrPairCostTripleAndVertexPtrPair
The data stored in the edge-queue binary heap.

typedef std::function< bool(const CostTripleAndVertexPtrPair &, const CostTripleAndVertexPtrPair &)> EdgeQueueSortFunc
The function signature of the sorting function for the Edge Queue.

typedef ompl::BinaryHeap< CostTripleAndVertexPtrPair, EdgeQueueSortFuncEdgeQueueAsPairBinHeap
The underlying edge queue. Using static keys for the same reason as the Vertex Queue.

typedef EdgeQueueAsPairBinHeap::Element * EdgeQueueElemPtr
An element pointer into the edge queue binary heap.

typedef std::vector< EdgeQueueElemPtrEdgeQueueElemPtrVector
A vector of edge queue pointers.

## Public Member Functions

SearchQueue (NameFunc nameFunc)
Construct a search queue. It must be setup before use.

void setup (CostHelper *costHelpPtr, ImplicitGraph *graphPtr)
Setup the SearchQueue, must be called before use.

void clear ()
Clear the queue to the state of construction.

void enqueueVertex (const VertexPtr &newVertex, bool removeFromFree)
Insert a vertex into the vertex expansion queue. Vertices remain in the vertex queue until pruned or manually removed. A moving token marks the line between expanded and not expanded vertices. Will instruct the ImplicitGraph to move the vertex between sets, as necessary.

void enqueueEdge (const VertexPtrPair &newEdge)
Insert an edge into the edge processing queue. The source vertex of this edge must be in the expansion queue (although it may already be expanded).

void enqueueEdge (const VertexPtr &sourceVertex, const VertexPtr &targetVertex)
Insert an edge into the edge processing queue. The source vertex of this edge must be in the expansion queue (although it may already be expanded).

void unqueueVertex (const VertexPtr &oldVertex)
Erase a vertex from the vertex expansion queue. Will disconnect the vertex from its parent and remove the associated incoming and outgoing edges from the edge queue. Will instruct the ImplicitGraph to move the vertex between sets, as necessary.

VertexPtr frontVertex ()

VertexPtrPair frontEdge ()

CostDouble frontVertexValue ()
Get the value of the best vertex on the queue, leaving it at the front of the vertex queue.

CostTriple frontEdgeValue ()
Get the value of the best edge on the queue, leaving it at the front of the edge queue.

void popFrontEdge (VertexPtrPair *bestEdge)
Pop the best edge off the queue, removing it from the front of the edge queue in the process.

VertexPtrPair popFrontEdge ()
Pop the best edge off the queue, removing it from the front of the edge queue in the process.

void hasSolution (const ompl::base::Cost &solnCost)
Mark that a solution has been found.

void removeEdgesTo (const VertexPtr &cVertex)
Erase all edges in the edge queue that lead to the given vertex.

void removeEdgesFrom (const VertexPtr &pVertex)
Erase all edges in the edge queue that leave from the given vertex.

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 now.

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 now.

void markVertexUnsorted (const VertexPtr &vertex)
Mark the queue as requiring resorting downstream of the specified vertex.

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. Descendents of pruned vertices that are not pruned themselves are returned to the set of free states. Returns the number of vertices removed, and the number of said vertices that are completely thrown away (i.e., are not even useful as a sample)

void resort ()
Resort the queue around the marked unsorted vertices. If allowed, will remove any vertices that need to be resorted but would later be pruned.

void finish ()
Finish the queue if it is sorted, if not resort the queue. Finishing the queue clears all the edge containers and moves the vertex expansion token to the end. After calling finish() ON A SORTED QUEUE, isEmpty() will return true. Keeps threshold, etc.

void reset ()
Reset the queue, clearing all the edge containers and moving the vertex expansion token to the start. After a call to reset, isEmpty() will return false (unless there is no data in the queue of course). Keeps threshold, list of unsorted vertices, etc.

bool vertexInsertCondition (const VertexPtr &state) const
The condition used to insert vertices into the queue. Compares lowerBoundHeuristicVertex to the given threshold. Returns true if the vertex's best cost is lower than the internally set threshold.

bool edgeInsertCondition (const VertexPtrPair &edge) const
The condition used to insert edges into the queue. Compares lowerBoundHeuristicEdge to the given threshold. Returns true if the edge's best cost is lower than the internally set threshold.

bool vertexPruneCondition (const VertexPtr &state) const
The condition used to prune vertices out of the queue. Compares currentHeuristicVertex to the given threshold. Returns true if the vertex's best cost is greater than the internally set threshold.

bool samplePruneCondition (const VertexPtr &state) const
The condition used to prune disconnected samples from the free set. Compares lowerBoundHeuristicVertex to the given threshold. Returns true if the vertex's best cost is greater than or equal to the internally set threshold.

bool edgePruneCondition (const VertexPtrPair &edge) const
The condition used to prune edge (i.e., vertex-pair) out of the queue. Compares currentHeuristicEdge to the given threshold. Returns true if the edge's best cost is greater than the internally set threshold.

unsigned int numEdges ()
Returns the number of edges in the queue. Will resort/expand the queue if necessary.

unsigned int numVertices ()
Returns the number of vertices left to expand. This has nontrivial cost, as the token must be moved through the vector to count. Will resort/expand the queue if necessary.

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 necessary.

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 necessary.

unsigned int numUnsorted () const
Return the number of vertices marked as unsorted.

bool isSorted () const
Return whether the queue is still sorted.

bool isReset () const
Returns true if the queue is reset. This means that no edges have been expanded and the vertex expansion token is pointing at the start.

bool isEmpty ()
Returns true if the queue is empty. In the case where the edge queue is empty but the vertex queue is not, this function will expand vertices until the edge queue is not empty or there are no vertices to expand.

bool isVertexExpanded (const VertexConstPtr &vertex) const
Returns whether a given vertex has been expanded or not.

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 only meant for animations/debugging.

void getEdges (VertexConstPtrPairVector *edgeQueue)
Get a copy of the edge queue. This is expensive and is only meant for animations/debugging.

void setStrictQueueOrdering (bool beStrict)
Set the queue to stay strictly sorted with each rewiring.

bool getStrictQueueOrdering () const
Get whether the queue stays strictly sorted with each rewiring.

void setDelayedRewiring (bool delayRewiring)
Delay considering rewiring edges until an initial solution is found.

bool getDelayedRewiring () const
Get whether BIT* is delaying rewiring until a solution is found.

void setPruneDuringResort (bool prune)
Prune during resorts.

bool getPruneDuringResort () const
Prune during resorts.

unsigned int numEdgesPopped () const
The number of edges popped off the queue for processing (numEdgesPopped_).

## Detailed Description

A queue of edges to be processed that integrates both the expansion of Vertices and the ordering of the resulting edges.

Short Description
A two-stage queue that consists of vertices expanded into edges to be processed. The queue consists of a vertex expansion queue and an edge processing queue. Vertices are expanded as needed from the vertex queue into edges places in the edge queue. Edges are removed from the edge queue for processing by BIT*. The vertex queue is implemented as a static ordered list of the vertices in the graph with a token (i.e., an iterator) pointing to the next vertex that needs to be expanded. This is specifically a multimap ordered on ompl::base::Cost. The edge queue is implemented as an ordered list of potential edges. It is filled by the vertex queue and emptied by popping the best value off the front. It is specifically a multimap ordered on std::pair<ompl::base::Cost, ompl::base::Cost>
Notes:
• An eraseEdge() function could be made by mimicking the vertex -> vertexQueue_::iterator lookup datastructure for the edgeQueue_

Definition at line 92 of file SearchQueue.h.

The documentation for this class was generated from the following files: