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