BITstar.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 */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_BITSTAR_BITSTAR_
38 #define OMPL_GEOMETRIC_PLANNERS_BITSTAR_BITSTAR_
39 
40 // STL:
41 // std::string
42 #include <string>
43 // std::pair
44 #include <utility>
45 // std::vector
46 #include <vector>
47 
48 // OMPL:
49 // The base-class of planners:
50 #include "ompl/base/Planner.h"
51 // The nearest neighbours structure
52 #include "ompl/datastructures/NearestNeighbors.h"
53 // The informed sampler structure
54 #include "ompl/base/samplers/InformedStateSampler.h"
55 // Planner includes:
56 //#include "ompl/geometric/planners/PlannerIncludes.h"
57 
58 // BIT*:
59 // The helper data classes, Vertex.h, CostHelper.h, ImplicitGraph.h, and SearchQueue.h are forward declared and then
60 // included in the source file (.cpp) as they are member classes of BITstar.
61 
62 // Debug setting. Defining BITSTAR_DEBUG enables (significant) debug output. Do not enable unless necessary.
63 //#define BITSTAR_DEBUG
64 
65 namespace ompl
66 {
67  namespace geometric
68  {
108  {
109  public:
110  // Forward declarations so that the classes belong to BIT*:
112  class Vertex;
114  class IdGenerator;
116  class CostHelper;
118  class ImplicitGraph;
121  class SearchQueue;
122  // Helpful alias declarations:
124  typedef std::shared_ptr<Vertex> VertexPtr;
126  typedef std::shared_ptr<const Vertex> VertexConstPtr;
128  typedef std::weak_ptr<Vertex> VertexWeakPtr;
130  typedef std::vector<VertexPtr> VertexPtrVector;
132  typedef std::vector<VertexConstPtr> VertexConstPtrVector;
134  typedef unsigned int VertexId;
136  typedef std::pair<VertexPtr, VertexPtr> VertexPtrPair;
138  typedef std::pair<VertexConstPtr, VertexConstPtr> VertexConstPtrPair;
140  typedef std::vector<VertexPtrPair> VertexPtrPairVector;
142  typedef std::vector<VertexConstPtrPair> VertexConstPtrPairVector;
144  typedef std::shared_ptr<NearestNeighbors<VertexPtr>> VertexPtrNNPtr;
146  typedef std::shared_ptr<CostHelper> CostHelperPtr;
148  typedef std::shared_ptr<ImplicitGraph> ImplicitGraphPtr;
150  typedef std::shared_ptr<SearchQueue> SearchQueuePtr;
151 
153  typedef std::function<std::string()> NameFunc;
154 
156  BITstar(const base::SpaceInformationPtr &si, const std::string &name = "BITstar");
157 
159  ~BITstar() override = default;
160 
162  void setup() override;
163 
165  void clear() override;
166 
169 
171  void getPlannerData(base::PlannerData &data) const override;
172 
174  // Planner info for debugging, etc:
177  std::pair<const ompl::base::State *, const ompl::base::State *> getNextEdgeInQueue();
178 
183 
185  void getEdgeQueue(VertexConstPtrPairVector *edgesInQueue);
186 
188  void getVertexQueue(VertexConstPtrVector *verticesInQueue);
189 
191  unsigned int numIterations() const;
192 
194  ompl::base::Cost bestCost() const;
195 
197  unsigned int numBatches() const;
199 
201  // Planner settings:
203  void setRewireFactor(double rewireFactor);
204 
206  double getRewireFactor() const;
207 
209  void setSamplesPerBatch(unsigned int n);
210 
212  unsigned int getSamplesPerBatch() const;
213 
215  void setUseKNearest(bool useKNearest);
216 
218  bool getUseKNearest() const;
219 
223  void setStrictQueueOrdering(bool beStrict);
224 
226  bool getStrictQueueOrdering() const;
227 
232  void setPruning(bool prune);
233 
235  bool getPruning() const;
236 
239  void setPruneThresholdFraction(double fractionalChange);
240 
243  double getPruneThresholdFraction() const;
244 
250  void setDelayRewiringUntilInitialSolution(bool delayRewiring);
251 
254 
264  void setJustInTimeSampling(bool useJit);
265 
267  bool getJustInTimeSampling() const;
268 
275  void setDropSamplesOnPrune(bool dropSamples);
276 
278  bool getDropSamplesOnPrune() const;
279 
282  void setStopOnSolnImprovement(bool stopOnChange);
283 
285  bool getStopOnSolnImprovement() const;
286 
288  void setConsiderApproximateSolutions(bool findApproximate);
289 
291  bool getConsiderApproximateSolutions() const;
292 
294  template <template <typename T> class NN>
295  void setNearestNeighbors();
297 
298  private:
300  // BIT* primitives:
302  void iterate();
303 
305  void newBatch();
306 
308  void prune();
309 
311  void publishSolution();
313 
315  // Helper functions for data manipulation and other low-level functions
318  std::vector<const ompl::base::State *> bestPathFromGoalToStart() const;
319 
322  bool checkEdge(const VertexConstPtrPair &edge);
323 
326  void addEdge(const VertexPtrPair &newEdge, const ompl::base::Cost &edgeCost);
327 
329  void replaceParent(const VertexPtrPair &newEdge, const ompl::base::Cost &edgeCost);
330 
332  void updateGoalVertex();
334 
336  // Helper functions for logging
338  void goalMessage() const;
339 
341  void endSuccessMessage() const;
342 
344  void endFailureMessage() const;
345 
347  void statusMessage(const ompl::msg::LogLevel &msgLevel, const std::string &status) const;
349 
351  // Planner progress property functions
353  std::string bestCostProgressProperty() const;
354 
357  std::string bestLengthProgressProperty() const;
358 
361  std::string currentFreeProgressProperty() const;
362 
365  std::string currentVertexProgressProperty() const;
366 
369  std::string vertexQueueSizeProgressProperty() const;
370 
373  std::string edgeQueueSizeProgressProperty() const;
374 
376  std::string iterationProgressProperty() const;
377 
379  std::string batchesProgressProperty() const;
380 
382  std::string pruningProgressProperty() const;
383 
386  std::string totalStatesCreatedProgressProperty() const;
387 
390  std::string verticesConstructedProgressProperty() const;
391 
394  std::string statesPrunedProgressProperty() const;
395 
398  std::string verticesDisconnectedProgressProperty() const;
399 
402  std::string rewiringProgressProperty() const;
403 
406  std::string stateCollisionCheckProgressProperty() const;
407 
410  std::string edgeCollisionCheckProgressProperty() const;
411 
414  std::string nearestNeighbourProgressProperty() const;
415 
418  std::string edgesProcessedProgressProperty() const;
420 
422  // Variables -- Make sure every one is configured in setup() and reset in clear():
424  CostHelperPtr costHelpPtr_;
425 
427  ImplicitGraphPtr graphPtr_;
428 
432  SearchQueuePtr queuePtr_;
433 
435  VertexConstPtr curGoalVertex_;
436 
439  ompl::base::Cost bestCost_;
440 
443  unsigned int bestLength_;
444 
447  ompl::base::Cost prunedCost_;
448 
451  double prunedMeasure_;
452 
454  bool hasExactSolution_;
455 
457  bool stopLoop_;
459 
461  // Informational variables - Make sure initialized in setup and reset in clear
463  unsigned int numBatches_;
464 
466  unsigned int numPrunings_;
467 
469  unsigned int numIterations_;
470 
472  unsigned int numRewirings_;
473 
475  unsigned int numEdgeCollisionChecks_;
477 
479  // Parameters - Set defaults in construction/setup and DO NOT reset in clear.
481  unsigned int samplesPerBatch_;
482 
484  bool usePruning_;
485 
487  double pruneFraction_;
488 
490  bool stopOnSolnChange_;
492  }; // class: BITstar
493 
495  // Basic helpers
497  template <typename T, typename U>
498  std::pair<T, U> operator+(const std::pair<T, U> &lhs, const std::pair<T, U> &rhs)
499  {
500  return std::make_pair(lhs.first + rhs.first, lhs.second + rhs.second);
501  }
503 
504  } // geometric
505 } // ompl
506 #endif // OMPL_GEOMETRIC_PLANNERS_BITSTAR_BITSTAR_
std::vector< VertexPtrPair > VertexPtrPairVector
A vector of pairs of vertices, i.e., a vector of edges.
Definition: BITstar.h:140
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
bool getJustInTimeSampling() const
Get whether we&#39;re using just-in-time sampling.
Definition: BITstar.cpp:1139
std::vector< VertexConstPtrPair > VertexConstPtrPairVector
A vector of pairs of const vertices, i.e., a vector of edges.
Definition: BITstar.h:142
void setNearestNeighbors()
Set a different nearest neighbours datastructure.
Definition: BITstar.cpp:1179
std::weak_ptr< Vertex > VertexWeakPtr
A vertex weak pointer.
Definition: BITstar.h:128
void getPlannerData(base::PlannerData &data) const override
Get results.
Definition: BITstar.cpp:392
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
A helper class to handle the various heuristic functions in one place.
Definition: CostHelper.h:64
unsigned int numBatches() const
Retrieve the number of batches processed as the raw data. (numBatches_)
Definition: BITstar.cpp:478
std::shared_ptr< NearestNeighbors< VertexPtr > > VertexPtrNNPtr
The OMPL::NearestNeighbors structure.
Definition: BITstar.h:144
void setConsiderApproximateSolutions(bool findApproximate)
Set BIT* to consider approximate solutions during its initial search.
Definition: BITstar.cpp:1164
bool getDelayRewiringUntilInitialSolution() const
Get whether BIT* is delaying rewiring until a solution is found.
Definition: BITstar.cpp:1129
void setDelayRewiringUntilInitialSolution(bool delayRewiring)
Delay the consideration of rewiring edges until an initial solution is found. When multiple batches a...
Definition: BITstar.cpp:1124
std::shared_ptr< ImplicitGraph > ImplicitGraphPtr
An implicit graph shared pointer.
Definition: BITstar.h:148
ompl::base::Cost bestCost() const
Retrieve the best exact-solution cost found.
Definition: BITstar.cpp:473
std::shared_ptr< const Vertex > VertexConstPtr
A constant vertex shared pointer.
Definition: BITstar.h:126
BITstar(const base::SpaceInformationPtr &si, const std::string &name="BITstar")
Construct!
Definition: BITstar.cpp:76
void setSamplesPerBatch(unsigned int n)
Set the number of samplers per batch.
Definition: BITstar.cpp:1046
std::shared_ptr< SearchQueue > SearchQueuePtr
An search queue shared pointer.
Definition: BITstar.h:150
void setStrictQueueOrdering(bool beStrict)
Enable "strict sorting" of the edge queue. Rewirings can change the position in the queue of an edge...
Definition: BITstar.cpp:1080
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Solve.
Definition: BITstar.cpp:334
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void setUseKNearest(bool useKNearest)
Enable a k-nearest search for instead of an r-disc search.
Definition: BITstar.cpp:1056
ompl::base::Cost getNextEdgeValueInQueue()
Get the value of the next edge to be processed. Causes vertices in the queue to be expanded (if neces...
Definition: BITstar.cpp:438
Base class for a planner.
Definition: Planner.h:232
void getEdgeQueue(VertexConstPtrPairVector *edgesInQueue)
Get the whole messy set of edges in the queue. Expensive but helpful for some videos.
Definition: BITstar.cpp:458
std::pair< VertexConstPtr, VertexConstPtr > VertexConstPtrPair
A pair of const vertices, i.e., an edge.
Definition: BITstar.h:138
bool getDropSamplesOnPrune() const
Get whether unconnected samples are dropped on pruning.
Definition: BITstar.cpp:1149
bool getPruning() const
Get whether graph and sample pruning is in use.
Definition: BITstar.cpp:1104
Batch Informed Trees (BIT*)
Definition: BITstar.h:107
std::pair< const ompl::base::State *, const ompl::base::State * > getNextEdgeInQueue()
Get the next edge to be processed. Causes vertices in the queue to be expanded (if necessary) and the...
Definition: BITstar.cpp:414
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
A shared pointer wrapper for ompl::base::SpaceInformation.
void setRewireFactor(double rewireFactor)
Set the rewiring scale factor, s, such that r_rrg = s r_rrg*.
Definition: BITstar.cpp:1036
void setPruneThresholdFraction(double fractionalChange)
Set the fractional change in the solution cost AND problem measure necessary for pruning to occur...
Definition: BITstar.cpp:1109
unsigned int numIterations() const
Get the number of iterations completed.
Definition: BITstar.cpp:468
An ID generator class for vertex IDs.
Definition: IdGenerator.h:58
std::pair< VertexPtr, VertexPtr > VertexPtrPair
A pair of vertices, i.e., an edge.
Definition: BITstar.h:136
The vertex of the underlying graphs in BIT*.
Definition: Vertex.h:79
void setJustInTimeSampling(bool useJit)
Delay the generation of samples until they are necessary. This only works when using an r-disc connec...
Definition: BITstar.cpp:1134
A queue of edges to be processed that integrates both the expansion of Vertices and the ordering of t...
Definition: SearchQueue.h:90
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition: BITstar.h:153
void clear() override
Clear.
Definition: BITstar.cpp:297
void setup() override
Setup.
Definition: BITstar.cpp:241
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers.
Definition: BITstar.h:130
A conceptual representation of samples as an edge-implicit random geometric graph.
Definition: ImplicitGraph.h:64
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared const pointers.
Definition: BITstar.h:132
bool getConsiderApproximateSolutions() const
Get whether BIT* is considering approximate solutions.
Definition: BITstar.cpp:1173
~BITstar() override=default
Destruct!
bool getUseKNearest() const
Get whether a k-nearest search is being used.
Definition: BITstar.cpp:1075
double getRewireFactor() const
Get the rewiring scale factor.
Definition: BITstar.cpp:1041
double getPruneThresholdFraction() const
Get the fractional change in the solution cost AND problem measure necessary for pruning to occur...
Definition: BITstar.cpp:1119
void getVertexQueue(VertexConstPtrVector *verticesInQueue)
Get the whole set of vertices to be expanded. Expensive but helpful for some videos.
Definition: BITstar.cpp:463
std::pair< T, U > operator+(const std::pair< T, U > &lhs, const std::pair< T, U > &rhs)
Define the addition operator for a std::pair<T,U> from the addition operators of T and U...
Definition: BITstar.h:498
void setDropSamplesOnPrune(bool dropSamples)
Drop all unconnected samples when pruning, regardless of their heuristic value. This provides a metho...
Definition: BITstar.cpp:1144
std::shared_ptr< CostHelper > CostHelperPtr
A cost helper shared pointer.
Definition: BITstar.h:146
void setPruning(bool prune)
Enable pruning of vertices/samples that CANNOT improve the current solution. When a vertex in the gra...
Definition: BITstar.cpp:1090
bool getStopOnSolnImprovement() const
Get whether BIT* stops each time a solution is found.
Definition: BITstar.cpp:1159
unsigned int getSamplesPerBatch() const
Get the number of samplers per batch.
Definition: BITstar.cpp:1051
std::shared_ptr< Vertex > VertexPtr
A vertex shared pointer.
Definition: BITstar.h:121
LogLevel
The set of priorities for message logging.
Definition: Console.h:84
bool getStrictQueueOrdering() const
Get whether strict queue ordering is in use.
Definition: BITstar.cpp:1085
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
void setStopOnSolnImprovement(bool stopOnChange)
Stop the planner each time a solution improvement is found. Useful for examining the intermediate sol...
Definition: BITstar.cpp:1154
unsigned int VertexId
The vertex id type.
Definition: BITstar.h:134