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  {
96  class AITstar : public ompl::base::Planner
97  {
98  public:
100  explicit AITstar(const ompl::base::SpaceInformationPtr &spaceInformation);
101 
103  ~AITstar() = default;
104 
106  void setup() override;
107 
110 
114 
116  void clear() override;
117 
120  solve(const ompl::base::PlannerTerminationCondition &terminationCondition) override;
121 
123  ompl::base::Cost bestCost() const;
124 
126  void getPlannerData(base::PlannerData &data) const override;
127 
129  void setBatchSize(std::size_t batchSize);
130 
132  std::size_t getBatchSize() const;
133 
135  void setRewireFactor(double rewireFactor);
136 
138  double getRewireFactor() const;
139 
141  void trackApproximateSolutions(bool track);
142 
144  bool areApproximateSolutionsTracked() const;
145 
147  void enablePruning(bool prune);
148 
150  bool isPruningEnabled() const;
151 
153  void setUseKNearest(bool useKNearest);
154 
156  bool getUseKNearest() const;
157 
159  void setMaxNumberOfGoals(unsigned int numberOfGoals);
160 
162  unsigned int getMaxNumberOfGoals() const;
163 
165  std::vector<aitstar::Edge> getEdgesInQueue() const;
166 
168  std::vector<std::shared_ptr<aitstar::Vertex>> getVerticesInQueue() const;
169 
172 
174  std::shared_ptr<aitstar::Vertex> getNextVertexInQueue() const;
175 
177  std::vector<std::shared_ptr<aitstar::Vertex>> getVerticesInReverseSearchTree() const;
178 
179  private:
181  void iterate(const ompl::base::PlannerTerminationCondition &terminationCondition);
182 
184  void iterateForwardSearch();
185 
187  void iterateReverseSearch();
188 
190  void updateReverseSearchVertex(const std::shared_ptr<aitstar::Vertex> &vertex);
191 
193  void updateReverseSearchNeighbors(const std::shared_ptr<aitstar::Vertex> &vertex);
194 
196  void insertOrUpdateInForwardQueue(const aitstar::Edge &edge);
197 
199  void insertOrUpdateInForwardQueue(const std::vector<aitstar::Edge> &edges);
200 
202  void insertOrUpdateInReverseQueue(const std::shared_ptr<aitstar::Vertex> &vertex);
203 
205  void rebuildForwardQueue();
206 
208  void rebuildReverseQueue();
209 
211  void clearForwardQueue();
212 
214  void clearReverseQueue();
215 
217  void informAboutNewSolution() const;
218 
220  void informAboutPlannerStatus(ompl::base::PlannerStatus::StatusType status) const;
221 
223  void insertGoalVerticesInReverseQueue();
224 
226  void expandStartVerticesIntoForwardQueue();
227 
229  bool continueReverseSearch() const;
230 
232  bool continueForwardSearch();
233 
235  std::shared_ptr<ompl::geometric::PathGeometric>
236  getPathToVertex(const std::shared_ptr<aitstar::Vertex> &vertex) const;
237 
239  std::array<ompl::base::Cost, 3u> computeSortKey(const std::shared_ptr<aitstar::Vertex> &parent,
240  const std::shared_ptr<aitstar::Vertex> &child) const;
241 
243  std::array<ompl::base::Cost, 2u> computeSortKey(const std::shared_ptr<aitstar::Vertex> &vertex) const;
244 
246  void insertOutgoingEdges(const std::shared_ptr<aitstar::Vertex> &vertex);
247 
249  std::vector<aitstar::Edge> getOutgoingEdges(const std::shared_ptr<aitstar::Vertex> &vertex) const;
250 
253  void updateExactSolution();
254 
257  void updateApproximateSolution(const std::shared_ptr<aitstar::Vertex> &vertex);
258 
260  void updateApproximateSolution();
261 
263  ompl::base::PlannerStatus::StatusType updateSolution();
264 
266  ompl::base::PlannerStatus::StatusType updateSolution(const std::shared_ptr<aitstar::Vertex> &vertex);
267 
269  ompl::base::Cost computeCostToGoToStartHeuristic(const std::shared_ptr<aitstar::Vertex> &vertex) const;
270 
272  ompl::base::Cost computeCostToGoToGoalHeuristic(const std::shared_ptr<aitstar::Vertex> &vertex) const;
273 
275  ompl::base::Cost computeCostToGoToGoal(const std::shared_ptr<aitstar::Vertex> &vertex) const;
276 
278  ompl::base::Cost computeBestCostToComeFromGoalOfAnyStart() const;
279 
281  std::size_t countNumVerticesInForwardTree() const;
282 
284  std::size_t countNumVerticesInReverseTree() const;
285 
288  void invalidateCostToComeFromGoalOfReverseBranch(const std::shared_ptr<aitstar::Vertex> &vertex);
289 
291  ompl::base::Cost solutionCost_;
292 
294  ompl::base::Cost approximateSolutionCost_{};
295 
297  ompl::base::Cost approximateSolutionCostToGoal_{};
298 
300  aitstar::ImplicitGraph graph_;
301 
303  aitstar::EdgeQueue forwardQueue_;
304 
306  aitstar::VertexQueue reverseQueue_;
307 
309  bool isEdgeBetter(const aitstar::Edge &lhs, const aitstar::Edge &rhs) const;
310 
312  bool isVertexBetter(const aitstar::KeyVertexPair &lhs, const aitstar::KeyVertexPair &rhs) const;
313 
315  std::size_t numInconsistentOrUnconnectedTargets_{0u};
316 
318  std::size_t numIterations_{0u};
319 
321  std::size_t batchSize_{100u};
322 
324  bool trackApproximateSolutions_{true};
325 
327  bool isPruningEnabled_{true};
328 
331 
333  ompl::base::MotionValidatorPtr motionValidator_;
334 
336  std::size_t numProcessedEdges_{0u};
337 
339  std::size_t numEdgeCollisionChecks_{0u};
340  };
341  } // namespace geometric
342 } // namespace ompl
343 
344 #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:595
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:617
void setBatchSize(std::size_t batchSize)
Set the batch size.
Definition: AITstar.cpp:306
void setMaxNumberOfGoals(unsigned int numberOfGoals)
Set the maximum number of goals AIT* will sample from sampleable goal regions.
Definition: AITstar.cpp:364
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:602
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:627
A class to store the exit status of Planner::solve()
bool areApproximateSolutionsTracked() const
Get whether approximate solutions are tracked.
Definition: AITstar.cpp:339
~AITstar()=default
Destructs a AIT*.
void enablePruning(bool prune)
Set whether pruning is enabled or not.
Definition: AITstar.cpp:344
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInReverseSearchTree() const
Get the vertices in the reverse search tree.
Definition: AITstar.cpp:637
void getPlannerData(base::PlannerData &data) const override
Get the planner data.
Definition: AITstar.cpp:262
double getRewireFactor() const
Get the rewire factor of the RGG graph.
Definition: AITstar.cpp:321
A shared pointer wrapper for ompl::base::MotionValidator.
bool isPruningEnabled() const
Get whether pruning is enabled or not.
Definition: AITstar.cpp:349
StatusType
The possible values of the status returned by a planner.
std::size_t getBatchSize() const
Get the batch size.
Definition: AITstar.cpp:311
bool getUseKNearest() const
Get whether to use a k-nearest RGG connection model. If false, AIT* uses an r-disc model.
Definition: AITstar.cpp:359
ompl::base::Cost bestCost() const
Get the cost of the incumbent solution.
Definition: AITstar.cpp:257
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:218
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:354
void setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG graph.
Definition: AITstar.cpp:316
unsigned int getMaxNumberOfGoals() const
Get the maximum number of goals AIT* will sample from sampleable goal regions.
Definition: AITstar.cpp:369
Main namespace. Contains everything in this library.
void trackApproximateSolutions(bool track)
Set whether to track approximate solutions.
Definition: AITstar.cpp:326