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
 An alias declaration for a pair of costs, i.e., the vertex sorting key. Done as an array instead of a pair for consistency with the EdgeQueue.
 
typedef std::array< ompl::base::Cost, 3u > CostTriple
 An alias declaration for a triple of costs, i.e., the edge sorting key.
 
typedef std::function< double(const VertexConstPtr &, const VertexConstPtr &)> DistanceFunc
 A std::function definition for the distance between two vertices.
 
typedef std::function< unsigned int(const VertexPtr &, VertexPtrVector *)> NeighbourhoodFunc
 A std::function definition for the neighbourhood of a vertex .
 

Public Member Functions

 SearchQueue (NameFunc nameFunc)
 Construct a search queue. It must be setup before use.
 
void setup (const CostHelperPtr &costHelpPtr, const ImplicitGraphPtr &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 ()
 Get the best vertex on the queue, leaving it at the front of the vertex queue.
 
VertexPtrPair frontEdge ()
 Get the best edge on the queue, leaving it at the front of the edge queue.
 
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 &pruneStartPtr)
 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 simply remove any vertices that need to be resorted but will 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 mocs the vertex expansion token to the end. After a call to finish, 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 &vertex) 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 &vertex) 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 &vertex) 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 90 of file SearchQueue.h.


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