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