Loading...
Searching...
No Matches
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
53namespace 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
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_
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
A helper class to handle the various heuristic functions in one place.
Definition CostHelper.h:70
A conceptual representation of samples as an edge-implicit random geometric graph.
A queue of edges, sorted according to a sort key.
Definition SearchQueue.h:65
void reset()
Reset the queue to the state of construction.
EdgeQueue::Element * EdgeQueueElemPtr
An element pointer into the edge queue binary heap.
Definition SearchQueue.h:84
VertexPtrPair popFrontEdge()
Pop the best edge off the queue, removing it from the front of the edge queue in the process.
void insertOutgoingEdges(const VertexPtr &vertex)
Update the edge queue by adding all the potential edges from the vertex to nearby states.
std::function< bool(const SortKeyAndVertexPtrPair &, const SortKeyAndVertexPtrPair &)> EdgeComparisonFunction
The function signature of the sorting function for the Edge Queue.
Definition SearchQueue.h:78
virtual ~SearchQueue()=default
Destruct the search queue using the default deconstructor.
double getInflationFactor() const
Get the cost-to-go inflation factor.
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.
bool isEmpty()
Returns true if the queue is empty. In the case where the edge queue is empty but the vertex queue is...
std::array< ompl::base::Cost, 3u > SortKey
A triplet of costs, i.e., the edge queue sorting key.
Definition SearchQueue.h:72
VertexPtrPair getFrontEdge()
Get 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.
ompl::BinaryHeap< SortKeyAndVertexPtrPair, EdgeComparisonFunction > EdgeQueue
The underlying edge queue. Using static keys for the same reason as the Vertex Queue.
Definition SearchQueue.h:81
bool canPossiblyImproveCurrentSolution(const VertexPtr &state) const
The condition used to insert vertices into the queue. Compares lowerBoundHeuristicVertex to the given...
std::shared_ptr< const unsigned int > getSearchId() const
Allow access to the current search id.
void clear()
Finish the queue if it is sorted, if not resort the queue. Finishing the queue clears all the edge co...
void insertOutgoingEdgesOfStartVertices()
Insert the outgoing edges of all start vertices.
unsigned int numEdges()
Returns the number of edges in the queue. Will resort/expand the queue if necessary.
void insertOutgoingEdgesOfInconsistentVertices()
Insert the outgoing edges of all inconsistent vertices.
void registerSolutionCost(const ompl::base::Cost &solutionCost)
Mark that a solution has been found.
SortKey getFrontEdgeValue()
Get the value of the best edge on the queue, leaving it at the front of the edge queue.
void addToInconsistentSet(const VertexPtr &vertex)
Add a vertex to the set of inconsistent vertices.
void removeFromInconsistentSet(const VertexPtr &vertex)
Remove a vertex from the set of inconsistent vertices.
void removeOutEdgesConnectedToVertexFromQueue(const VertexPtr &vertex)
Erase all edges in the edge queue that leave from the given vertex.
unsigned int numEdgesPopped() const
The number of edges popped off the queue for processing (numEdgesPopped_).
void setInflationFactor(double factor)
Set the cost-to-go inflation factor.
std::vector< EdgeQueueElemPtr > EdgeQueueElemPtrVector
A vector of edge queue pointers.
Definition SearchQueue.h:87
void removeAllEdgesConnectedToVertexFromQueue(const VertexPtr &vertex)
Erase all edges in the edge queue that are connected to the given vertex.
void getEdges(VertexConstPtrPairVector *edgeQueue)
Get a copy of the edge queue. This is expensive and is only meant for animations/debugging.
void update(const EdgeQueueElemPtr elementPtr)
Update the sort key of a particular edge and its position in the queue.
std::pair< SortKey, VertexPtrPair > SortKeyAndVertexPtrPair
The data stored in the edge-queue binary heap.
Definition SearchQueue.h:75
void clearInconsistentSet()
Clear the set of inconsistent vertices.
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers to vertices.
Definition BITstar.h:125
std::pair< VertexPtr, VertexPtr > VertexPtrPair
A pair of vertices, i.e., an edge.
Definition BITstar.h:134
std::shared_ptr< Vertex > VertexPtr
A shared pointer to a vertex.
Definition BITstar.h:116
void setup() override
Setup the algorithm.
Definition BITstar.cpp:175
std::vector< VertexConstPtrPair > VertexConstPtrPairVector
A vector of pairs of const vertices, i.e., a vector of edges.
Definition BITstar.h:143
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition BITstar.h:149
Main namespace. Contains everything in this library.