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 
62 // BIT*:
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
65 // include any I use in my cpp (to avoid dependency loops).
66 #include "ompl/geometric/planners/bitstar/BITstar.h"
67 
68 namespace ompl
69 {
70  namespace geometric
71  {
91  {
92  public:
94  // Data alias declarations:
97  typedef std::array<ompl::base::Cost, 2u> CostDouble;
99  typedef std::array<ompl::base::Cost, 3u> CostTriple;
101 
103  // Function alias declarations:
105  typedef std::function<double(const VertexConstPtr &, const VertexConstPtr &)> DistanceFunc;
106 
108  typedef std::function<unsigned int(const VertexPtr &, VertexPtrVector *)> NeighbourhoodFunc;
110 
113  // Public functions:
115  SearchQueue(NameFunc nameFunc);
116 
117  virtual ~SearchQueue() = default;
118 
120  void setup(const CostHelperPtr &costHelpPtr, const ImplicitGraphPtr &graphPtr);
121 
123  void clear();
125 
127  // Insert and erase
131  void enqueueVertex(const VertexPtr &newVertex, bool removeFromFree);
132 
135  void enqueueEdge(const VertexPtrPair &newEdge);
136 
139  void enqueueEdge(const VertexPtr &sourceVertex, const VertexPtr &targetVertex);
140 
144  void unqueueVertex(const VertexPtr &oldVertex);
146 
148  // Access the queue
151 
154 
156  CostDouble frontVertexValue();
157 
159  CostTriple frontEdgeValue();
160 
162  void popFrontEdge(VertexPtrPair *bestEdge);
163 
167 
169  // Queue maintenance
171  void hasSolution(const ompl::base::Cost &solnCost);
172 
174  void removeEdgesTo(const VertexPtr &cVertex);
175 
177  void removeEdgesFrom(const VertexPtr &pVertex);
178 
181  void removeExtraEdgesTo(const VertexPtr &cVertex);
182 
185  void removeExtraEdgesFrom(const VertexPtr &pVertex);
186 
188  void markVertexUnsorted(const VertexPtr &vertex);
189 
194  std::pair<unsigned int, unsigned int> prune(const VertexConstPtr &goalVertexPtr);
195 
198  void resort();
199 
203  void finish();
204 
208  void reset();
210 
212  // Queue info:
215  bool vertexInsertCondition(const VertexPtr &state) const;
216 
219  bool edgeInsertCondition(const VertexPtrPair &edge) const;
220 
223  bool vertexPruneCondition(const VertexPtr &state) const;
224 
228  bool samplePruneCondition(const VertexPtr &state) const;
229 
233  bool edgePruneCondition(const VertexPtrPair &edge) const;
234 
236  unsigned int numEdges();
237 
240  unsigned int numVertices();
241 
244  unsigned int numEdgesTo(const VertexPtr &cVertex);
245 
248  unsigned int numEdgesFrom(const VertexPtr &pVertex);
249 
251  unsigned int numUnsorted() const;
252 
254  bool isSorted() const;
255 
258  bool isReset() const;
259 
263  bool isEmpty();
264 
266  bool isVertexExpanded(const VertexConstPtr &vertex) const;
267 
270  void getVertices(VertexConstPtrVector *vertexQueue);
271 
273  void getEdges(VertexConstPtrPairVector *edgeQueue);
275 
277  // Queue settings:
279  void setStrictQueueOrdering(bool beStrict);
280 
282  bool getStrictQueueOrdering() const;
283 
285  void setDelayedRewiring(bool delayRewiring);
286 
288  bool getDelayedRewiring() const;
289 
291  void setPruneDuringResort(bool prune);
292 
294  bool getPruneDuringResort() const;
296 
298  // Get the progress property counters
300  unsigned int numEdgesPopped() const;
302  private:
304  // Helpful alias declarations:
309  typedef std::multimap<CostDouble, VertexPtr, std::function<bool(const CostDouble &, const CostDouble &)>>
310  QValueToVertexMMap;
311 
314  typedef std::multimap<CostTriple, VertexPtrPair,
315  std::function<bool(const CostTriple &, const CostTriple &)>>
316  QValueToVertexPtrPairMMap;
317 
319  typedef QValueToVertexMMap::iterator VertexQueueIter;
320 
322  typedef std::unordered_map<BITstar::VertexId, VertexQueueIter> VertexIdToVertexQueueIterUMap;
323 
325  typedef QValueToVertexPtrPairMMap::iterator EdgeQueueIter;
326 
328  typedef std::vector<EdgeQueueIter> EdgeQueueIterVector;
329 
331  typedef std::unordered_map<BITstar::VertexId, EdgeQueueIterVector> VertexIdToEdgeQueueIterVectorUMap;
333 
335  // High level primitives:
338  void updateQueue();
339 
341  void expandNextVertex();
342 
344  void expandVertex(const VertexPtr &vertex);
345 
347  void enqueueEdgeConditionally(const VertexPtr &parent, const VertexPtr &child);
348 
351  void processKNearest(const VertexConstPtr &vertex, VertexPtrVector *kNearSamples,
352  VertexPtrVector *kNearVertices);
354 
356  // Vertex helper functions:
358  void reinsertVertex(const VertexPtr &unorderedVertex);
359 
362  std::pair<unsigned int, unsigned int> pruneBranch(const VertexPtr &branchBase);
363 
366  void disconnectParent(const VertexPtr &oldVertex, bool cascadeCostUpdates);
367 
370  void vertexInsertHelper(const VertexPtr &newVertex, bool expandIfBeforeToken, bool removeFromFree,
371  bool addToNNStruct);
372 
375  unsigned int vertexRemoveHelper(const VertexPtr &oldVertex, bool fullyRemove);
377 
379  // Edge helper functions:
382  void edgeInsertHelper(const VertexPtrPair &newEdge, EdgeQueueIter positionHint);
383 
385  void edgeRemoveHelper(const EdgeQueueIter &oldEdgeIter, bool rmIncomingLookup, bool rmOutgoingLookup);
386 
388  void rmEdgeLookupHelper(VertexIdToEdgeQueueIterVectorUMap &lookup, const BITstar::VertexId &idx,
389  const EdgeQueueIter &mmapIterToRm);
391 
393  // Base-queue basic helper functions:
395  CostDouble vertexQueueValue(const VertexPtr &vertex) const;
396 
398  CostTriple edgeQueueValue(const VertexPtrPair &edge) const;
399 
402  template <std::size_t SIZE>
403  bool queueComparison(const std::array<ompl::base::Cost, SIZE> &lhs,
404  const std::array<ompl::base::Cost, SIZE> &rhs) const;
406 
408  // Debug
410  void confirmSetup() const;
412 
414  // Variables -- Make sure every one is configured in setup() and reset in clear():
416  NameFunc nameFunc_;
417 
419  bool isSetup_{false};
420 
423  CostHelperPtr costHelpPtr_{nullptr};
424 
426  ImplicitGraphPtr graphPtr_{nullptr};
427 
429  QValueToVertexMMap vertexQueue_;
430 
432  VertexQueueIter vertexToExpand_;
433 
435  QValueToVertexPtrPairMMap edgeQueue_;
436 
438  VertexIdToVertexQueueIterUMap vertexIterLookup_;
439 
441  VertexIdToEdgeQueueIterVectorUMap outgoingEdges_;
442 
444  VertexIdToEdgeQueueIterVectorUMap incomingEdges_;
445 
447  VertexPtrVector resortVertices_;
448 
450  ompl::base::Cost costThreshold_{std::numeric_limits<double>::infinity()};
451 
453  bool hasExactSolution_{false};
455 
457  // Informational variables - Make sure initialized in setup and reset in clear
460  unsigned int numEdgesPopped_{0u};
462 
464  // Parameters - Set defaults in construction/setup and DO NOT reset in clear.
466  bool useStrictQueueOrdering_{false};
467 
469  bool delayRewiring_{true};
470 
473  bool pruneDuringResort_{true};
475  }; // class: SearchQueue
476  } // geometric
477 } // ompl
478 #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...
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...
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...
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...
void finish()
Finish the queue if it is sorted, if not resort the queue. Finishing the queue clears all the edge co...
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.
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: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...
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.
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
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.
unsigned int numUnsorted() const
Return the number of vertices marked as unsorted.
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.
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...
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.
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.
std::shared_ptr< CostHelper > CostHelperPtr
A cost helper shared pointer.
Definition: BITstar.h:146
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...
std::shared_ptr< Vertex > VertexPtr
A vertex shared pointer.
Definition: BITstar.h:121
std::function< unsigned int(const VertexPtr &, VertexPtrVector *)> NeighbourhoodFunc
A std::function definition for the neighbourhood of a vertex .
Definition: SearchQueue.h:108
void clear()
Clear the queue to the state of construction.
Definition: SearchQueue.cpp:92
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.
unsigned int VertexId
The vertex id type.
Definition: BITstar.h:134
void resort()
Resort the queue around the marked unsorted vertices. If allowed, will simply remove any vertices tha...
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...
unsigned int numEdges()
Returns the number of edges in the queue. Will resort/expand the queue if necessary.