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;
145 
147  typedef std::function<std::string()> NameFunc;
148 
150  BITstar(const base::SpaceInformationPtr &si, const std::string &name = "BITstar");
151 
153  virtual ~BITstar() override = default;
154 
156  void setup() override;
157 
159  void clear() override;
160 
163 
165  void getPlannerData(base::PlannerData &data) const override;
166 
168  // Planner info for debugging, etc:
171  std::pair<const ompl::base::State *, const ompl::base::State *> getNextEdgeInQueue();
172 
177 
179  void getEdgeQueue(VertexConstPtrPairVector *edgesInQueue);
180 
182  void getVertexQueue(VertexConstPtrVector *verticesInQueue);
183 
185  unsigned int numIterations() const;
186 
188  ompl::base::Cost bestCost() const;
189 
191  unsigned int numBatches() const;
193 
195  // Planner settings:
197  void setRewireFactor(double rewireFactor);
198 
200  double getRewireFactor() const;
201 
203  void setSamplesPerBatch(unsigned int n);
204 
206  unsigned int getSamplesPerBatch() const;
207 
209  void setUseKNearest(bool useKNearest);
210 
212  bool getUseKNearest() const;
213 
217  void setStrictQueueOrdering(bool beStrict);
218 
220  bool getStrictQueueOrdering() const;
221 
226  void setPruning(bool prune);
227 
229  bool getPruning() const;
230 
233  void setPruneThresholdFraction(double fractionalChange);
234 
237  double getPruneThresholdFraction() const;
238 
244  void setDelayRewiringUntilInitialSolution(bool delayRewiring);
245 
248 
258  void setJustInTimeSampling(bool useJit);
259 
261  bool getJustInTimeSampling() const;
262 
269  void setDropSamplesOnPrune(bool dropSamples);
270 
272  bool getDropSamplesOnPrune() const;
273 
276  void setStopOnSolnImprovement(bool stopOnChange);
277 
279  bool getStopOnSolnImprovement() const;
280 
282  void setConsiderApproximateSolutions(bool findApproximate);
283 
285  bool getConsiderApproximateSolutions() const;
286 
288  template <template <typename T> class NN>
289  void setNearestNeighbors();
291 
292  private:
294  // BIT* primitives:
296  void iterate();
297 
299  void newBatch();
300 
302  void prune();
303 
305  void publishSolution();
307 
309  // Helper functions for data manipulation and other low-level functions
312  std::vector<const ompl::base::State *> bestPathFromGoalToStart() const;
313 
316  bool checkEdge(const VertexConstPtrPair &edge);
317 
320  void addEdge(const VertexPtrPair &newEdge, const ompl::base::Cost &edgeCost);
321 
323  void replaceParent(const VertexPtrPair &newEdge, const ompl::base::Cost &edgeCost);
324 
326  void updateGoalVertex();
328 
330  // Helper functions for logging
332  void goalMessage() const;
333 
335  void endSuccessMessage() const;
336 
338  void endFailureMessage() const;
339 
341  void statusMessage(const ompl::msg::LogLevel &msgLevel, const std::string &status) const;
343 
345  // Planner progress property functions
347  std::string bestCostProgressProperty() const;
348 
351  std::string bestLengthProgressProperty() const;
352 
355  std::string currentFreeProgressProperty() const;
356 
359  std::string currentVertexProgressProperty() const;
360 
363  std::string vertexQueueSizeProgressProperty() const;
364 
367  std::string edgeQueueSizeProgressProperty() const;
368 
370  std::string iterationProgressProperty() const;
371 
373  std::string batchesProgressProperty() const;
374 
376  std::string pruningProgressProperty() const;
377 
380  std::string totalStatesCreatedProgressProperty() const;
381 
384  std::string verticesConstructedProgressProperty() const;
385 
388  std::string statesPrunedProgressProperty() const;
389 
392  std::string verticesDisconnectedProgressProperty() const;
393 
396  std::string rewiringProgressProperty() const;
397 
400  std::string stateCollisionCheckProgressProperty() const;
401 
404  std::string edgeCollisionCheckProgressProperty() const;
405 
408  std::string nearestNeighbourProgressProperty() const;
409 
412  std::string edgesProcessedProgressProperty() const;
414 
416  // Variables -- Make sure every one is configured in setup() and reset in clear():
418  std::shared_ptr<CostHelper> costHelpPtr_{nullptr};
419 
421  std::shared_ptr<ImplicitGraph> graphPtr_{nullptr};
422 
426  std::shared_ptr<SearchQueue> queuePtr_{nullptr};
427 
429  VertexConstPtr curGoalVertex_{nullptr};
430 
433  // Gets set in setup to the proper calls from OptimizationObjective
434  ompl::base::Cost bestCost_{std::numeric_limits<double>::infinity()};
435 
438  unsigned int bestLength_{0u};
439 
442  // Gets set in setup to the proper calls from OptimizationObjective
443  ompl::base::Cost prunedCost_{std::numeric_limits<double>::infinity()};
444 
447  // Gets set in setup with the proper call to Planner::si_->getSpaceMeasure()
448  double prunedMeasure_{0.0};
449 
451  bool hasExactSolution_{false};
452 
454  bool stopLoop_{false};
456 
458  // Informational variables - Make sure initialized in setup and reset in clear
460  unsigned int numBatches_{0u};
461 
463  unsigned int numPrunings_{0u};
464 
466  unsigned int numIterations_{0u};
467 
469  unsigned int numRewirings_{0u};
470 
472  unsigned int numEdgeCollisionChecks_{0u};
474 
476  // Parameters - Set defaults in construction/setup and DO NOT reset in clear.
478  unsigned int samplesPerBatch_{100u};
479 
481  bool usePruning_{true};
482 
484  double pruneFraction_{0.05};
485 
487  bool stopOnSolnChange_{false};
489  }; // class: BITstar
490  } // geometric
491 } // ompl
492 #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:1154
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:1194
std::weak_ptr< Vertex > VertexWeakPtr
A vertex weak pointer.
Definition: BITstar.h:128
virtual ~BITstar() override=default
Destruct!
void getPlannerData(base::PlannerData &data) const override
Get results.
Definition: BITstar.cpp:407
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:69
unsigned int numBatches() const
Retrieve the number of batches processed as the raw data. (numBatches_)
Definition: BITstar.cpp:493
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:1179
bool getDelayRewiringUntilInitialSolution() const
Get whether BIT* is delaying rewiring until a solution is found.
Definition: BITstar.cpp:1144
void setDelayRewiringUntilInitialSolution(bool delayRewiring)
Delay the consideration of rewiring edges until an initial solution is found. When multiple batches a...
Definition: BITstar.cpp:1139
ompl::base::Cost bestCost() const
Retrieve the best exact-solution cost found.
Definition: BITstar.cpp:488
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:82
void setSamplesPerBatch(unsigned int n)
Set the number of samplers per batch.
Definition: BITstar.cpp:1061
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:1095
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Solve.
Definition: BITstar.cpp:324
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:1071
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:453
Base class for a planner.
Definition: Planner.h:223
void getEdgeQueue(VertexConstPtrPairVector *edgesInQueue)
Get the whole messy set of edges in the queue. Expensive but helpful for some videos.
Definition: BITstar.cpp:473
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:1164
bool getPruning() const
Get whether graph and sample pruning is in use.
Definition: BITstar.cpp:1119
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:429
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:1051
void setPruneThresholdFraction(double fractionalChange)
Set the fractional change in the solution cost AND problem measure necessary for pruning to occur...
Definition: BITstar.cpp:1124
unsigned int numIterations() const
Get the number of iterations completed.
Definition: BITstar.cpp:483
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:80
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:1149
A queue of edges to be processed that integrates both the expansion of Vertices and the ordering of t...
Definition: SearchQueue.h:92
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition: BITstar.h:147
void clear() override
Clear.
Definition: BITstar.cpp:287
void setup() override
Setup.
Definition: BITstar.cpp:226
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:1188
bool getUseKNearest() const
Get whether a k-nearest search is being used.
Definition: BITstar.cpp:1090
double getRewireFactor() const
Get the rewiring scale factor.
Definition: BITstar.cpp:1056
double getPruneThresholdFraction() const
Get the fractional change in the solution cost AND problem measure necessary for pruning to occur...
Definition: BITstar.cpp:1134
void getVertexQueue(VertexConstPtrVector *verticesInQueue)
Get the whole set of vertices to be expanded. Expensive but helpful for some videos.
Definition: BITstar.cpp:478
void setDropSamplesOnPrune(bool dropSamples)
Drop all unconnected samples when pruning, regardless of their heuristic value. This provides a metho...
Definition: BITstar.cpp:1159
void setPruning(bool prune)
Enable pruning of vertices/samples that CANNOT improve the current solution. When a vertex in the gra...
Definition: BITstar.cpp:1105
bool getStopOnSolnImprovement() const
Get whether BIT* stops each time a solution is found.
Definition: BITstar.cpp:1174
unsigned int getSamplesPerBatch() const
Get the number of samplers per batch.
Definition: BITstar.cpp:1066
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:1100
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:1169
unsigned int VertexId
The vertex id type.
Definition: BITstar.h:134