BITstar.cpp
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 // My definition:
38 #include "ompl/geometric/planners/bitstar/BITstar.h"
39 
40 // For stringstreams
41 #include <sstream>
42 // For stream manipulations
43 #include <iomanip>
44 // For smart pointers
45 #include <memory>
46 // For boost::adaptors::reverse which let "for (auto ...)" loops iterate in reverse
47 #include <boost/range/adaptor/reversed.hpp>
48 
49 // For OMPL_INFORM et al.
50 #include "ompl/util/Console.h"
51 // For exceptions:
52 #include "ompl/util/Exception.h"
53 // For ompl::geometric::path
54 #include "ompl/geometric/PathGeometric.h"
55 // For the default optimization objective:
56 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
57 
58 // BIT*:
59 // The Vertex ID generator class
60 #include "ompl/geometric/planners/bitstar/datastructures/IdGenerator.h"
61 // My vertex class:
62 #include "ompl/geometric/planners/bitstar/datastructures/Vertex.h"
63 // My cost & heuristic helper class
64 #include "ompl/geometric/planners/bitstar/datastructures/CostHelper.h"
65 // My implicit graph
66 #include "ompl/geometric/planners/bitstar/datastructures/ImplicitGraph.h"
67 // My queue class
68 #include "ompl/geometric/planners/bitstar/datastructures/SearchQueue.h"
69 
70 namespace ompl
71 {
72  namespace geometric
73  {
75  // Public functions:
76  BITstar::BITstar(const ompl::base::SpaceInformationPtr &si, const std::string &name /*= "BITstar"*/)
77  : ompl::base::Planner(si, name)
78  , costHelpPtr_(nullptr)
79  , graphPtr_(nullptr)
80  , queuePtr_(nullptr)
81  , curGoalVertex_(nullptr)
82  , bestCost_(std::numeric_limits<double>::infinity()) // Gets set in setup to the proper calls from
83  // OptimizationObjective
84  , bestLength_(0u)
85  , prunedCost_(std::numeric_limits<double>::infinity()) // Gets set in setup to the proper calls from
86  // OptimizationObjective
87  , prunedMeasure_(0.0) // Gets set in setup with the proper call to Planner::si_->getSpaceMeasure()
88  , hasExactSolution_(false)
89  , stopLoop_(false)
90  , numBatches_(0u)
91  , numPrunings_(0u)
92  , numIterations_(0u)
93  , numRewirings_(0u)
94  , numEdgeCollisionChecks_(0u)
95  , samplesPerBatch_(100u)
96  , usePruning_(true)
97  , pruneFraction_(0.05)
98  , stopOnSolnChange_(false)
99  {
100 #ifdef BITSTAR_DEBUG
101  OMPL_WARN("%s: Compiled with debug-level asserts.", Planner::getName().c_str());
102 #endif // BITSTAR_DEBUG
103 
104  // Allocate my helper classes, they hold settings and must never be deallocated. Give them a pointer to my
105  // name, so they can output helpful error messages
106  costHelpPtr_ = std::make_shared<CostHelper>();
107  graphPtr_ = std::make_shared<ImplicitGraph>([this]()
108  {
109  return getName();
110  });
111  queuePtr_ = std::make_shared<SearchQueue>([this]()
112  {
113  return getName();
114  });
115  queuePtr_->setPruneDuringResort(usePruning_);
116 
117  // Make sure the default name reflects the default k-nearest setting
118  if (graphPtr_->getUseKNearest() == true && Planner::getName() == "BITstar")
119  {
120  // It's the current default r-disc BIT* name, but we're using k-nearest, so change
121  Planner::setName("kBITstar");
122  }
123  else if (graphPtr_->getUseKNearest() == false && Planner::getName() == "kBITstar")
124  {
125  // It's the current default k-nearest BIT* name, but we're using r-disc, so change
126  Planner::setName("BITstar");
127  }
128  // It's not default named, don't change it
129 
130  // Specify my planner specs:
131  Planner::specs_.recognizedGoal = ompl::base::GOAL_SAMPLEABLE_REGION;
132  Planner::specs_.multithreaded = false;
133  // Approximate solutions are supported but must be enabled with the appropriate configuration parameter.
134  Planner::specs_.approximateSolutions = graphPtr_->getTrackApproximateSolutions();
135  Planner::specs_.optimizingPaths = true;
136  Planner::specs_.directed = true;
137  Planner::specs_.provingSolutionNonExistence = false;
138  Planner::specs_.canReportIntermediateSolutions = true;
139 
140  // Register my setting callbacks
141  Planner::declareParam<double>("rewire_factor", this, &BITstar::setRewireFactor, &BITstar::getRewireFactor,
142  "1.0:0.01:3.0");
143  Planner::declareParam<unsigned int>("samples_per_batch", this, &BITstar::setSamplesPerBatch,
144  &BITstar::getSamplesPerBatch, "1:1:1000000");
145  Planner::declareParam<bool>("use_k_nearest", this, &BITstar::setUseKNearest, &BITstar::getUseKNearest, "0,"
146  "1");
147  Planner::declareParam<bool>("use_graphPtr_pruning", this, &BITstar::setPruning, &BITstar::getPruning, "0,"
148  "1");
149  Planner::declareParam<double>("prune_threshold_as_fractional_cost_change", this,
151  "0.0:0.01:1.0");
152  Planner::declareParam<bool>("delay_rewiring_to_first_solution", this,
155  Planner::declareParam<bool>("use_just_in_time_sampling", this, &BITstar::setJustInTimeSampling,
157  Planner::declareParam<bool>("drop_unconnected_samples_on_prune", this, &BITstar::setDropSamplesOnPrune,
159  Planner::declareParam<bool>("stop_on_each_solution_improvement", this, &BITstar::setStopOnSolnImprovement,
161  Planner::declareParam<bool>("use_strict_queue_ordering", this, &BITstar::setStrictQueueOrdering,
163  Planner::declareParam<bool>("find_approximate_solutions", this, &BITstar::setConsiderApproximateSolutions,
165 
166  // Register my progress info:
167  addPlannerProgressProperty("best cost DOUBLE", [this]
168  {
169  return bestCostProgressProperty();
170  });
171  addPlannerProgressProperty("number of segments in solution path INTEGER", [this]
172  {
173  return bestLengthProgressProperty();
174  });
175  addPlannerProgressProperty("current free states INTEGER", [this]
176  {
177  return currentFreeProgressProperty();
178  });
179  addPlannerProgressProperty("current graph vertices INTEGER", [this]
180  {
181  return currentVertexProgressProperty();
182  });
183  addPlannerProgressProperty("state collision checks INTEGER", [this]
184  {
185  return stateCollisionCheckProgressProperty();
186  });
187  addPlannerProgressProperty("edge collision checks INTEGER", [this]
188  {
189  return edgeCollisionCheckProgressProperty();
190  });
191  addPlannerProgressProperty("nearest neighbour calls INTEGER", [this]
192  {
193  return nearestNeighbourProgressProperty();
194  });
195 
196  // Extra progress info that aren't necessary for every day use. Uncomment if desired.
197  /*
198  addPlannerProgressProperty("vertex queue size INTEGER", [this]
199  {
200  return vertexQueueSizeProgressProperty();
201  });
202  addPlannerProgressProperty("edge queue size INTEGER", [this]
203  {
204  return edgeQueueSizeProgressProperty();
205  });
206  addPlannerProgressProperty("iterations INTEGER", [this]
207  {
208  return iterationProgressProperty();
209  });
210  addPlannerProgressProperty("batches INTEGER", [this]
211  {
212  return batchesProgressProperty();
213  });
214  addPlannerProgressProperty("graph prunings INTEGER", [this]
215  {
216  return pruningProgressProperty();
217  });
218  addPlannerProgressProperty("total states generated INTEGER", [this]
219  {
220  return totalStatesCreatedProgressProperty();
221  });
222  addPlannerProgressProperty("vertices constructed INTEGER", [this]
223  {
224  return verticesConstructedProgressProperty();
225  });
226  addPlannerProgressProperty("states pruned INTEGER", [this]
227  {
228  return statesPrunedProgressProperty();
229  });
230  addPlannerProgressProperty("graph vertices disconnected INTEGER", [this]
231  {
232  return verticesDisconnectedProgressProperty();
233  });
234  addPlannerProgressProperty("rewiring edges INTEGER", [this]
235  {
236  return rewiringProgressProperty();
237  });
238  */
239  }
240 
242  {
243  // Call the base class setup:
244  Planner::setup();
245 
246  // Do some sanity checks
247  // Make sure we have a problem definition
248  if (static_cast<bool>(Planner::pdef_) == false)
249  {
250  OMPL_ERROR("%s::setup() was called without a problem definition.", Planner::getName().c_str());
251  Planner::setup_ = false;
252  return;
253  }
254 
255  // Make sure we have an optimization objective
256  if (Planner::pdef_->hasOptimizationObjective() == false)
257  {
258  OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length.",
259  Planner::getName().c_str());
260  Planner::pdef_->setOptimizationObjective(
261  std::make_shared<base::PathLengthOptimizationObjective>(Planner::si_));
262  }
263 
264  // If the problem definition *has* a goal, make sure it is of appropriate type
265  if (static_cast<bool>(Planner::pdef_->getGoal()) == true)
266  {
267  if (Planner::pdef_->getGoal()->hasType(ompl::base::GOAL_SAMPLEABLE_REGION) == false)
268  {
269  OMPL_ERROR("%s::setup() BIT* currently only supports goals that can be cast to a sampleable goal "
270  "region.",
271  Planner::getName().c_str());
272  Planner::setup_ = false;
273  return;
274  }
275  // No else, of correct type.
276  }
277  // No else, called without a goal. Is this MoveIt?
278 
279  // Setup the CostHelper, it provides everything I need from optimization objective plus some frills
280  costHelpPtr_->setup(Planner::pdef_->getOptimizationObjective(), graphPtr_);
281 
282  // Setup the queue
283  queuePtr_->setup(costHelpPtr_, graphPtr_);
284 
285  // Setup the graph, it does not hold a copy of this or Planner::pis_, but uses them to create a NN struct
286  // and check for starts/goals, respectively.
287  graphPtr_->setup(Planner::si_, Planner::pdef_, costHelpPtr_, queuePtr_, this, Planner::pis_);
288 
289  // Set the best and pruned costs to the proper objective-based values:
290  bestCost_ = costHelpPtr_->infiniteCost();
291  prunedCost_ = costHelpPtr_->infiniteCost();
292 
293  // Get the measure of the problem
294  prunedMeasure_ = Planner::si_->getSpaceMeasure();
295  }
296 
298  {
299  // Clear all the variables.
300  // Keep this in the order of the constructors:
301 
302  // The various helper classes. DO NOT reset the pointers, they hold configuration parameters:
303  costHelpPtr_->clear();
304  graphPtr_->clear();
305  queuePtr_->clear();
306 
307  // Reset the various calculations and convenience containers. Will be recalculated on setup
308  curGoalVertex_.reset();
309  bestCost_ = ompl::base::Cost(std::numeric_limits<double>::infinity());
310  bestLength_ = 0u;
311  prunedCost_ = ompl::base::Cost(std::numeric_limits<double>::infinity());
312  prunedMeasure_ = 0.0;
313  hasExactSolution_ = false;
314  stopLoop_ = false;
315  numBatches_ = 0u;
316  numPrunings_ = 0u;
317  numIterations_ = 0u;
318  numEdgeCollisionChecks_ = 0u;
319  numRewirings_ = 0u;
320 
321  // DO NOT reset the configuration parameters:
322  // samplesPerBatch_
323  // usePruning_
324  // pruneFraction_
325  // stopOnSolnChange_
326 
327  // Mark as not setup:
328  Planner::setup_ = false;
329 
330  // Call my base clear:
331  Planner::clear();
332  }
333 
335  {
336  Planner::checkValidity();
337  OMPL_INFORM("%s: Searching for a solution to the given planning problem.", Planner::getName().c_str());
338 
339  // Reset the manual stop to the iteration loop:
340  stopLoop_ = false;
341 
342  // If we don't have a goal yet, recall updateStartAndGoalStates, but wait for the first goal (or until the
343  // PTC comes true and we give up):
344  if (graphPtr_->hasAGoal() == false)
345  {
346  graphPtr_->updateStartAndGoalStates(Planner::pis_, ptc);
347  }
348 
349  /* Iterate as long as:
350  - We're allowed (ptc == false && stopLoop_ == false), AND
351  - We haven't found a good enough solution (costHelpPtr_->isSatisfied(bestCost) == false),
352  - AND
353  - There is a theoretically better solution (costHelpPtr_->isCostBetterThan(graphPtr_->minCost(),
354  bestCost_) == true), OR
355  - There is are start/goal states we've yet to consider (pis_.haveMoreStartStates() == true ||
356  pis_.haveMoreGoalStates() == true)
357  */
358  while (ptc == false && stopLoop_ == false && costHelpPtr_->isSatisfied(bestCost_) == false &&
359  (costHelpPtr_->isCostBetterThan(graphPtr_->minCost(), bestCost_) == true ||
360  Planner::pis_.haveMoreStartStates() == true || Planner::pis_.haveMoreGoalStates() == true))
361  {
362  this->iterate();
363  }
364 
365  // Announce
366  if (hasExactSolution_ == true)
367  {
368  this->endSuccessMessage();
369  }
370  else
371  {
372  this->endFailureMessage();
373  }
374 
375  // Publish
376  if (hasExactSolution_ == true || graphPtr_->getTrackApproximateSolutions() == true)
377  {
378  // Any solution
379  this->publishSolution();
380  }
381  // No else, no solution to publish
382 
383  // From OMPL's point-of-view, BIT* can always have an approximate solution, so mark solution true if either
384  // we have an exact solution or are finding approximate ones.
385  // Our returned solution will only be approximate if it is not exact and we are finding approximate
386  // solutions.
387  // PlannerStatus(addedSolution, approximate)
388  return ompl::base::PlannerStatus(hasExactSolution_ || graphPtr_->getTrackApproximateSolutions(),
389  !hasExactSolution_ && graphPtr_->getTrackApproximateSolutions());
390  }
391 
393  {
394  // Get the base planner class data:
395  Planner::getPlannerData(data);
396 
397  // Add the samples (the graph)
398  graphPtr_->getGraphAsPlannerData(data);
399 
400  // Did we find a solution?
401  if (hasExactSolution_ == true)
402  {
403  // Exact solution
404  data.markGoalState(curGoalVertex_->stateConst());
405  }
406  else if (hasExactSolution_ == false && graphPtr_->getTrackApproximateSolutions() == true)
407  {
408  // Approximate solution
409  data.markGoalState(graphPtr_->closestVertexToGoal()->stateConst());
410  }
411  // No else, no solution
412  }
413 
414  std::pair<ompl::base::State const *, ompl::base::State const *> BITstar::getNextEdgeInQueue()
415  {
416  // Variable:
417  // The next edge as a basic pair of states
418  std::pair<ompl::base::State const *, ompl::base::State const *> nextEdge;
419 
420  if (queuePtr_->isEmpty() == false)
421  {
422  // Variable
423  // The edge in the front of the queue
424  VertexConstPtrPair frontEdge = queuePtr_->frontEdge();
425 
426  // The next edge in the queue:
427  nextEdge = std::make_pair(frontEdge.first->stateConst(), frontEdge.second->stateConst());
428  }
429  else
430  {
431  // An empty edge:
432  nextEdge = std::make_pair<ompl::base::State *, ompl::base::State *>(nullptr, nullptr);
433  }
434 
435  return nextEdge;
436  }
437 
439  {
440  // Variable
441  // The cost of the next edge
442  ompl::base::Cost nextCost;
443 
444  if (queuePtr_->isEmpty() == false)
445  {
446  // The next cost in the queue:
447  nextCost = queuePtr_->frontEdgeValue().at(0u);
448  }
449  else
450  {
451  // An infinite cost:
452  nextCost = costHelpPtr_->infiniteCost();
453  }
454 
455  return nextCost;
456  }
457 
459  {
460  queuePtr_->getEdges(edgesInQueue);
461  }
462 
464  {
465  queuePtr_->getVertices(verticesInQueue);
466  }
467 
468  unsigned int BITstar::numIterations() const
469  {
470  return numIterations_;
471  }
472 
474  {
475  return bestCost_;
476  }
477 
478  unsigned int BITstar::numBatches() const
479  {
480  return numBatches_;
481  }
483 
485  // Protected functions:
486  void BITstar::iterate()
487  {
488  // Info:
489  ++numIterations_;
490 
491  // Is the edge queue empty
492  if (queuePtr_->isEmpty() == true)
493  {
494  // Yes, we must have just finished a batch. Increase the resolution of the graph and restart the queue.
495  this->newBatch();
496  }
497  else
498  {
499  // If the edge queue is not empty, then there is work to do!
500 
501  // Variables:
502  // The current edge:
503  VertexPtrPair bestEdge;
504 
505  // Pop the minimum edge
506  queuePtr_->popFrontEdge(&bestEdge);
507 
508  // In the best case, can this edge improve our solution given the current graph?
509  // g_t(v) + c_hat(v,x) + h_hat(x) < g_t(x_g)?
510  if (costHelpPtr_->isCostBetterThan(costHelpPtr_->currentHeuristicEdge(bestEdge), bestCost_) == true)
511  {
512  // What about improving the current graph?
513  // g_t(v) + c_hat(v,x) < g_t(x)?
514  if (costHelpPtr_->isCostBetterThan(costHelpPtr_->currentHeuristicTarget(bestEdge),
515  bestEdge.second->getCost()) == true)
516  {
517  // Ok, so it *could* be a useful edge. Do the work of calculating its cost for real
518 
519  // Variables:
520  // The true cost of the edge:
521  ompl::base::Cost trueEdgeCost;
522 
523  // Get the true cost of the edge
524  trueEdgeCost = costHelpPtr_->trueEdgeCost(bestEdge);
525 
526  // Can this actual edge ever improve our solution?
527  // g_hat(v) + c(v,x) + h_hat(x) < g_t(x_g)?
528  if (costHelpPtr_->isCostBetterThan(
529  costHelpPtr_->combineCosts(costHelpPtr_->costToComeHeuristic(bestEdge.first),
530  trueEdgeCost,
531  costHelpPtr_->costToGoHeuristic(bestEdge.second)),
532  bestCost_) == true)
533  {
534  // Does this edge have a collision?
535  if (this->checkEdge(bestEdge) == true)
536  {
537  // Does the current edge improve our graph?
538  // g_t(v) + c(v,x) < g_t(x)?
539  if (costHelpPtr_->isCostBetterThan(
540  costHelpPtr_->combineCosts(bestEdge.first->getCost(), trueEdgeCost),
541  bestEdge.second->getCost()) == true)
542  {
543  // YAAAAH. Add the edge! Allowing for the sample to be removed from free if it is
544  // not currently connected and otherwise propagate cost updates to descendants.
545  // addEdge will update the queue and handle the extra work that occurs if this edge
546  // improves the solution.
547  this->addEdge(bestEdge, trueEdgeCost);
548 
549  // If the path to the goal has changed, we will need to update the cached info about
550  // the solution cost or solution length:
551  this->updateGoalVertex();
552 
553  /*
554  //Remove any unnecessary incoming edges in the edge queue
555  queuePtr_->removeExtraEdgesTo(bestEdge.second);
556  */
557 
558  // We will only prune the whole graph/samples on a new batch.
559  }
560  // No else, this edge may be useful at some later date.
561  }
562  // No else, we failed
563  }
564  // No else, we failed
565  }
566  // No else, we failed
567  }
568  else
569  {
570  // The edge cannot improve our solution, and therefore neither can any other edge in the queue. Give
571  // up on the batch:
572  queuePtr_->finish();
573  }
574  } // Search queue not empty.
575  }
576 
577  void BITstar::newBatch()
578  {
579  // Info:
580  ++numBatches_;
581 
582  // Reset the queue:
583  queuePtr_->reset();
584 
585  // Do we need to update our starts or goals?
586  if (Planner::pis_.haveMoreStartStates() == true || Planner::pis_.haveMoreGoalStates() == true)
587  {
588  // There are new starts/goals to get.
589  graphPtr_->updateStartAndGoalStates(Planner::pis_, ompl::base::plannerAlwaysTerminatingCondition());
590  }
591  // No else, we have enough of a problem to do some work, and everything's up to date.
592 
593  // Prune the graph (if enabled)
594  this->prune();
595 
596  // Add a new batch of samples
597  graphPtr_->addNewSamples(samplesPerBatch_);
598  }
599 
600  void BITstar::prune()
601  {
602  /* Test if we should we do a little tidying up:
603  - Is pruning enabled?
604  - Do we have an exact solution?
605  - Has the solution changed more than the specified pruning threshold?
606  */
607  if ((usePruning_ == true) && (hasExactSolution_ == true) &&
608  (std::abs(costHelpPtr_->fractionalChange(bestCost_, prunedCost_)) > pruneFraction_))
609  {
610  // Variables:
611  // The number of vertices and samples pruned:
612  std::pair<unsigned int, unsigned int> numPruned(0u, 0u);
613  // The current measure of the problem space:
614  double informedMeasure = graphPtr_->getInformedMeasure(bestCost_);
615  // The change in the informed measure
616  double relativeMeasure = std::abs((informedMeasure - prunedMeasure_) / prunedMeasure_);
617 
618  /* Is there good reason to prune?
619  - Is the informed subset measurably less than the total problem domain?
620  - Has its measured changed more than the specified pruning threshold?
621  - If an informed measure is not available, we'll assume yes
622  */
623  if ((graphPtr_->hasInformedMeasure() == true && informedMeasure < Planner::si_->getSpaceMeasure() &&
624  relativeMeasure > pruneFraction_) ||
625  (graphPtr_->hasInformedMeasure() == false))
626  {
627  OMPL_INFORM("%s: Pruning the planning problem from a solution of %.4f to %.4f, changing the "
628  "problem size from %.4f to %.4f.",
629  Planner::getName().c_str(), prunedCost_.value(), bestCost_.value(), prunedMeasure_,
630  informedMeasure);
631 
632  // Increment the pruning counter:
633  ++numPrunings_;
634 
635  // Prune the graph
636  numPruned = numPruned + graphPtr_->prune(informedMeasure);
637 
638  // Prune the search.
639  numPruned = numPruned + queuePtr_->prune(curGoalVertex_);
640 
641  // Store the cost at which we pruned:
642  prunedCost_ = bestCost_;
643 
644  // And the measure:
645  prunedMeasure_ = informedMeasure;
646 
647  OMPL_INFORM("%s: Pruning disconnected %d vertices from the tree and completely removed %d samples.",
648  Planner::getName().c_str(), numPruned.first, numPruned.second);
649  }
650  // No else, it's not worth the work to prune...
651  }
652  // No else, why was I called?
653  }
654 
655  void BITstar::publishSolution()
656  {
657  // Variable
658  // The reverse path of state pointers
659  std::vector<const ompl::base::State *> reversePath;
660  // Allocate a path geometric
661  auto pathGeoPtr = std::make_shared<ompl::geometric::PathGeometric>(Planner::si_);
662 
663  // Get the reversed path
664  reversePath = this->bestPathFromGoalToStart();
665 
666  // Now iterate that vector in reverse, putting the states into the path geometric
667  for (const auto &solnState : boost::adaptors::reverse(reversePath))
668  {
669  pathGeoPtr->append(solnState);
670  }
671 
672  // Now create the solution
673  ompl::base::PlannerSolution soln(pathGeoPtr);
674 
675  // Mark the name:
676  soln.setPlannerName(Planner::getName());
677 
678  // Mark as approximate if not exact:
679  if (hasExactSolution_ == false && graphPtr_->getTrackApproximateSolutions() == true)
680  {
681  soln.setApproximate(graphPtr_->smallestDistanceToGoal());
682  }
683 
684  // Mark whether the solution met the optimization objective:
685  soln.optimized_ = costHelpPtr_->isSatisfied(bestCost_);
686 
687  // Add the solution to the Problem Definition:
688  Planner::pdef_->addSolutionPath(soln);
689  }
690 
691  std::vector<const ompl::base::State *> BITstar::bestPathFromGoalToStart() const
692  {
693  // Variables:
694  // A vector of states from goal->start:
695  std::vector<const ompl::base::State *> reversePath;
696  // The vertex used to ascend up from the goal:
697  VertexConstPtr curVertex;
698 
699  // Iterate up the chain from the goal, creating a backwards vector:
700  if (hasExactSolution_ == true)
701  {
702  // Start at vertex in the goal
703  curVertex = curGoalVertex_;
704  }
705  else if (hasExactSolution_ == false && graphPtr_->getTrackApproximateSolutions() == true)
706  {
707  // Start at the vertex closest to the goal
708  curVertex = graphPtr_->closestVertexToGoal();
709  }
710  else
711  {
712  throw ompl::Exception("bestPathFromGoalToStart called without an exact or approximate solution.");
713  }
714 
715  // Insert the goal into the path
716  reversePath.push_back(curVertex->stateConst());
717 
718  // Then, use the vertex pointer like an iterator. Starting at the goal, we iterate up the chain pushing the
719  // *parent* of the iterator into the vector until the vertex has no parent.
720  // This will allows us to add the start (as the parent of the first child) and then stop when we get to the
721  // start itself, avoiding trying to find its nonexistent child
722  for (/*Already allocated & initialized*/; curVertex->isRoot() == false;
723  curVertex = curVertex->getParentConst())
724  {
725 #ifdef BITSTAR_DEBUG
726  // Check the case where the chain ends incorrectly.
727  if (curVertex->hasParent() == false)
728  {
729  throw ompl::Exception("The path to the goal does not originate at a start state. Something went "
730  "wrong.");
731  }
732 #endif // BITSTAR_DEBUG
733 
734  // Push back the parent into the vector as a state pointer:
735  reversePath.push_back(curVertex->getParentConst()->stateConst());
736  }
737  return reversePath;
738  }
739 
740  bool BITstar::checkEdge(const VertexConstPtrPair &edge)
741  {
742  ++numEdgeCollisionChecks_;
743  return Planner::si_->checkMotion(edge.first->stateConst(), edge.second->stateConst());
744  }
745 
746  void BITstar::addEdge(const VertexPtrPair &newEdge, const ompl::base::Cost &edgeCost)
747  {
748 #ifdef BITSTAR_DEBUG
749  if (newEdge.first->isInTree() == false)
750  {
751  throw ompl::Exception("Adding an edge from a vertex not connected to the graph");
752  }
753  if (costHelpPtr_->isCostEquivalentTo(costHelpPtr_->trueEdgeCost(newEdge), edgeCost) == false)
754  {
755  throw ompl::Exception("You have passed the wrong edge cost to addEdge.");
756  }
757 #endif // BITSTAR_DEBUG
758 
759  // Variables
760  // The edge is a rewiring if it is current in the tree:
761  bool isRewiring = newEdge.second->hasParent();
762 
763  // Perform a rewiring?
764  if (isRewiring == true)
765  {
766  // Replace the edge
767  this->replaceParent(newEdge, edgeCost);
768  }
769  else
770  {
771  // If not, we just add the vertex, first mark the target vertex as no longer new and unexpanded:
772  newEdge.second->markUnexpandedToSamples();
773  newEdge.second->markUnexpandedToVertices();
774 
775  // Then add a child to the parent, not updating costs:
776  newEdge.first->addChild(newEdge.second, false);
777 
778  // Add a parent to the child, updating descendant costs if requested:
779  newEdge.second->addParent(newEdge.first, edgeCost, true);
780 
781  // Then enqueue the vertex, moving it from the free set to the vertex set.
782  queuePtr_->enqueueVertex(newEdge.second, true);
783  }
784  }
785 
786  void BITstar::replaceParent(const VertexPtrPair &newEdge, const ompl::base::Cost &edgeCost)
787  {
788 #ifdef BITSTAR_DEBUG
789  if (newEdge.second->getParent()->getId() == newEdge.first->getId())
790  {
791  throw ompl::Exception("The new and old parents of the given rewiring are the same.");
792  }
793  if (costHelpPtr_->isCostBetterThan(newEdge.second->getCost(),
794  costHelpPtr_->combineCosts(newEdge.first->getCost(), edgeCost)) == true)
795  {
796  throw ompl::Exception("The new edge will increase the cost-to-come of the vertex!");
797  }
798 #endif // BITSTAR_DEBUG
799 
800  // Increment our counter:
801  ++numRewirings_;
802 
803  // Remove the child from the parent, not updating costs
804  newEdge.second->getParent()->removeChild(newEdge.second, false);
805 
806  // Remove the parent from the child, not updating costs
807  newEdge.second->removeParent(false);
808 
809  // Add the child to the parent, not updating costs
810  newEdge.first->addChild(newEdge.second, false);
811 
812  // Add the parent to the child. This updates the cost of the child as well as all it's descendents (if
813  // requested).
814  newEdge.second->addParent(newEdge.first, edgeCost);
815 
816  // Mark the queues as unsorted below this child
817  queuePtr_->markVertexUnsorted(newEdge.second);
818  }
819 
820  void BITstar::updateGoalVertex()
821  {
822  // Variable
823  // Whether we've updated the goal, be pessimistic.
824  bool goalUpdated = false;
825  // The new goal, start with the current goal
826  VertexConstPtr newBestGoal = curGoalVertex_;
827  // The new cost, start as the current bestCost_
828  ompl::base::Cost newCost = bestCost_;
829 
830  // Iterate through the vector of goals, and see if the solution has changed
831  for (auto goalIter = graphPtr_->goalVerticesBeginConst(); goalIter != graphPtr_->goalVerticesEndConst();
832  ++goalIter)
833  {
834  // First, is this goal even in the tree?
835  if ((*goalIter)->isInTree() == true)
836  {
837  // Next, is there currently a solution?
838  if (static_cast<bool>(newBestGoal) == true)
839  {
840  // There is already a solution, is it to this goal?
841  if ((*goalIter)->getId() == newBestGoal->getId())
842  {
843  // Ah-ha, We meet again! Are we doing any better? We check the length as sometimes the path
844  // length changes with minimal change in cost.
845  if (costHelpPtr_->isCostEquivalentTo((*goalIter)->getCost(), newCost) == false ||
846  ((*goalIter)->getDepth() + 1u) != bestLength_)
847  {
848  // The path to the current best goal has changed, so we need to update it.
849  goalUpdated = true;
850  newBestGoal = *goalIter;
851  newCost = newBestGoal->getCost();
852  }
853  // No else, no change
854  }
855  else
856  {
857  // It is not to this goal, we have a second solution! What an easy problem... but is it
858  // better?
859  if (costHelpPtr_->isCostBetterThan((*goalIter)->getCost(), newCost) == true)
860  {
861  // It is! Save this as a better goal:
862  goalUpdated = true;
863  newBestGoal = *goalIter;
864  newCost = newBestGoal->getCost();
865  }
866  // No else, not a better solution
867  }
868  }
869  else
870  {
871  // There isn't a preexisting solution, that means that any goal is an update:
872  goalUpdated = true;
873  newBestGoal = *goalIter;
874  newCost = newBestGoal->getCost();
875  }
876  }
877  // No else, can't be a better solution if it's not in the spanning tree, can it?
878  }
879 
880  // Did we update the goal?
881  if (goalUpdated == true)
882  {
883  // Mark that we have a solution
884  hasExactSolution_ = true;
885 
886  // Store the current goal
887  curGoalVertex_ = newBestGoal;
888 
889  // Update the best cost:
890  bestCost_ = newCost;
891 
892  // and best length
893  bestLength_ = curGoalVertex_->getDepth() + 1u;
894 
895  // Tell everyone else about it.
896  queuePtr_->hasSolution(bestCost_);
897  graphPtr_->hasSolution(bestCost_);
898 
899  // Stop the solution loop if enabled:
900  stopLoop_ = stopOnSolnChange_;
901 
902  // Brag:
903  this->goalMessage();
904 
905  // If enabled, pass the intermediate solution back through the call back:
906  if (static_cast<bool>(Planner::pdef_->getIntermediateSolutionCallback()) == true)
907  {
908  // The form of path passed to the intermediate solution callback is not well documented, but it
909  // *appears* that it's not supposed
910  // to include the start or goal; however, that makes no sense for multiple start/goal problems, so
911  // we're going to include it anyway (sorry).
912  // Similarly, it appears to be ordered as (goal, goal-1, goal-2,...start+1, start) which
913  // conveniently allows us to reuse code.
914  Planner::pdef_->getIntermediateSolutionCallback()(this, this->bestPathFromGoalToStart(), bestCost_);
915  }
916  }
917  // No else, the goal didn't change
918  }
919 
920  void BITstar::goalMessage() const
921  {
922  OMPL_INFORM("%s (%u iters): Found a solution of cost %.4f (%u vertices) from %u samples by processing %u "
923  "edges (%u collision checked) to create %u vertices and perform %u rewirings. The graph "
924  "currently has %u vertices.",
925  Planner::getName().c_str(), numIterations_, bestCost_.value(), bestLength_,
926  graphPtr_->numStatesGenerated(), queuePtr_->numEdgesPopped(), numEdgeCollisionChecks_,
927  graphPtr_->numVerticesConnected(), numRewirings_, graphPtr_->numConnectedVertices());
928  }
929 
930  void BITstar::endSuccessMessage() const
931  {
932  OMPL_INFORM("%s: Finished with a solution of cost %.4f (%u vertices) found from %u samples by processing "
933  "%u edges (%u collision checked) to create %u vertices and perform %u rewirings. The final "
934  "graph has %u vertices.",
935  Planner::getName().c_str(), bestCost_.value(), bestLength_, graphPtr_->numStatesGenerated(),
936  queuePtr_->numEdgesPopped(), numEdgeCollisionChecks_, graphPtr_->numVerticesConnected(),
937  numRewirings_, graphPtr_->numConnectedVertices());
938  }
939 
940  void BITstar::endFailureMessage() const
941  {
942  if (graphPtr_->getTrackApproximateSolutions() == true)
943  {
944  OMPL_INFORM("%s (%u iters): Did not find an exact solution from %u samples after processing %u edges "
945  "(%u collision checked) to create %u vertices and perform %u rewirings. The final graph "
946  "has %u vertices. The best approximate solution was %.4f from the goal and has a cost of "
947  "%.4f.",
948  Planner::getName().c_str(), numIterations_, graphPtr_->numStatesGenerated(),
949  queuePtr_->numEdgesPopped(), numEdgeCollisionChecks_, graphPtr_->numVerticesConnected(),
950  numRewirings_, graphPtr_->numConnectedVertices(), graphPtr_->smallestDistanceToGoal(),
951  graphPtr_->closestVertexToGoal()->getCost().value());
952  }
953  else
954  {
955  OMPL_INFORM("%s (%u iters): Did not find an exact solution from %u samples after processing %u edges "
956  "(%u collision checked) to create %u vertices and perform %u rewirings. The final graph "
957  "has %u vertices.",
958  Planner::getName().c_str(), numIterations_, graphPtr_->numStatesGenerated(),
959  queuePtr_->numEdgesPopped(), numEdgeCollisionChecks_, graphPtr_->numVerticesConnected(),
960  numRewirings_, graphPtr_->numConnectedVertices());
961  }
962  }
963 
964  void BITstar::statusMessage(const ompl::msg::LogLevel &msgLevel, const std::string &status) const
965  {
966  // Check if we need to create the message
967  if (msgLevel >= ompl::msg::getLogLevel())
968  {
969  // Variable
970  // The message as a stream:
971  std::stringstream outputStream;
972 
973  // Create the stream:
974  // The name of the planner
975  outputStream << Planner::getName();
976  outputStream << " (";
977  // The current path cost:
978  outputStream << "l: " << std::setw(6) << std::setfill(' ') << std::setprecision(5) << bestCost_.value();
979  // The number of batches:
980  outputStream << ", b: " << std::setw(5) << std::setfill(' ') << numBatches_;
981  // The number of iterations
982  outputStream << ", i: " << std::setw(5) << std::setfill(' ') << numIterations_;
983  // The number of states current in the graph
984  outputStream << ", g: " << std::setw(5) << std::setfill(' ') << graphPtr_->numConnectedVertices();
985  // The number of free states
986  outputStream << ", f: " << std::setw(5) << std::setfill(' ') << graphPtr_->numFreeSamples();
987  // The number edges in the queue:
988  outputStream << ", q: " << std::setw(5) << std::setfill(' ') << queuePtr_->numEdges();
989  // The total number of edges taken out of the queue:
990  outputStream << ", t: " << std::setw(5) << std::setfill(' ') << queuePtr_->numEdgesPopped();
991  // The number of samples generated
992  outputStream << ", s: " << std::setw(5) << std::setfill(' ') << graphPtr_->numStatesGenerated();
993  // The number of vertices ever added to the graph:
994  outputStream << ", v: " << std::setw(5) << std::setfill(' ') << graphPtr_->numVerticesConnected();
995  // The number of prunings:
996  outputStream << ", p: " << std::setw(5) << std::setfill(' ') << numPrunings_;
997  // The number of rewirings:
998  outputStream << ", r: " << std::setw(5) << std::setfill(' ') << numRewirings_;
999  // The number of nearest-neighbour calls
1000  outputStream << ", n: " << std::setw(5) << std::setfill(' ') << graphPtr_->numNearestLookups();
1001  // The number of state collision checks:
1002  outputStream << ", c(s): " << std::setw(5) << std::setfill(' ') << graphPtr_->numStateCollisionChecks();
1003  // The number of edge collision checks:
1004  outputStream << ", c(e): " << std::setw(5) << std::setfill(' ') << numEdgeCollisionChecks_;
1005  outputStream << "): ";
1006  // The message:
1007  outputStream << status;
1008 
1009  if (msgLevel == ompl::msg::LOG_DEBUG)
1010  {
1011  OMPL_DEBUG("%s: ", outputStream.str().c_str());
1012  }
1013  else if (msgLevel == ompl::msg::LOG_INFO)
1014  {
1015  OMPL_INFORM("%s: ", outputStream.str().c_str());
1016  }
1017  else if (msgLevel == ompl::msg::LOG_WARN)
1018  {
1019  OMPL_WARN("%s: ", outputStream.str().c_str());
1020  }
1021  else if (msgLevel == ompl::msg::LOG_ERROR)
1022  {
1023  OMPL_ERROR("%s: ", outputStream.str().c_str());
1024  }
1025  else
1026  {
1027  throw ompl::Exception("Log level not recognized");
1028  }
1029  }
1030  // No else, this message is below the log level
1031  }
1033 
1035  // Boring sets/gets (Public) and progress properties (Protected):
1036  void BITstar::setRewireFactor(double rewireFactor)
1037  {
1038  graphPtr_->setRewireFactor(rewireFactor);
1039  }
1040 
1042  {
1043  return graphPtr_->getRewireFactor();
1044  }
1045 
1046  void BITstar::setSamplesPerBatch(unsigned int n)
1047  {
1048  samplesPerBatch_ = n;
1049  }
1050 
1051  unsigned int BITstar::getSamplesPerBatch() const
1052  {
1053  return samplesPerBatch_;
1054  }
1055 
1056  void BITstar::setUseKNearest(bool useKNearest)
1057  {
1058  // Store
1059  graphPtr_->setUseKNearest(useKNearest);
1060 
1061  // If the planner is default named, we change it:
1062  if (graphPtr_->getUseKNearest() == true && Planner::getName() == "kBITstar")
1063  {
1064  // It's current the default k-nearest BIT* name, and we're toggling, so set to the default r-disc
1065  Planner::setName("BITstar");
1066  }
1067  else if (graphPtr_->getUseKNearest() == false && Planner::getName() == "BITstar")
1068  {
1069  // It's current the default r-disc BIT* name, and we're toggling, so set to the default k-nearest
1070  Planner::setName("kBITstar");
1071  }
1072  // It's not default named, don't change it
1073  }
1074 
1076  {
1077  return graphPtr_->getUseKNearest();
1078  }
1079 
1081  {
1082  queuePtr_->setStrictQueueOrdering(beStrict);
1083  }
1084 
1086  {
1087  return queuePtr_->getStrictQueueOrdering();
1088  }
1089 
1090  void BITstar::setPruning(bool prune)
1091  {
1092  if (prune == false)
1093  {
1094  OMPL_WARN("%s: Turning pruning off has never really been tested.", Planner::getName().c_str());
1095  }
1096 
1097  // Store
1098  usePruning_ = prune;
1099 
1100  // Pass the setting onto the queue
1101  queuePtr_->setPruneDuringResort(usePruning_);
1102  }
1103 
1104  bool BITstar::getPruning() const
1105  {
1106  return usePruning_;
1107  }
1108 
1109  void BITstar::setPruneThresholdFraction(double fractionalChange)
1110  {
1111  if (fractionalChange < 0.0 || fractionalChange > 1.0)
1112  {
1113  throw ompl::Exception("Prune threshold must be specified as a fraction between [0, 1].");
1114  }
1115 
1116  pruneFraction_ = fractionalChange;
1117  }
1118 
1120  {
1121  return pruneFraction_;
1122  }
1123 
1125  {
1126  queuePtr_->setDelayedRewiring(delayRewiring);
1127  }
1128 
1130  {
1131  return queuePtr_->getDelayedRewiring();
1132  }
1133 
1135  {
1136  graphPtr_->setJustInTimeSampling(useJit);
1137  }
1138 
1140  {
1141  return graphPtr_->getJustInTimeSampling();
1142  }
1143 
1144  void BITstar::setDropSamplesOnPrune(bool dropSamples)
1145  {
1146  graphPtr_->setDropSamplesOnPrune(dropSamples);
1147  }
1148 
1150  {
1151  return graphPtr_->getDropSamplesOnPrune();
1152  }
1153 
1154  void BITstar::setStopOnSolnImprovement(bool stopOnChange)
1155  {
1156  stopOnSolnChange_ = stopOnChange;
1157  }
1158 
1160  {
1161  return stopOnSolnChange_;
1162  }
1163 
1164  void BITstar::setConsiderApproximateSolutions(bool findApproximate)
1165  {
1166  // Store
1167  graphPtr_->setTrackApproximateSolutions(findApproximate);
1168 
1169  // Mark the Planner spec:
1170  Planner::specs_.approximateSolutions = graphPtr_->getTrackApproximateSolutions();
1171  }
1172 
1174  {
1175  return graphPtr_->getTrackApproximateSolutions();
1176  }
1177 
1178  template <template <typename T> class NN>
1180  {
1181  // Check if the problem is already setup, if so, the NN structs have data in them and you can't really
1182  // change them:
1183  if (Planner::setup_ == true)
1184  {
1185  OMPL_WARN("%s: The nearest neighbour datastructures cannot be changed once the planner is setup. "
1186  "Continuing to use the existing containers.",
1187  Planner::getName().c_str());
1188  }
1189  else
1190  {
1191  graphPtr_->setNearestNeighbors<NN>();
1192  }
1193  }
1194 
1195  std::string BITstar::bestCostProgressProperty() const
1196  {
1197  return std::to_string(this->bestCost().value());
1198  }
1199 
1200  std::string BITstar::bestLengthProgressProperty() const
1201  {
1202  return std::to_string(bestLength_);
1203  }
1204 
1205  std::string BITstar::currentFreeProgressProperty() const
1206  {
1207  return std::to_string(graphPtr_->numFreeSamples());
1208  }
1209 
1210  std::string BITstar::currentVertexProgressProperty() const
1211  {
1212  return std::to_string(graphPtr_->numConnectedVertices());
1213  }
1214 
1215  std::string BITstar::vertexQueueSizeProgressProperty() const
1216  {
1217  return std::to_string(queuePtr_->numVertices());
1218  }
1219 
1220  std::string BITstar::edgeQueueSizeProgressProperty() const
1221  {
1222  return std::to_string(queuePtr_->numEdges());
1223  }
1224 
1225  std::string BITstar::iterationProgressProperty() const
1226  {
1227  return std::to_string(this->numIterations());
1228  }
1229 
1230  std::string BITstar::batchesProgressProperty() const
1231  {
1232  return std::to_string(this->numBatches());
1233  }
1234 
1235  std::string BITstar::pruningProgressProperty() const
1236  {
1237  return std::to_string(numPrunings_);
1238  }
1239 
1240  std::string BITstar::totalStatesCreatedProgressProperty() const
1241  {
1242  return std::to_string(graphPtr_->numStatesGenerated());
1243  }
1244 
1245  std::string BITstar::verticesConstructedProgressProperty() const
1246  {
1247  return std::to_string(graphPtr_->numVerticesConnected());
1248  }
1249 
1250  std::string BITstar::statesPrunedProgressProperty() const
1251  {
1252  return std::to_string(graphPtr_->numFreeStatesPruned());
1253  }
1254 
1255  std::string BITstar::verticesDisconnectedProgressProperty() const
1256  {
1257  return std::to_string(graphPtr_->numVerticesDisconnected());
1258  }
1259 
1260  std::string BITstar::rewiringProgressProperty() const
1261  {
1262  return std::to_string(numRewirings_);
1263  }
1264 
1265  std::string BITstar::stateCollisionCheckProgressProperty() const
1266  {
1267  return std::to_string(graphPtr_->numStateCollisionChecks());
1268  }
1269 
1270  std::string BITstar::edgeCollisionCheckProgressProperty() const
1271  {
1272  return std::to_string(numEdgeCollisionChecks_);
1273  }
1274 
1275  std::string BITstar::nearestNeighbourProgressProperty() const
1276  {
1277  return std::to_string(graphPtr_->numNearestLookups());
1278  }
1279 
1280  std::string BITstar::edgesProcessedProgressProperty() const
1281  {
1282  return std::to_string(queuePtr_->numEdgesPopped());
1283  }
1285  } // geometric
1286 } // ompl
void addPlannerProgressProperty(const std::string &progressPropertyName, const PlannerProgressProperty &prop)
Add a planner progress property called progressPropertyName with a property querying function prop to...
Definition: Planner.h:408
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
void setApproximate(double difference)
Specify that the solution is approximate and set the difference to the goal.
bool optimized_
True if the solution was optimized to meet the specified optimization criterion.
Representation of a solution to a planning problem.
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...
STL namespace.
unsigned int numBatches() const
Retrieve the number of batches processed as the raw data. (numBatches_)
Definition: BITstar.cpp:478
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
ompl::base::Cost bestCost() const
Retrieve the best exact-solution cost found.
Definition: BITstar.cpp:473
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met...
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
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
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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
LogLevel getLogLevel()
Retrieve the current level of logging data. Messages with lower logging levels will not be recorded...
Definition: Console.cpp:142
bool getPruning() const
Get whether graph and sample pruning is in use.
Definition: BITstar.cpp:1104
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
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
std::pair< VertexPtr, VertexPtr > VertexPtrPair
A pair of vertices, i.e., an edge.
Definition: BITstar.h:136
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
The exception type for ompl.
Definition: Exception.h:46
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
void clear() override
Clear.
Definition: BITstar.cpp:297
void setup() override
Setup.
Definition: BITstar.cpp:241
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared const pointers.
Definition: BITstar.h:132
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
bool markGoalState(const State *st)
Mark the given state as a goal vertex. If the given state does not exist in a vertex, false is returned.
bool getConsiderApproximateSolutions() const
Get whether BIT* is considering approximate solutions.
Definition: BITstar.cpp:1173
bool getUseKNearest() const
Get whether a k-nearest search is being used.
Definition: BITstar.cpp:1075
double value() const
The value of the cost.
Definition: Cost.h:56
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
void setDropSamplesOnPrune(bool dropSamples)
Drop all unconnected samples when pruning, regardless of their heuristic value. This provides a metho...
Definition: BITstar.cpp:1144
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
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
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
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
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
Definition: GoalTypes.h:56
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68