SearchQueue.cpp
82 , edgeQueue_([this](const CostTripleAndVertexPtrPair &lhs, const CostTripleAndVertexPtrPair &rhs)
104 // Reset everything to the state of construction OTHER than planner name and settings/parameters
176 void BITstar::SearchQueue::enqueueEdge(const VertexPtr &sourceVertex, const VertexPtr &targetVertex)
340 // Iterate over the vector of incoming edges to this vertex and remove them from the queue (and clean up their other lookup)
345 // Remove the edge from the *other* lookup (by value since this is NOT an iter to THAT container).
347 (*inQueueElemIter)->data.second.first->rmOutgoingEdgeQueuePtr(*inQueueElemIter, numQueueResets_);
365 // Iterate over the vector of outgoing edges to this vertex and remove them from the queue (and clean up their other lookup)
370 // Remove the edge from the *other* lookup (by value since this is NOT an iter to THAT container).
372 (*outQueueElemIter)->data.second.second->rmIncomingEdgeQueuePtr(*outQueueElemIter, numQueueResets_);
408 // Now, iterate over the vector of iterators to delete and remove them from the queue and their lookups
449 // Now, iterate over the vector of iterators to delete and remove them from the queue and their lookups
488 // The vertex expansion queue is sorted on an estimated solution cost considering the *current* cost-to-come
490 // This means that the value of the vertices in the queue are an upper-bounding estimate of the value we
492 // Therefore, we can start our pruning at the goal vertex and iterate forward through the queue from there.
548 OMPL_INFORM("%s: Resorting %d vertices in the queue.", nameFunc_().c_str(), resortVertices_.size());
552 // Iterate over the vector and place into the unique queue indexed on *depth*. This guarantees that we
626 OMPL_INFORM("%s: Resorting the queue disconnected %d vertices from the tree and completely removed "
694 return costHelpPtr_->isCostBetterThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicVertex(state),
704 // Can it ever be a better solution? Less-than-equal to just in case we're using an edge that is exactly
707 rval = costHelpPtr_->isCostBetterThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicEdge(edge),
710 // If the child is connected already, we need to check if we could do better than it's current connection.
714 // Can it ever be a better path to the vertex? Less-than-equal to just in case we're using an edge that
717 rval = costHelpPtr_->isCostBetterThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicToTarget(edge),
730 // Prune the vertex if it could cannot part of a better solution in the current graph. Greater-than just in
743 return costHelpPtr_->isCostWorseThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicVertex(state),
754 // Prune the edge if it could cannot part of a better solution in the current graph. Greater-than just in
759 // If the child is connected already, we need to check if we could do better than it's current connection.
763 // Can it ever be a better path to the vertex in the current graph? Greater-than to just in case we're
798 for (VertexQueueAsMMap::const_iterator vIter = vertexToExpand_; vIter != vertexQueue_.end(); ++vIter)
865 // If the edge queue is actually empty, than use this opportunity to resort any unsorted vertices
888 // By virtue of the vertex expansion rules, the token will always sit at the front of a group of
889 // equivalent cost vertices (that is to say, all vertices with the same cost get expanded at the same
905 for (VertexQueueAsMMap::const_iterator vIter = vertexToExpand_; vIter != vertexQueue_.end(); ++vIter)
1040 // Do so intelligently to avoid repeatedly considering the same failed edges (likely due to collision).
1051 // It has, which means that outgoing edges to old unconnected vertices have already been considered.
1056 // If the vertex has never been expanded into possible rewiring edges *and* either we're not delaying
1060 // If we're using an r-disc RGG, we will not have gotten the neighbour vertices yet, get them now
1068 // Iterate over the vector of connected targets and add only those who could ever provide a better
1077 void BITstar::SearchQueue::enqueueSamples(const VertexPtr &vertex, const VertexPtrVector& neighbourSamples, bool addAll)
1095 void BITstar::SearchQueue::enqueueVertices(const VertexPtr &vertex, const VertexPtrVector& neighbourVertices)
1097 // Iterate over the vector of connected targets and add only those who could ever provide a better
1129 void BITstar::SearchQueue::enqueueEdgeConditionally(const VertexPtr &parent, const VertexPtr &child)
1146 void BITstar::SearchQueue::processKNearest(const VertexConstPtr &vertex, VertexPtrVector *kNearSamples,
1204 else if (this->queueComparison(unorderedVertex->getVertexQueueIter()->first, vertexToExpand_->first))
1206 // This vertex is currently in the queue with a cost that is in front of the current token. It has been expanded:
1227 // Reinsert myself, expanding if I cross the token if I am not already expanded but not removing/adding
1247 // No else, I was not previously expanded so my edges (if there are now any) were created up to date
1250 std::pair<unsigned int, unsigned int> BITstar::SearchQueue::pruneBranch(const VertexPtr &branchBase)
1271 // Disconnect myself from my parent, not cascading costs as I know my children are also being disconnected:
1291 void BITstar::SearchQueue::disconnectParent(const VertexPtr &oldVertex, bool cascadeCostUpdates)
1296 throw ompl::Exception("An orphaned vertex has been passed for disconnection. Something went wrong.");
1300 // Check if my parent has already been pruned. This can occur if we're cascading vertex disconnections.
1311 void BITstar::SearchQueue::vertexInsertHelper(const VertexPtr &newVertex, bool expandIfBeforeToken,
1325 vertexIter = vertexQueue_.insert(std::make_pair(this->vertexQueueValue(newVertex), newVertex));
1333 // If the vertex queue is now of size 1, that means that this was the first vertex. Set the token to it
1344 2 The new vertex is before the token, but *not* immediately (i.e., there are vertices between it):
1347 3 The new vertex is after the token: Don't expand. It cleanly goes into the vector of vertices to
1349 Note: By shifting the token, we assure that if the new vertex is better than the best edge, it will get
1352 The cases look like this (-: expanded vertex, x: unexpanded vertex, X: token (next to expand), *: new
1364 // The vertex before the token. Remember that since we have already added the new vertex, this could be
1375 // The vertex before the token is the newly added vertex. Therefore we can just move the token up to
1406 unsigned int BITstar::SearchQueue::vertexRemoveHelper(const VertexPtr &oldVertex, bool fullyRemove)
1409 // The number of samples deleted (i.e., if this vertex is NOT recycled as a sample, this is a 1)
1412 // The use count of the passed shared pointer. Used in debug mode to assert that we took ownership of our own copy.
1415 // A copy of the vertex pointer to be removed so we can't delete it out from under ourselves (occurs when
1430 throw ompl::Exception("Cannot delete a vertex connected to a parent unless the vertex is being "
1467 BITstar::SearchQueue::CostDouble BITstar::SearchQueue::vertexQueueValue(const VertexPtr &vertex) const
1473 BITstar::SearchQueue::CostTriple BITstar::SearchQueue::edgeQueueValue(const VertexPtrPair &edge) const
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
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: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
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
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
A conceptual representation of samples as an edge-implicit random geometric graph.
Definition: ImplicitGraph.h:128
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
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