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 edgePruneCondition(const VertexPtrPair &edge) const
The condition used to prune edge (i.e., vertex-pair) out of the queue. Compares currentHeuristicEdge ...
Definition: SearchQueue.cpp:764
void getEdges(VertexConstPtrPairVector *edgeQueue)
Get a copy of the edge queue. This is expensive and is only meant for animations/debugging.
Definition: SearchQueue.cpp:981
void markVertexUnsorted(const VertexPtr &vertex)
Mark the queue as requiring resorting downstream of the specified vertex.
Definition: SearchQueue.cpp:470
std::vector< VertexConstPtrPair > VertexConstPtrPairVector
A vector of pairs of const vertices, i.e., a vector of edges.
Definition: BITstar.h:142
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:907
bool edgeInsertCondition(const VertexPtrPair &edge) const
The condition used to insert edges into the queue. Compares lowerBoundHeuristicEdge to the given thre...
Definition: SearchQueue.cpp:715
CostTriple frontEdgeValue()
Get the value of the best edge on the queue, leaving it at the front of the edge queue.
Definition: SearchQueue.cpp:226
bool vertexInsertCondition(const VertexPtr &state) const
The condition used to insert vertices into the queue. Compares lowerBoundHeuristicVertex to the given...
Definition: SearchQueue.cpp:702
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:354
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:800
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:477
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:653
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:97
bool getStrictQueueOrdering() const
Get whether the queue stays strictly sorted with each rewiring.
Definition: SearchQueue.cpp:1681
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:142
bool samplePruneCondition(const VertexPtr &state) const
The condition used to prune disconnected samples from the free set. Compares lowerBoundHeuristicVerte...
Definition: SearchQueue.cpp:753
SearchQueue(NameFunc nameFunc)
Construct a search queue. It must be setup before use.
Definition: SearchQueue.cpp:65
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:963
std::shared_ptr< ImplicitGraph > ImplicitGraphPtr
An implicit graph shared pointer.
Definition: BITstar.h:148
void setDelayedRewiring(bool delayRewiring)
Delay considering rewiring edges until an initial solution is found.
Definition: SearchQueue.cpp:1686
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:859
std::shared_ptr< const Vertex > VertexConstPtr
A constant vertex shared pointer.
Definition: BITstar.h:126
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:825
void hasSolution(const ompl::base::Cost &solnCost)
Mark that a solution has been found.
Definition: SearchQueue.cpp:279
bool getDelayedRewiring() const
Get whether BIT* is delaying rewiring until a solution is found.
Definition: SearchQueue.cpp:1691
std::array< ompl::base::Cost, 3u > CostTriple
An alias declaration for a triple of costs, i.e., the edge sorting key.
Definition: SearchQueue.h:99
VertexPtr frontVertex()
Get the best vertex on the queue, leaving it at the front of the vertex queue.
Definition: SearchQueue.cpp:172
unsigned int numUnsorted() const
Return the number of vertices marked as unsorted.
Definition: SearchQueue.cpp:893
std::function< double(const VertexConstPtr &, const VertexConstPtr &)> DistanceFunc
A std::function definition for the distance between two vertices.
Definition: SearchQueue.h:105
void removeEdgesFrom(const VertexPtr &pVertex)
Erase all edges in the edge queue that leave from the given vertex.
Definition: SearchQueue.cpp:322
void setup(const CostHelperPtr &costHelpPtr, const ImplicitGraphPtr &graphPtr)
Setup the SearchQueue, must be called before use.
Definition: SearchQueue.cpp:79
std::pair< VertexPtr, VertexPtr > VertexPtrPair
A pair of vertices, i.e., an edge.
Definition: BITstar.h:136
A queue of edges to be processed that integrates both the expansion of Vertices and the ordering of t...
Definition: SearchQueue.h:90
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition: BITstar.h:153
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:914
unsigned int numEdgesPopped() const
The number of edges popped off the queue for processing (numEdgesPopped_).
Definition: SearchQueue.cpp:1706
void setStrictQueueOrdering(bool beStrict)
Set the queue to stay strictly sorted with each rewiring.
Definition: SearchQueue.cpp:1676
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared const pointers.
Definition: BITstar.h:132
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:158
CostDouble frontVertexValue()
Get the value of the best vertex on the queue, leaving it at the front of the vertex queue...
Definition: SearchQueue.cpp:208
VertexPtrPair popFrontEdge()
Pop the best edge off the queue, removing it from the front of the edge queue in the process...
Definition: SearchQueue.cpp:268
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:134
bool isVertexExpanded(const VertexConstPtr &vertex) const
Returns whether a given vertex has been expanded or not.
Definition: SearchQueue.cpp:932
void removeEdgesTo(const VertexPtr &cVertex)
Erase all edges in the edge queue that lead to the given vertex.
Definition: SearchQueue.cpp:290
VertexPtrPair frontEdge()
Get the best edge on the queue, leaving it at the front of the edge queue.
Definition: SearchQueue.cpp:190
void reset()
Reset the queue, clearing all the edge containers and moving the vertex expansion token to the start...
Definition: SearchQueue.cpp:679
std::function< unsigned int(const VertexPtr &, VertexPtrVector *)> NeighbourhoodFunc
A std::function definition for the neighbourhood of a vertex .
Definition: SearchQueue.h:108
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:741
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
void resort()
Resort the queue around the marked unsorted vertices. If allowed, will simply remove any vertices tha...
Definition: SearchQueue.cpp:551
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:412
unsigned int numEdges()
Returns the number of edges in the queue. Will resort/expand the queue if necessary.
Definition: SearchQueue.cpp:790