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, Marlin Strub */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_SEARCHQUEUE_
38 #define OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_SEARCHQUEUE_
39 
40 #include <array>
41 #include <functional>
42 #include <map>
43 #include <unordered_map>
44 #include <utility>
45 #include <vector>
46 
47 #include "ompl/base/Cost.h"
48 #include "ompl/base/OptimizationObjective.h"
49 #include "ompl/datastructures/BinaryHeap.h"
50 #include "ompl/datastructures/NearestNeighbors.h"
51 #include "ompl/geometric/planners/informedtrees/BITstar.h"
52 
53 namespace ompl
54 {
55  namespace geometric
56  {
65  {
66  public:
67  // ---
68  // Aliases.
69  // ---
70 
72  using SortKey = std::array<ompl::base::Cost, 3u>;
73 
75  using SortKeyAndVertexPtrPair = std::pair<SortKey, VertexPtrPair>;
76 
78  using EdgeComparisonFunction = std::function<bool(const SortKeyAndVertexPtrPair &, const SortKeyAndVertexPtrPair &)>;
79 
82 
84  using EdgeQueueElemPtr = EdgeQueue::Element*;
85 
87  using EdgeQueueElemPtrVector = std::vector<EdgeQueueElemPtr>;
88 
89  // ---
90  // Construction, initialization, and destruction.
91  // ---
92 
94  SearchQueue(NameFunc nameFunc);
95 
97  virtual ~SearchQueue() = default;
98 
100  void setup(CostHelper *costHelpPtr, ImplicitGraph *graphPtr);
101 
103  void reset();
104 
106  void enableCascadingRewirings(bool enable);
107 
108  // ---
109  // Insertion.
110  // ---
111 
113  void insertOutgoingEdges(const VertexPtr &vertex);
114 
117 
120 
122  void addToInconsistentSet(const VertexPtr &vertex);
123 
124  // ---
125  // Access.
126  // ---
127 
130 
133 
136 
137  // ---
138  // Modification.
139  // ---
140 
144  void clear();
145 
147  void clearInconsistentSet();
148 
150  void rebuildEdgeQueue();
151 
153  void update(const EdgeQueueElemPtr elementPtr);
154 
156  void setInflationFactor(double factor);
157 
159  void registerSolutionCost(const ompl::base::Cost &solutionCost);
160 
163 
166 
169 
171  void removeFromInconsistentSet(const VertexPtr &vertex);
172 
173  // ---
174  // Information access.
175  // ---
176 
178  double getInflationFactor() const;
179 
181  std::shared_ptr<const unsigned int> getSearchId() const;
182 
185  bool canPossiblyImproveCurrentSolution(const VertexPtr &state) const;
186 
189  bool canPossiblyImproveCurrentSolution(const VertexPtrPair &edge) const;
190 
192  unsigned int numEdges();
193 
197  bool isEmpty();
198 
200  void getEdges(VertexConstPtrPairVector *edgeQueue);
201 
203  unsigned int numEdgesPopped() const;
204 
205  private:
206  // ---
207  // High level primitives.
208  // ---
209 
211  void enqueueEdges(const VertexPtr &parent, const VertexPtrVector &possibleChildren);
212 
214  void enqueueEdgeConditionally(const VertexPtr &parent, const VertexPtr &child);
215 
218  void enqueueEdge(const VertexPtrPair &edge);
219 
220  // ---
221  // Sorting.
222  // ---
223 
225  SortKey createSortKey(const VertexPtrPair &edge) const;
226 
229  bool lexicographicalBetterThan(const std::array<ompl::base::Cost, 3> &lhs,
230  const std::array<ompl::base::Cost, 3> &rhs) const;
231 
232  // ---
233  // Debug helpers.
234  // ---
235 
237  void assertSetup() const;
238 
239  // ---
240  // Member variables (Make all are configured in setup() and reset in reset()).
241  // ---
242 
244  NameFunc nameFunc_;
245 
247  bool isSetup_{false};
248 
250  bool isCascadingOfRewiringsEnabled_{false};
251 
254  CostHelper *costHelpPtr_{nullptr};
255 
257  ImplicitGraph *graphPtr_{nullptr};
258 
260  EdgeQueue edgeQueue_;
261 
263  VertexPtrVector inconsistentVertices_;
264 
266  double inflationFactor_{1.0};
267 
269  ompl::base::Cost solutionCost_{std::numeric_limits<double>::infinity()};
270 
272  bool hasExactSolution_{false};
273 
275  const std::shared_ptr<unsigned int> searchId_;
276 
279  unsigned int numEdgesPopped_{0u};
280 
281  }; // class SearchQueue
282  } // namespace geometric
283 } // namespace ompl
284 #endif // OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_SEARCHQUEUE_
std::function< bool(const SortKeyAndVertexPtrPair &, const SortKeyAndVertexPtrPair &)> EdgeComparisonFunction
The function signature of the sorting function for the Edge Queue.
Definition: SearchQueue.h:78
void insertOutgoingEdgesOfInconsistentVertices()
Insert the outgoing edges of all inconsistent vertices.
SortKey getFrontEdgeValue()
Get the value of the best edge on the queue, leaving it at the front of the edge queue.
void removeInEdgesConnectedToVertexFromQueue(const VertexPtr &vertex)
Erase all edges in the edge queue that lead to the given vertex.
void clear()
Finish the queue if it is sorted, if not resort the queue. Finishing the queue clears all the edge co...
A queue of edges, sorted according to a sort key.
Definition: SearchQueue.h:65
unsigned int numEdgesPopped() const
The number of edges popped off the queue for processing (numEdgesPopped_).
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition: BITstar.h:149
VertexPtrPair popFrontEdge()
Pop the best edge off the queue, removing it from the front of the edge queue in the process.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
EdgeQueue::Element * EdgeQueueElemPtr
An element pointer into the edge queue binary heap.
Definition: SearchQueue.h:84
void getEdges(VertexConstPtrPairVector *edgeQueue)
Get a copy of the edge queue. This is expensive and is only meant for animations/debugging.
std::pair< VertexPtr, VertexPtr > VertexPtrPair
A pair of vertices, i.e., an edge.
Definition: BITstar.h:134
std::shared_ptr< const unsigned int > getSearchId() const
Allow access to the current search id.
A helper class to handle the various heuristic functions in one place.
Definition: CostHelper.h:70
bool canPossiblyImproveCurrentSolution(const VertexPtr &state) const
The condition used to insert vertices into the queue. Compares lowerBoundHeuristicVertex to the given...
void removeOutEdgesConnectedToVertexFromQueue(const VertexPtr &vertex)
Erase all edges in the edge queue that leave from the given vertex.
void enableCascadingRewirings(bool enable)
Set whether cascading of rewirings is enabled.
void rebuildEdgeQueue()
Update all the sort keys of the edges in the queue and resort.
std::pair< SortKey, VertexPtrPair > SortKeyAndVertexPtrPair
The data stored in the edge-queue binary heap.
Definition: SearchQueue.h:75
bool isEmpty()
Returns true if the queue is empty. In the case where the edge queue is empty but the vertex queue is...
ompl::BinaryHeap< SortKeyAndVertexPtrPair, EdgeComparisonFunction > EdgeQueue
The underlying edge queue. Using static keys for the same reason as the Vertex Queue.
Definition: SearchQueue.h:81
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers to vertices.
Definition: BITstar.h:125
SearchQueue(NameFunc nameFunc)
Construct a search queue. It must be setup before use.
Definition: SearchQueue.cpp:79
void clearInconsistentSet()
Clear the set of inconsistent vertices.
void setInflationFactor(double factor)
Set the cost-to-go inflation factor.
unsigned int numEdges()
Returns the number of edges in the queue. Will resort/expand the queue if necessary.
std::shared_ptr< Vertex > VertexPtr
A shared pointer to a vertex.
Definition: BITstar.h:116
std::array< ompl::base::Cost, 3u > SortKey
A triplet of costs, i.e., the edge queue sorting key.
Definition: SearchQueue.h:72
void reset()
Reset the queue to the state of construction.
void registerSolutionCost(const ompl::base::Cost &solutionCost)
Mark that a solution has been found.
double getInflationFactor() const
Get the cost-to-go inflation factor.
A conceptual representation of samples as an edge-implicit random geometric graph.
Definition: ImplicitGraph.h:57
virtual ~SearchQueue()=default
Destruct the search queue using the default deconstructor.
std::vector< VertexConstPtrPair > VertexConstPtrPairVector
A vector of pairs of const vertices, i.e., a vector of edges.
Definition: BITstar.h:143
void addToInconsistentSet(const VertexPtr &vertex)
Add a vertex to the set of inconsistent vertices.
VertexPtrPair getFrontEdge()
Get the best edge on the queue, leaving it at the front of the edge queue.
void setup(CostHelper *costHelpPtr, ImplicitGraph *graphPtr)
Setup the SearchQueue, must be called before use.
Definition: SearchQueue.cpp:88
void removeAllEdgesConnectedToVertexFromQueue(const VertexPtr &vertex)
Erase all edges in the edge queue that are connected to the given vertex.
void insertOutgoingEdges(const VertexPtr &vertex)
Update the edge queue by adding all the potential edges from the vertex to nearby states.
std::vector< EdgeQueueElemPtr > EdgeQueueElemPtrVector
A vector of edge queue pointers.
Definition: SearchQueue.h:87
void removeFromInconsistentSet(const VertexPtr &vertex)
Remove a vertex from the set of inconsistent vertices.
void update(const EdgeQueueElemPtr elementPtr)
Update the sort key of a particular edge and its position in the queue.
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
void insertOutgoingEdgesOfStartVertices()
Insert the outgoing edges of all start vertices.