SearchQueue.h
63 // 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
64 // aware of the class BITstar. It has a forward declaration to me and the other helper classes but I will need to
309 typedef std::multimap<CostDouble, VertexPtr, std::function<bool(const CostDouble &, const CostDouble &)>>
331 typedef std::unordered_map<BITstar::VertexId, EdgeQueueIterVector> VertexIdToEdgeQueueIterVectorUMap;
370 void vertexInsertHelper(const VertexPtr &newVertex, bool expandIfBeforeToken, bool removeFromFree,
385 void edgeRemoveHelper(const EdgeQueueIter &oldEdgeIter, bool rmIncomingLookup, bool rmOutgoingLookup);
388 void rmEdgeLookupHelper(VertexIdToEdgeQueueIterVectorUMap &lookup, const BITstar::VertexId &idx,
bool edgeInsertCondition(const VertexPtrPair &edge) const
The condition used to insert edges into the queue. Compares lowerBoundHeuristicEdge to the given thre...
Definition: SearchQueue.cpp:779
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition: BITstar.h:249
VertexPtr frontVertex()
Get the best vertex on the queue, leaving it at the front of the vertex queue.
Definition: SearchQueue.cpp:236
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...
Definition: SearchQueue.h:193
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:1027
unsigned int numEdgesPopped() const
The number of edges popped off the queue for processing (numEdgesPopped_).
Definition: SearchQueue.cpp:1770
bool isVertexExpanded(const VertexConstPtr &vertex) const
Returns whether a given vertex has been expanded or not.
Definition: SearchQueue.cpp:996
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:717
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:923
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:476
VertexPtrPair popFrontEdge()
Pop the best edge off the queue, removing it from the front of the edge queue in the process.
Definition: SearchQueue.cpp:332
void setup(const CostHelperPtr &costHelpPtr, const ImplicitGraphPtr &graphPtr)
Setup the SearchQueue, must be called before use.
Definition: SearchQueue.cpp:143
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:206
std::array< ompl::base::Cost, 3u > CostTriple
An alias declaration for a triple of costs, i.e., the edge sorting key.
Definition: SearchQueue.h:195
std::function< unsigned int(const VertexPtr &, VertexPtrVector *)> NeighbourhoodFunc
A std::function definition for the neighbourhood of a vertex .
Definition: SearchQueue.h:204
void resort()
Resort the queue around the marked unsorted vertices. If allowed, will simply remove any vertices tha...
Definition: SearchQueue.cpp:615
void setStrictQueueOrdering(bool beStrict)
Set the queue to stay strictly sorted with each rewiring.
Definition: SearchQueue.cpp:1740
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:1045
void setDelayedRewiring(bool delayRewiring)
Delay considering rewiring edges until an initial solution is found.
Definition: SearchQueue.cpp:1750
bool vertexInsertCondition(const VertexPtr &state) const
The condition used to insert vertices into the queue. Compares lowerBoundHeuristicVertex to the given...
Definition: SearchQueue.cpp:766
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:828
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:864
std::vector< VertexConstPtrPair > VertexConstPtrPairVector
A vector of pairs of const vertices, i.e., a vector of edges.
Definition: BITstar.h:238
VertexPtrPair frontEdge()
Get the best edge on the queue, leaving it at the front of the edge queue.
Definition: SearchQueue.cpp:254
void markVertexUnsorted(const VertexPtr &vertex)
Mark the queue as requiring resorting downstream of the specified vertex.
Definition: SearchQueue.cpp:534
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:978
void removeEdgesTo(const VertexPtr &cVertex)
Erase all edges in the edge queue that lead to the given vertex.
Definition: SearchQueue.cpp:354
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:418
std::pair< unsigned int, unsigned int > prune(const VertexConstPtr &goalVertexPtr)
Prune the vertex queue of vertices whose their lower-bound heuristic is greater then the threshold....
Definition: SearchQueue.cpp:541
std::function< double(const VertexConstPtr &, const VertexConstPtr &)> DistanceFunc
A std::function definition for the distance between two vertices.
Definition: SearchQueue.h:201
CostTriple frontEdgeValue()
Get the value of the best edge on the queue, leaving it at the front of the edge queue.
Definition: SearchQueue.cpp:290
SearchQueue(NameFunc nameFunc)
Construct a search queue. It must be setup before use.
Definition: SearchQueue.cpp:129
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:198
unsigned int numEdges()
Returns the number of edges in the queue. Will resort/expand the queue if necessary.
Definition: SearchQueue.cpp:854
void reset()
Reset the queue, clearing all the edge containers and moving the vertex expansion token to the start....
Definition: SearchQueue.cpp:743
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:971
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:889
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:222
bool getStrictQueueOrdering() const
Get whether the queue stays strictly sorted with each rewiring.
Definition: SearchQueue.cpp:1745
std::shared_ptr< ImplicitGraph > ImplicitGraphPtr
An implicit graph shared pointer.
Definition: BITstar.h:244
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:805
bool getDelayedRewiring() const
Get whether BIT* is delaying rewiring until a solution is found.
Definition: SearchQueue.cpp:1755
CostDouble frontVertexValue()
Get the value of the best vertex on the queue, leaving it at the front of the vertex queue.
Definition: SearchQueue.cpp:272
void hasSolution(const ompl::base::Cost &solnCost)
Mark that a solution has been found.
Definition: SearchQueue.cpp:343
bool samplePruneCondition(const VertexPtr &state) const
The condition used to prune disconnected samples from the free set. Compares lowerBoundHeuristicVerte...
Definition: SearchQueue.cpp:817
unsigned int numUnsorted() const
Return the number of vertices marked as unsorted.
Definition: SearchQueue.cpp:957
void removeEdgesFrom(const VertexPtr &pVertex)
Erase all edges in the edge queue that leave from the given vertex.
Definition: SearchQueue.cpp:386