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 names of the copyright holders 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/geometric/PathGeometric.h"
45 #include "ompl/geometric/planners/informedtrees/aitstar/Edge.h"
46 #include "ompl/geometric/planners/informedtrees/aitstar/ImplicitGraph.h"
47 #include "ompl/geometric/planners/informedtrees/aitstar/Vertex.h"
48 #include "ompl/geometric/planners/informedtrees/aitstar/Queuetypes.h"
49 
50 namespace ompl
51 {
52  namespace geometric
53  {
87  class AITstar : public ompl::base::Planner
88  {
89  public:
91  explicit AITstar(const ompl::base::SpaceInformationPtr &spaceInformation);
92 
94  ~AITstar() = default;
95 
97  void setup() override;
98 
101 
105 
107  void clear() override;
108 
111  solve(const ompl::base::PlannerTerminationCondition &terminationCondition) override;
112 
114  ompl::base::Cost bestCost() const;
115 
117  void getPlannerData(base::PlannerData &data) const override;
118 
120  void setBatchSize(std::size_t batchSize);
121 
123  std::size_t getBatchSize() const;
124 
126  void setRewireFactor(double rewireFactor);
127 
129  double getRewireFactor() const;
130 
132  void trackApproximateSolutions(bool track);
133 
135  bool areApproximateSolutionsTracked() const;
136 
138  void enablePruning(bool prune);
139 
141  bool isPruningEnabled() const;
142 
144  void setUseKNearest(bool useKNearest);
145 
147  bool getUseKNearest() const;
148 
150  void setMaxNumberOfGoals(unsigned int numberOfGoals);
151 
153  unsigned int getMaxNumberOfGoals() const;
154 
156  std::vector<aitstar::Edge> getEdgesInQueue() const;
157 
159  std::vector<std::shared_ptr<aitstar::Vertex>> getVerticesInQueue() const;
160 
163 
165  std::shared_ptr<aitstar::Vertex> getNextVertexInQueue() const;
166 
168  std::vector<std::shared_ptr<aitstar::Vertex>> getVerticesInReverseSearchTree() const;
169 
170  private:
172  void iterate(const ompl::base::PlannerTerminationCondition &terminationCondition);
173 
175  void iterateForwardSearch();
176 
178  void iterateReverseSearch();
179 
181  void updateReverseSearchVertex(const std::shared_ptr<aitstar::Vertex> &vertex);
182 
184  void updateReverseSearchNeighbors(const std::shared_ptr<aitstar::Vertex> &vertex);
185 
187  void insertOrUpdateInForwardQueue(const aitstar::Edge &edge);
188 
190  void insertOrUpdateInForwardQueue(const std::vector<aitstar::Edge> &edges);
191 
193  void insertOrUpdateInReverseQueue(const std::shared_ptr<aitstar::Vertex> &vertex);
194 
196  void rebuildForwardQueue();
197 
199  void rebuildReverseQueue();
200 
202  void clearForwardQueue();
203 
205  void clearReverseQueue();
206 
208  void informAboutNewSolution() const;
209 
211  void informAboutPlannerStatus(ompl::base::PlannerStatus::StatusType status) const;
212 
214  void insertGoalVerticesInReverseQueue();
215 
217  void expandStartVerticesIntoForwardQueue();
218 
220  bool continueReverseSearch() const;
221 
223  bool continueForwardSearch();
224 
226  std::shared_ptr<ompl::geometric::PathGeometric>
227  getPathToVertex(const std::shared_ptr<aitstar::Vertex> &vertex) const;
228 
230  std::array<ompl::base::Cost, 3u> computeSortKey(const std::shared_ptr<aitstar::Vertex> &parent,
231  const std::shared_ptr<aitstar::Vertex> &child) const;
232 
234  std::array<ompl::base::Cost, 2u> computeSortKey(const std::shared_ptr<aitstar::Vertex> &vertex) const;
235 
237  void insertOutgoingEdges(const std::shared_ptr<aitstar::Vertex> &vertex);
238 
240  std::vector<aitstar::Edge> getOutgoingEdges(const std::shared_ptr<aitstar::Vertex> &vertex) const;
241 
244  void updateExactSolution();
245 
248  void updateApproximateSolution(const std::shared_ptr<aitstar::Vertex> &vertex);
249 
251  void updateApproximateSolution();
252 
254  ompl::base::PlannerStatus::StatusType updateSolution();
255 
257  ompl::base::PlannerStatus::StatusType updateSolution(const std::shared_ptr<aitstar::Vertex> &vertex);
258 
260  ompl::base::Cost computeCostToGoToStartHeuristic(const std::shared_ptr<aitstar::Vertex> &vertex) const;
261 
263  ompl::base::Cost computeCostToGoToGoalHeuristic(const std::shared_ptr<aitstar::Vertex> &vertex) const;
264 
266  ompl::base::Cost computeCostToGoToGoal(const std::shared_ptr<aitstar::Vertex> &vertex) const;
267 
269  ompl::base::Cost computeBestCostToComeFromGoalOfAnyStart() const;
270 
272  std::size_t countNumVerticesInForwardTree() const;
273 
275  std::size_t countNumVerticesInReverseTree() const;
276 
279  void invalidateCostToComeFromGoalOfReverseBranch(const std::shared_ptr<aitstar::Vertex> &vertex);
280 
282  ompl::base::Cost solutionCost_;
283 
285  ompl::base::Cost approximateSolutionCost_{};
286 
288  ompl::base::Cost approximateSolutionCostToGoal_{};
289 
291  aitstar::ImplicitGraph graph_;
292 
294  aitstar::EdgeQueue forwardQueue_;
295 
297  aitstar::VertexQueue reverseQueue_;
298 
300  bool isEdgeBetter(const aitstar::Edge &lhs, const aitstar::Edge &rhs) const;
301 
303  bool isVertexBetter(const aitstar::KeyVertexPair &lhs, const aitstar::KeyVertexPair &rhs) const;
304 
306  std::size_t numInconsistentOrUnconnectedTargets_{0u};
307 
309  std::size_t numIterations_{0u};
310 
312  std::size_t batchSize_{100u};
313 
315  bool trackApproximateSolutions_{true};
316 
318  bool isPruningEnabled_{true};
319 
322 
324  ompl::base::MotionValidatorPtr motionValidator_;
325 
327  std::size_t numProcessedEdges_{0u};
328 
330  std::size_t numEdgeCollisionChecks_{0u};
331  };
332  } // namespace geometric
333 } // namespace ompl
334 
335 #endif // OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_AITSTAR
void clear() override
Clears the algorithm's internal state.
Definition: AITstar.cpp:201
Base class for a planner.
Definition: Planner.h:279
ompl::base::PlannerStatus::StatusType ensureSetup()
Checks whether the planner is successfully setup.
Definition: AITstar.cpp:145
A shared pointer wrapper for ompl::base::SpaceInformation.
std::vector< aitstar::Edge > getEdgesInQueue() const
Get the edge queue.
Definition: AITstar.cpp:592
AITstar(const ompl::base::SpaceInformationPtr &spaceInformation)
Constructs a AIT*.
Definition: AITstar.cpp:55
aitstar::Edge getNextEdgeInQueue() const
Get the next edge in the queue.
Definition: AITstar.cpp:614
void setBatchSize(std::size_t batchSize)
Set the batch size.
Definition: AITstar.cpp:303
void setMaxNumberOfGoals(unsigned int numberOfGoals)
Set the maximum number of goals AIT* will sample from sampleable goal regions.
Definition: AITstar.cpp:361
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
A shared pointer wrapper for ompl::base::OptimizationObjective.
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInQueue() const
Get the vertex queue.
Definition: AITstar.cpp:599
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
ompl::base::PlannerStatus::StatusType ensureStartAndGoalStates(const ompl::base::PlannerTerminationCondition &terminationCondition)
Checks whether the problem is successfully setup.
Definition: AITstar.cpp:169
std::shared_ptr< aitstar::Vertex > getNextVertexInQueue() const
Get the next vertex in the queue.
Definition: AITstar.cpp:624
A class to store the exit status of Planner::solve()
bool areApproximateSolutionsTracked() const
Get whether approximate solutions are tracked.
Definition: AITstar.cpp:336
~AITstar()=default
Destructs a AIT*.
void enablePruning(bool prune)
Set whether pruning is enabled or not.
Definition: AITstar.cpp:341
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInReverseSearchTree() const
Get the vertices in the reverse search tree.
Definition: AITstar.cpp:634
void getPlannerData(base::PlannerData &data) const override
Get the planner data.
Definition: AITstar.cpp:259
double getRewireFactor() const
Get the rewire factor of the RGG graph.
Definition: AITstar.cpp:318
A shared pointer wrapper for ompl::base::MotionValidator.
bool isPruningEnabled() const
Get whether pruning is enabled or not.
Definition: AITstar.cpp:346
StatusType
The possible values of the status returned by a planner.
std::size_t getBatchSize() const
Get the batch size.
Definition: AITstar.cpp:308
bool getUseKNearest() const
Get whether to use a k-nearest RGG connection model. If false, AIT* uses an r-disc model.
Definition: AITstar.cpp:356
ompl::base::Cost bestCost() const
Get the cost of the incumbent solution.
Definition: AITstar.cpp:254
void setup() override
Additional setup that can only be done once a problem definition is set.
Definition: AITstar.cpp:94
ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &terminationCondition) override
Solves a motion planning problem.
Definition: AITstar.cpp:215
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:351
void setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG graph.
Definition: AITstar.cpp:313
unsigned int getMaxNumberOfGoals() const
Get the maximum number of goals AIT* will sample from sampleable goal regions.
Definition: AITstar.cpp:366
Main namespace. Contains everything in this library.
void trackApproximateSolutions(bool track)
Set whether to track approximate solutions.
Definition: AITstar.cpp:323