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