SearchQueue.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, University of Toronto
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the University of Toronto nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Authors: Jonathan Gammell */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_BITSTAR_DATASTRUCTURES_SEARCHQUEUE_
38 #define OMPL_GEOMETRIC_PLANNERS_BITSTAR_DATASTRUCTURES_SEARCHQUEUE_
39 
40 // STL:
41 // std::pair
42 #include <utility>
43 // std::vector
44 #include <vector>
45 // std::array
46 #include <array>
47 // std::multimap
48 #include <map>
49 // std::unordered_map
50 #include <unordered_map>
51 // For std::function
52 #include <functional>
53 
54 // OMPL:
55 // The cost class:
56 #include "ompl/base/Cost.h"
57 // The optimization objective class:
58 #include "ompl/base/OptimizationObjective.h"
59 // The nearest neighbours structure
60 #include "ompl/datastructures/NearestNeighbors.h"
61 // The binary heap data structure
62 #include "ompl/datastructures/BinaryHeap.h"
63 
64 // BIT*:
65 // 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
66 // aware of the class BITstar. It has a forward declaration to me and the other helper classes but I will need to
67 // include any I use in my cpp (to avoid dependency loops).
68 #include "ompl/geometric/planners/bitstar/BITstar.h"
69 
70 namespace ompl
71 {
72  namespace geometric
73  {
93  {
94  public:
96  // Aliases to underlying data types used by the SearchQueue
97  // Types for the vertex queue:
100  typedef std::array<ompl::base::Cost, 2u> CostDouble;
102  typedef std::function<bool(const CostDouble &, const CostDouble &)> VertexQueueSortFunc;
107  typedef std::multimap<CostDouble, VertexPtr, VertexQueueSortFunc> VertexQueueAsMMap;
109  typedef VertexQueueAsMMap::iterator VertexQueueIter;
110  // Types for the edge queue
112  typedef std::array<ompl::base::Cost, 3u> CostTriple;
114  typedef std::pair<CostTriple, VertexPtrPair> CostTripleAndVertexPtrPair;
116  typedef std::function<bool(const CostTripleAndVertexPtrPair &, const CostTripleAndVertexPtrPair &)> EdgeQueueSortFunc;
120  typedef EdgeQueueAsPairBinHeap::Element* EdgeQueueElemPtr;
122  typedef std::vector<EdgeQueueElemPtr> EdgeQueueElemPtrVector;
124 
127  // Public functions:
129  SearchQueue(NameFunc nameFunc);
130 
131  virtual ~SearchQueue() = default;
132 
134  void setup(CostHelper *costHelpPtr, ImplicitGraph *graphPtr);
135 
137  void clear();
139 
141  // Insert and erase
145  void enqueueVertex(const VertexPtr &newVertex, bool removeFromFree);
146 
149  void enqueueEdge(const VertexPtrPair &newEdge);
150 
153  void enqueueEdge(const VertexPtr &sourceVertex, const VertexPtr &targetVertex);
154 
158  void unqueueVertex(const VertexPtr &oldVertex);
160 
162  // Access the queue
165 
168 
171 
174 
176  void popFrontEdge(VertexPtrPair *bestEdge);
177 
181 
183  // Queue maintenance
185  void hasSolution(const ompl::base::Cost &solnCost);
186 
188  void removeEdgesTo(const VertexPtr &cVertex);
189 
191  void removeEdgesFrom(const VertexPtr &pVertex);
192 
195  void removeExtraEdgesTo(const VertexPtr &cVertex);
196 
199  void removeExtraEdgesFrom(const VertexPtr &pVertex);
200 
202  void markVertexUnsorted(const VertexPtr &vertex);
203 
208  std::pair<unsigned int, unsigned int> prune(const VertexConstPtr &vertex);
209 
212  void resort();
213 
217  void finish();
218 
222  void reset();
224 
226  // Queue info:
229  bool vertexInsertCondition(const VertexPtr &state) const;
230 
233  bool edgeInsertCondition(const VertexPtrPair &edge) const;
234 
237  bool vertexPruneCondition(const VertexPtr &state) const;
238 
242  bool samplePruneCondition(const VertexPtr &state) const;
243 
247  bool edgePruneCondition(const VertexPtrPair &edge) const;
248 
250  unsigned int numEdges();
251 
254  unsigned int numVertices();
255 
258  unsigned int numEdgesTo(const VertexPtr &cVertex);
259 
262  unsigned int numEdgesFrom(const VertexPtr &pVertex);
263 
265  unsigned int numUnsorted() const;
266 
268  bool isSorted() const;
269 
272  bool isReset() const;
273 
277  bool isEmpty();
278 
280  bool isVertexExpanded(const VertexConstPtr &vertex) const;
281 
284  void getVertices(VertexConstPtrVector *vertexQueue);
285 
287  void getEdges(VertexConstPtrPairVector *edgeQueue);
289 
291  // Queue settings:
293  void setStrictQueueOrdering(bool beStrict);
294 
296  bool getStrictQueueOrdering() const;
297 
299  void setDelayedRewiring(bool delayRewiring);
300 
302  bool getDelayedRewiring() const;
303 
305  void setPruneDuringResort(bool prune);
306 
308  bool getPruneDuringResort() const;
310 
312  // Get the progress property counters
314  unsigned int numEdgesPopped() const;
316  private:
318  // High level primitives:
321  void updateQueue();
322 
324  void expandNextVertex();
325 
327  void expandVertex(const VertexPtr &vertex);
328 
331  void enqueueSamples(const VertexPtr &vertex, const VertexPtrVector& neighbourSamples, bool addAll);
332 
334  void enqueueVertices(const VertexPtr &vertex, const VertexPtrVector& neighbourVertices);
335 
337  void enqueueEdgeConditionally(const VertexPtr &parent, const VertexPtr &child);
338 
341  void processKNearest(const VertexConstPtr &vertex, VertexPtrVector *kNearSamples,
342  VertexPtrVector *kNearVertices);
344 
346  // Vertex helper functions:
348  void resortVertex(const VertexPtr &unorderedVertex);
349 
352  std::pair<unsigned int, unsigned int> pruneBranch(const VertexPtr &branchBase);
353 
356  void disconnectParent(const VertexPtr &oldVertex, bool cascadeCostUpdates);
357 
360  void vertexInsertHelper(const VertexPtr &newVertex, bool expandIfBeforeToken, bool removeFromFree,
361  bool addToNNStruct);
362 
365  unsigned int vertexRemoveHelper(const VertexPtr &oldVertex, bool fullyRemove);
367 
369  // Base-queue basic helper functions:
371  CostDouble vertexQueueValue(const VertexPtr &vertex) const;
372 
374  CostTriple edgeQueueValue(const VertexPtrPair &edge) const;
375 
378  template <std::size_t SIZE>
379  bool queueComparison(const std::array<ompl::base::Cost, SIZE> &lhs,
380  const std::array<ompl::base::Cost, SIZE> &rhs) const;
382 
384  // Debug
386  void assertSetup() const;
388 
390  // Variables -- Make sure every one is configured in setup() and reset in clear():
392  NameFunc nameFunc_;
393 
395  bool isSetup_{false};
396 
399  CostHelper *costHelpPtr_{nullptr};
400 
402  ImplicitGraph *graphPtr_{nullptr};
403 
405  VertexQueueAsMMap vertexQueue_;
406 
408  VertexQueueIter vertexToExpand_;
409 
411  EdgeQueueAsPairBinHeap edgeQueue_;
412 
414  VertexPtrVector resortVertices_;
415 
417  ompl::base::Cost solnCost_{std::numeric_limits<double>::infinity()};
418 
420  bool hasExactSolution_{false};
421 
423  unsigned int numQueueResets_{0u};
425 
427  // Informational variables - Make sure initialized in setup and reset in clear
430  unsigned int numEdgesPopped_{0u};
432 
434  // Parameters - Set defaults in construction/setup and DO NOT reset in clear.
436  bool useStrictQueueOrdering_{false};
437 
439  bool delayRewiring_{true};
440 
443  bool pruneDuringResort_{true};
445  }; // class: SearchQueue
446  } // geometric
447 } // ompl
448 #endif // OMPL_GEOMETRIC_PLANNERS_BITSTAR_DATASTRUCTURES_SEARCHQUEUE_
bool edgePruneCondition(const VertexPtrPair &edge) const
The condition used to prune edge (i.e., vertex-pair) out of the queue. Compares currentHeuristicEdge ...
void getEdges(VertexConstPtrPairVector *edgeQueue)
Get a copy of the edge queue. This is expensive and is only meant for animations/debugging.
bool getPruneDuringResort() const
Prune during resorts.
void markVertexUnsorted(const VertexPtr &vertex)
Mark the queue as requiring resorting downstream of the specified vertex.
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...
std::function< bool(const CostDouble &, const CostDouble &)> VertexQueueSortFunc
The function signature of the sorting function for the Vertex Queue.
Definition: SearchQueue.h:102
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...
bool edgeInsertCondition(const VertexPtrPair &edge) const
The condition used to insert edges into the queue. Compares lowerBoundHeuristicEdge to the given thre...
CostTriple frontEdgeValue()
Get the value of the best edge on the queue, leaving it at the front of the edge queue.
bool vertexInsertCondition(const VertexPtr &state) const
The condition used to insert vertices into the queue. Compares lowerBoundHeuristicVertex to the given...
std::pair< CostTriple, VertexPtrPair > CostTripleAndVertexPtrPair
The data stored in the edge-queue binary heap.
Definition: SearchQueue.h:114
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...
unsigned int numVertices()
Returns the number of vertices left to expand. This has nontrivial cost, as the token must be moved t...
void finish()
Finish the queue if it is sorted, if not resort the queue. Finishing the queue clears all the edge co...
A helper class to handle the various heuristic functions in one place.
Definition: CostHelper.h:69
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:100
std::vector< EdgeQueueElemPtr > EdgeQueueElemPtrVector
A vector of edge queue pointers.
Definition: SearchQueue.h:122
bool getStrictQueueOrdering() const
Get whether the queue stays strictly sorted with each rewiring.
void enqueueEdge(const VertexPtrPair &newEdge)
Insert an edge into the edge processing queue. The source vertex of this edge must be in the expansio...
bool samplePruneCondition(const VertexPtr &state) const
The condition used to prune disconnected samples from the free set. Compares lowerBoundHeuristicVerte...
bool isSorted() const
Return whether the queue is still sorted.
SearchQueue(NameFunc nameFunc)
Construct a search queue. It must be setup before use.
Definition: SearchQueue.cpp:75
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...
void setDelayedRewiring(bool delayRewiring)
Delay considering rewiring edges until an initial solution is found.
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...
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...
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void hasSolution(const ompl::base::Cost &solnCost)
Mark that a solution has been found.
bool getDelayedRewiring() const
Get whether BIT* is delaying rewiring until a solution is found.
std::array< ompl::base::Cost, 3u > CostTriple
A triplet of costs, i.e., the edge queue sorting key.
Definition: SearchQueue.h:112
VertexPtr frontVertex()
Get the best vertex on the queue, leaving it at the front of the vertex queue.
std::function< bool(const CostTripleAndVertexPtrPair &, const CostTripleAndVertexPtrPair &)> EdgeQueueSortFunc
The function signature of the sorting function for the Edge Queue.
Definition: SearchQueue.h:116
void setup(CostHelper *costHelpPtr, ImplicitGraph *graphPtr)
Setup the SearchQueue, must be called before use.
Definition: SearchQueue.cpp:89
unsigned int numUnsorted() const
Return the number of vertices marked as unsorted.
void removeEdgesFrom(const VertexPtr &pVertex)
Erase all edges in the edge queue that leave from the given vertex.
std::pair< VertexPtr, VertexPtr > VertexPtrPair
A pair of vertices, i.e., an edge.
Definition: BITstar.h:136
EdgeQueueAsPairBinHeap::Element * EdgeQueueElemPtr
An element pointer into the edge queue binary heap.
Definition: SearchQueue.h:120
A queue of edges to be processed that integrates both the expansion of Vertices and the ordering of t...
Definition: SearchQueue.h:92
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition: BITstar.h:147
bool isEmpty()
Returns true if the queue is empty. In the case where the edge queue is empty but the vertex queue is...
unsigned int numEdgesPopped() const
The number of edges popped off the queue for processing (numEdgesPopped_).
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers.
Definition: BITstar.h:130
void setStrictQueueOrdering(bool beStrict)
Set the queue to stay strictly sorted with each rewiring.
A conceptual representation of samples as an edge-implicit random geometric graph.
Definition: ImplicitGraph.h:64
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...
CostDouble frontVertexValue()
Get the value of the best vertex on the queue, leaving it at the front of the vertex queue...
VertexPtrPair popFrontEdge()
Pop the best edge off the queue, removing it from the front of the edge queue in the process...
void enqueueVertex(const VertexPtr &newVertex, bool removeFromFree)
Insert a vertex into the vertex expansion queue. Vertices remain in the vertex queue until pruned or ...
bool isVertexExpanded(const VertexConstPtr &vertex) const
Returns whether a given vertex has been expanded or not.
void removeEdgesTo(const VertexPtr &cVertex)
Erase all edges in the edge queue that lead to the given vertex.
VertexPtrPair frontEdge()
Get the best edge on the queue, leaving it at the front of the edge queue.
void reset()
Reset the queue, clearing all the edge containers and moving the vertex expansion token to the start...
ompl::BinaryHeap< CostTripleAndVertexPtrPair, EdgeQueueSortFunc > EdgeQueueAsPairBinHeap
The underlying edge queue. Using static keys for the same reason as the Vertex Queue.
Definition: SearchQueue.h:118
std::shared_ptr< Vertex > VertexPtr
A vertex shared pointer.
Definition: BITstar.h:121
void clear()
Clear the queue to the state of construction.
bool vertexPruneCondition(const VertexPtr &state) const
The condition used to prune vertices out of the queue. Compares currentHeuristicVertex to the given t...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
void setPruneDuringResort(bool prune)
Prune during resorts.
void resort()
Resort the queue around the marked unsorted vertices. If allowed, will remove any vertices that need ...
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...
std::multimap< CostDouble, VertexPtr, VertexQueueSortFunc > VertexQueueAsMMap
The underlying vertex queue. The advantage of a multimap over a multiset is that a copy of the key is...
Definition: SearchQueue.h:107
VertexQueueAsMMap::iterator VertexQueueIter
An iterator into the vertex queue multimap.
Definition: SearchQueue.h:109
unsigned int numEdges()
Returns the number of edges in the queue. Will resort/expand the queue if necessary.