AITstar.h
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, University of Oxford
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: Marlin Strub
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_AITSTAR_
38 #define OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_AITSTAR_
39 
40 #include <algorithm>
41 #include <memory>
42 
43 #include "ompl/base/Planner.h"
44 #include "ompl/datastructures/BinaryHeap.h"
45 #include "ompl/geometric/PathGeometric.h"
46 #include "ompl/geometric/planners/informedtrees/aitstar/Edge.h"
47 #include "ompl/geometric/planners/informedtrees/aitstar/ImplicitGraph.h"
48 #include "ompl/geometric/planners/informedtrees/aitstar/Vertex.h"
49 
50 namespace ompl
51 {
52  namespace geometric
53  {
87  class AITstar : public ompl::base::Planner
88  {
89  public:
91  AITstar(const ompl::base::SpaceInformationPtr &spaceInformation);
92 
94  ~AITstar() = default;
95 
97  void setup() override;
98 
100  void clear() override;
101 
104  solve(const ompl::base::PlannerTerminationCondition &terminationCondition) override;
105 
107  ompl::base::Cost bestCost() const;
108 
110  void getPlannerData(base::PlannerData &data) const override;
111 
113  void setBatchSize(std::size_t batchSize);
114 
116  std::size_t getBatchSize() const;
117 
119  void setRewireFactor(double rewireFactor);
120 
122  double getRewireFactor() const;
123 
125  void trackApproximateSolutions(bool track);
126 
128  bool areApproximateSolutionsTracked() const;
129 
131  void enablePruning(bool prune);
132 
134  bool isPruningEnabled() const;
135 
137  void setUseKNearest(bool useKNearest);
138 
140  bool getUseKNearest() const;
141 
143  void setRepairReverseSearch(bool repairReverseSearch);
144 
146  std::vector<aitstar::Edge> getEdgesInQueue() const;
147 
149  std::vector<std::shared_ptr<aitstar::Vertex>> getVerticesInQueue() const;
150 
152  aitstar::Edge getNextEdgeInQueue() const;
153 
155  std::shared_ptr<aitstar::Vertex> getNextVertexInQueue() const;
156 
158  std::vector<std::shared_ptr<aitstar::Vertex>> getVerticesInReverseSearchTree() const;
159 
160  private:
162  void iterate();
163 
165  void performForwardSearchIteration();
166 
168  void performReverseSearchIteration();
169 
171  void reverseSearchUpdateVertex(const std::shared_ptr<aitstar::Vertex> &vertex);
172 
174  void insertOrUpdateInForwardQueue(const aitstar::Edge &edge);
175 
177  void insertOrUpdateInReverseQueue(const std::shared_ptr<aitstar::Vertex> &vertex);
178 
180  void rebuildForwardQueue();
181 
183  void rebuildReverseQueue();
184 
186  void informAboutNewSolution() const;
187 
189  void informAboutPlannerStatus(ompl::base::PlannerStatus::StatusType status) const;
190 
192  std::shared_ptr<ompl::geometric::PathGeometric>
193  getPathToVertex(const std::shared_ptr<aitstar::Vertex> &vertex) const;
194 
196  std::array<ompl::base::Cost, 3u> computeSortKey(const std::shared_ptr<aitstar::Vertex> &parent,
197  const std::shared_ptr<aitstar::Vertex> &child) const;
198 
200  std::array<ompl::base::Cost, 2u> computeSortKey(const std::shared_ptr<aitstar::Vertex> &vertex) const;
201 
203  void insertOutgoingEdges(const std::shared_ptr<aitstar::Vertex> &vertex);
204 
206  std::vector<aitstar::Edge> getOutgoingEdges(const std::shared_ptr<aitstar::Vertex> &vertex) const;
207 
209  bool haveAllVerticesBeenProcessed(const std::vector<aitstar::Edge> &edges) const;
210 
213  bool haveAllVerticesBeenProcessed(const aitstar::Edge &edges) const;
214 
217  void updateExactSolution();
218 
221  void updateApproximateSolution(const std::shared_ptr<aitstar::Vertex> &vertex);
222 
224  ompl::base::Cost computeCostToGoToStartHeuristic(const std::shared_ptr<aitstar::Vertex> &vertex) const;
225 
227  ompl::base::Cost computeCostToGoToGoalHeuristic(const std::shared_ptr<aitstar::Vertex> &vertex) const;
228 
230  ompl::base::Cost computeCostToGoToGoal(const std::shared_ptr<aitstar::Vertex> &vertex) const;
231 
233  ompl::base::Cost computeBestCostToComeFromGoalOfAnyStart() const;
234 
236  std::size_t countNumVerticesInForwardTree() const;
237 
239  std::size_t countNumVerticesInReverseTree() const;
240 
243  void invalidateCostToComeFromGoalOfReverseBranch(const std::shared_ptr<aitstar::Vertex> &vertex);
244 
246  ompl::base::Cost solutionCost_;
247 
249  ompl::base::Cost approximateSolutionCost_{};
250 
252  ompl::base::Cost approximateSolutionCostToGoal_{};
253 
255  aitstar::ImplicitGraph graph_;
256 
258  using EdgeQueue =
259  ompl::BinaryHeap<aitstar::Edge, std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>;
260 
262  EdgeQueue forwardQueue_;
263 
265  using KeyVertexPair = std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>>;
266 
268  using VertexQueue =
269  ompl::BinaryHeap<KeyVertexPair, std::function<bool(const KeyVertexPair &, const KeyVertexPair &)>>;
270 
272  VertexQueue reverseQueue_;
273 
275  std::vector<aitstar::Edge> edgesToBeInserted_{};
276 
278  std::size_t numIterations_{0u};
279 
281  std::size_t batchSize_{100u};
282 
284  bool performReverseSearchIteration_{true};
285 
287  bool isForwardSearchStartedOnBatch_{false};
288 
290  bool forwardQueueMustBeRebuilt_{false};
291 
293  bool repairReverseSearch_{true};
294 
296  bool trackApproximateSolutions_{true};
297 
299  bool isPruningEnabled_{true};
300 
303 
305  ompl::base::MotionValidatorPtr motionValidator_;
306 
308  std::size_t numProcessedEdges_{0u};
309 
311  std::size_t numEdgeCollisionChecks_{0u};
312  };
313  } // namespace geometric
314 } // namespace ompl
315 
316 #endif // OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_AITSTAR
void clear() override
Clears the algorithm's internal state.
Definition: AITstar.cpp:155
Base class for a planner.
Definition: Planner.h:287
A shared pointer wrapper for ompl::base::SpaceInformation.
AITstar(const ompl::base::SpaceInformationPtr &spaceInformation)
Constructs a AIT*.
Definition: AITstar.cpp:53
void setBatchSize(std::size_t batchSize)
Set the batch size.
Definition: AITstar.cpp:301
std::vector< aitstar::Edge > getEdgesInQueue() const
Get the edge queue.
Definition: AITstar.cpp:497
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:112
aitstar::Edge getNextEdgeInQueue() const
Get the next edge in the queue.
Definition: AITstar.cpp:519
A shared pointer wrapper for ompl::base::OptimizationObjective.
This class provides an implementation of an updatable min-heap. Using it is a bit cumbersome,...
Definition: BinaryHeap.h:85
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
A class to store the exit status of Planner::solve()
bool areApproximateSolutionsTracked() const
Get whether approximate solutions are tracked.
Definition: AITstar.cpp:334
~AITstar()=default
Destructs a AIT*.
void enablePruning(bool prune)
Set whether pruning is enabled or not.
Definition: AITstar.cpp:339
void setRepairReverseSearch(bool repairReverseSearch)
Enable LPA* repair of reverse search.
Definition: AITstar.cpp:359
void getPlannerData(base::PlannerData &data) const override
Get the planner data.
Definition: AITstar.cpp:256
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInReverseSearchTree() const
Get the vertices in the reverse search tree.
Definition: AITstar.cpp:539
double getRewireFactor() const
Get the rewire factor of the RGG graph.
Definition: AITstar.cpp:316
A shared pointer wrapper for ompl::base::MotionValidator.
std::shared_ptr< aitstar::Vertex > getNextVertexInQueue() const
Get the next vertex in the queue.
Definition: AITstar.cpp:529
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInQueue() const
Get the vertex queue.
Definition: AITstar.cpp:504
bool isPruningEnabled() const
Get whether pruning is enabled or not.
Definition: AITstar.cpp:344
StatusType
The possible values of the status returned by a planner.
std::size_t getBatchSize() const
Get the batch size.
Definition: AITstar.cpp:306
bool getUseKNearest() const
Get whether to use a k-nearest RGG connection model. If false, AIT* uses an r-disc model.
Definition: AITstar.cpp:354
ompl::base::Cost bestCost() const
Get the cost of the incumbent solution.
Definition: AITstar.cpp:251
void setup() override
Additional setup that can only be done once a problem definition is set.
Definition: AITstar.cpp:104
ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &terminationCondition) override
Solves a motion planning problem.
Definition: AITstar.cpp:171
void setUseKNearest(bool useKNearest)
Set whether to use a k-nearest RGG connection model. If false, AIT* uses an r-disc model.
Definition: AITstar.cpp:349
void setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG graph.
Definition: AITstar.cpp:311
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
void trackApproximateSolutions(bool track)
Set whether to track approximate solutions.
Definition: AITstar.cpp:321