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