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