SPARS.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Rutgers the State University of New Jersey, New Brunswick
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 Rutgers University 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 /* Author: Andrew Dobson */
36 
37 #include "ompl/geometric/planners/prm/SPARS.h"
38 #include "ompl/geometric/planners/prm/ConnectionStrategy.h"
39 #include "ompl/base/goals/GoalSampleableRegion.h"
40 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
41 #include "ompl/tools/config/SelfConfig.h"
42 #include "ompl/tools/config/MagicConstants.h"
43 #include <thread>
44 #include <boost/graph/astar_search.hpp>
45 #include <boost/graph/incremental_components.hpp>
46 #include <boost/property_map/vector_property_map.hpp>
47 #include <boost/foreach.hpp>
48 
49 #include "GoalVisitor.hpp"
50 
51 #define foreach BOOST_FOREACH
52 #define foreach_reverse BOOST_REVERSE_FOREACH
53 
54 ompl::geometric::SPARS::SPARS(const base::SpaceInformationPtr &si)
55  : base::Planner(si, "SPARS")
56  , geomPath_(si)
57  , stateProperty_(boost::get(vertex_state_t(), g_))
58  , sparseStateProperty_(boost::get(vertex_state_t(), s_))
59  , sparseColorProperty_(boost::get(vertex_color_t(), s_))
60  , representativesProperty_(boost::get(vertex_representative_t(), g_))
61  , nonInterfaceListsProperty_(boost::get(vertex_list_t(), s_))
62  , interfaceListsProperty_(boost::get(vertex_interface_list_t(), s_))
63  , weightProperty_(boost::get(boost::edge_weight, g_))
64  , sparseDJSets_(boost::get(boost::vertex_rank, s_), boost::get(boost::vertex_predecessor, s_))
65 {
68  specs_.optimizingPaths = true;
69  specs_.multithreaded = true;
70 
71  psimp_ = std::make_shared<PathSimplifier>(si_);
72  psimp_->freeStates(false);
73 
74  Planner::declareParam<double>("stretch_factor", this, &SPARS::setStretchFactor, &SPARS::getStretchFactor, "1.1:0.1:"
75  "3.0");
76  Planner::declareParam<double>("sparse_delta_fraction", this, &SPARS::setSparseDeltaFraction,
77  &SPARS::getSparseDeltaFraction, "0.0:0.01:1.0");
78  Planner::declareParam<double>("dense_delta_fraction", this, &SPARS::setDenseDeltaFraction,
79  &SPARS::getDenseDeltaFraction, "0.0:0.0001:0.1");
80  Planner::declareParam<unsigned int>("max_failures", this, &SPARS::setMaxFailures, &SPARS::getMaxFailures, "100:10:"
81  "3000");
82 
83  addPlannerProgressProperty("iterations INTEGER", [this]
84  {
85  return getIterationCount();
86  });
87  addPlannerProgressProperty("best cost REAL", [this]
88  {
89  return getBestCost();
90  });
91 }
92 
94 {
95  freeMemory();
96 }
97 
99 {
100  Planner::setup();
101  if (!nn_)
102  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<DenseVertex>(this));
103  nn_->setDistanceFunction([this](const DenseVertex a, const DenseVertex b)
104  {
105  return distanceFunction(a, b);
106  });
107  if (!snn_)
108  snn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<SparseVertex>(this));
109  snn_->setDistanceFunction([this](const SparseVertex a, const SparseVertex b)
110  {
111  return sparseDistanceFunction(a, b);
112  });
113  if (!connectionStrategy_)
114  connectionStrategy_ = KStarStrategy<DenseVertex>(
115  [this]
116  {
117  return milestoneCount();
118  },
119  nn_, si_->getStateDimension());
120  double maxExt = si_->getMaximumExtent();
121  sparseDelta_ = sparseDeltaFraction_ * maxExt;
122  denseDelta_ = denseDeltaFraction_ * maxExt;
123 
124  // Setup optimization objective
125  //
126  // If no optimization objective was specified, then default to
127  // optimizing path length as computed by the distance() function
128  // in the state space.
129  if (pdef_)
130  {
131  if (pdef_->hasOptimizationObjective())
132  {
133  opt_ = pdef_->getOptimizationObjective();
134  if (dynamic_cast<base::PathLengthOptimizationObjective *>(opt_.get()) == nullptr)
135  OMPL_WARN("%s: Asymptotic optimality has only been proven with path length optimizaton; convergence "
136  "for other optimizaton objectives is not guaranteed.",
137  getName().c_str());
138  }
139  else
140  opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
141  }
142  else
143  {
144  OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
145  setup_ = false;
146  }
147 }
148 
149 void ompl::geometric::SPARS::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
150 {
151  Planner::setProblemDefinition(pdef);
152  clearQuery();
153 }
154 
156 {
157  consecutiveFailures_ = 0;
158 }
159 
161 {
162  startM_.clear();
163  goalM_.clear();
164  pis_.restart();
165 
166  // Clear past solutions if there are any
167  if (pdef_)
168  pdef_->clearSolutionPaths();
169 }
170 
172 {
173  Planner::clear();
174  sampler_.reset();
175  freeMemory();
176  if (nn_)
177  nn_->clear();
178  if (snn_)
179  snn_->clear();
180  clearQuery();
181  resetFailures();
182  iterations_ = 0;
183  bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
184 }
185 
187 {
188  foreach (DenseVertex v, boost::vertices(g_))
189  if (stateProperty_[v] != nullptr)
190  {
191  si_->freeState(stateProperty_[v]);
192  stateProperty_[v] = nullptr;
193  }
194  foreach (SparseVertex n, boost::vertices(s_))
195  if (sparseStateProperty_[n] != nullptr)
196  {
197  si_->freeState(sparseStateProperty_[n]);
198  sparseStateProperty_[n] = nullptr;
199  }
200  s_.clear();
201  g_.clear();
202 }
203 
206 {
207  DenseVertex result = boost::graph_traits<DenseGraph>::null_vertex();
208 
209  // search for a valid state
210  bool found = false;
211  while (!found && !ptc)
212  {
213  unsigned int attempts = 0;
214  do
215  {
216  found = sampler_->sample(workState);
217  attempts++;
218  } while (attempts < magic::FIND_VALID_STATE_ATTEMPTS_WITHOUT_TERMINATION_CHECK && !found);
219  }
220 
221  if (found)
222  result = addMilestone(si_->cloneState(workState));
223  return result;
224 }
225 
227 {
228  auto *goal = static_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
229  while (!ptc && !addedSolution_)
230  {
231  // Check for any new goal states
232  if (goal->maxSampleCount() > goalM_.size())
233  {
234  const base::State *st = pis_.nextGoal();
235  if (st != nullptr)
236  {
237  addMilestone(si_->cloneState(st));
238  goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
239  }
240  }
241 
242  // Check for a solution
243  addedSolution_ = haveSolution(startM_, goalM_, solution);
244  // Sleep for 1ms
245  if (!addedSolution_)
246  std::this_thread::sleep_for(std::chrono::milliseconds(1));
247  }
248 }
249 
250 bool ompl::geometric::SPARS::haveSolution(const std::vector<DenseVertex> &starts, const std::vector<DenseVertex> &goals,
251  base::PathPtr &solution)
252 {
253  base::Goal *g = pdef_->getGoal().get();
254  base::Cost sol_cost(opt_->infiniteCost());
255  foreach (DenseVertex start, starts)
256  {
257  foreach (DenseVertex goal, goals)
258  {
259  // we lock because the connected components algorithm is incremental and may change disjointSets_
260  graphMutex_.lock();
261  bool same_component = sameComponent(start, goal);
262  graphMutex_.unlock();
263 
264  if (same_component && g->isStartGoalPairValid(sparseStateProperty_[goal], sparseStateProperty_[start]))
265  {
266  base::PathPtr p = constructSolution(start, goal);
267  if (p)
268  {
269  base::Cost pathCost = p->cost(opt_);
270  if (opt_->isCostBetterThan(pathCost, bestCost_))
271  bestCost_ = pathCost;
272  // Check if optimization objective is satisfied
273  if (opt_->isSatisfied(pathCost))
274  {
275  solution = p;
276  return true;
277  }
278  if (opt_->isCostBetterThan(pathCost, sol_cost))
279  {
280  solution = p;
281  sol_cost = pathCost;
282  }
283  }
284  }
285  }
286  }
287 
288  return false;
289 }
290 
292 {
293  return consecutiveFailures_ >= maxFailures_ || addedSolution_;
294 }
295 
297 {
298  return consecutiveFailures_ >= maxFailures_;
299 }
300 
302 {
303  std::lock_guard<std::mutex> _(graphMutex_);
304  if (boost::num_vertices(g_) < 1)
305  {
306  sparseQueryVertex_ = boost::add_vertex(s_);
307  queryVertex_ = boost::add_vertex(g_);
308  sparseStateProperty_[sparseQueryVertex_] = nullptr;
309  stateProperty_[queryVertex_] = nullptr;
310  }
311 }
312 
314 {
315  return boost::same_component(m1, m2, sparseDJSets_);
316 }
317 
319 {
320  checkValidity();
321  checkQueryStateInitialization();
322 
323  auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
324 
325  if (goal == nullptr)
326  {
327  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
329  }
330 
331  // Add the valid start states as milestones
332  while (const base::State *st = pis_.nextStart())
333  {
334  addMilestone(si_->cloneState(st));
335  startM_.push_back(addGuard(si_->cloneState(st), START));
336  }
337  if (startM_.empty())
338  {
339  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
341  }
342 
343  if (goalM_.empty() && !goal->couldSample())
344  {
345  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
347  }
348 
349  // Add the valid goal states as milestones
350  while (const base::State *st = (goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal()))
351  {
352  addMilestone(si_->cloneState(st));
353  goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
354  }
355  if (goalM_.empty())
356  {
357  OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
359  }
360 
361  unsigned int nrStartStatesDense = boost::num_vertices(g_) - 1; // don't count query vertex
362  unsigned int nrStartStatesSparse = boost::num_vertices(s_) - 1; // don't count query vertex
363  OMPL_INFORM("%s: Starting planning with %u dense states, %u sparse states", getName().c_str(), nrStartStatesDense,
364  nrStartStatesSparse);
365 
366  // Reset addedSolution_ member
367  addedSolution_ = false;
368  resetFailures();
369  base::PathPtr sol;
370  base::PlannerTerminationCondition ptcOrFail([this, &ptc]
371  {
372  return ptc || reachedFailureLimit();
373  });
374  std::thread slnThread([this, &ptcOrFail, &sol]
375  {
376  checkForSolution(ptcOrFail, sol);
377  });
378 
379  // Construct planner termination condition which also takes maxFailures_ and addedSolution_ into account
380  base::PlannerTerminationCondition ptcOrStop([this, &ptc]
381  {
382  return ptc || reachedTerminationCriterion();
383  });
384  constructRoadmap(ptcOrStop);
385 
386  // Ensure slnThread is ceased before exiting solve
387  slnThread.join();
388 
389  if (sol)
390  pdef_->addSolutionPath(sol, false, -1.0, getName());
391 
392  OMPL_INFORM("%s: Created %u dense states, %u sparse states", getName().c_str(),
393  (unsigned int)(boost::num_vertices(g_) - nrStartStatesDense),
394  (unsigned int)(boost::num_vertices(s_) - nrStartStatesSparse));
395 
396  // Return true if any solution was found.
398 }
399 
401 {
402  if (stopOnMaxFail)
403  {
404  resetFailures();
405  base::PlannerTerminationCondition ptcOrFail([this, &ptc]
406  {
407  return ptc || reachedFailureLimit();
408  });
409  constructRoadmap(ptcOrFail);
410  }
411  else
412  constructRoadmap(ptc);
413 }
414 
416 {
417  checkQueryStateInitialization();
418 
419  if (!isSetup())
420  setup();
421  if (!sampler_)
422  sampler_ = si_->allocValidStateSampler();
423 
424  base::State *workState = si_->allocState();
425 
426  /* The whole neighborhood set which has been most recently computed */
427  std::vector<SparseVertex> graphNeighborhood;
428 
429  /* The visible neighborhood set which has been most recently computed */
430  std::vector<SparseVertex> visibleNeighborhood;
431 
432  /* Storage for the interface neighborhood, populated by getInterfaceNeighborhood() */
433  std::vector<DenseVertex> interfaceNeighborhood;
434 
435  bestCost_ = opt_->infiniteCost();
436  while (!ptc)
437  {
438  iterations_++;
439 
440  // Generate a single sample, and attempt to connect it to nearest neighbors.
441  DenseVertex q = addSample(workState, ptc);
442  if (q == boost::graph_traits<DenseGraph>::null_vertex())
443  continue;
444 
445  // Now that we've added to D, try adding to S
446  // Start by figuring out who our neighbors are
447  getSparseNeighbors(workState, graphNeighborhood);
448  filterVisibleNeighbors(workState, graphNeighborhood, visibleNeighborhood);
449  // Check for addition for Coverage
450  if (!checkAddCoverage(workState, graphNeighborhood))
451  // If not for Coverage, then Connectivity
452  if (!checkAddConnectivity(workState, graphNeighborhood))
453  // Check for the existence of an interface
454  if (!checkAddInterface(graphNeighborhood, visibleNeighborhood, q))
455  {
456  // Then check to see if it's on an interface
457  getInterfaceNeighborhood(q, interfaceNeighborhood);
458  if (!interfaceNeighborhood.empty())
459  {
460  // Check for addition for spanner prop
461  if (!checkAddPath(q, interfaceNeighborhood))
462  // All of the tests have failed. Report failure for the sample
463  ++consecutiveFailures_;
464  }
465  else
466  // There's no interface here, so drop it
467  ++consecutiveFailures_;
468  }
469  }
470 
471  si_->freeState(workState);
472 }
473 
475 {
476  std::lock_guard<std::mutex> _(graphMutex_);
477 
478  DenseVertex m = boost::add_vertex(g_);
479  stateProperty_[m] = state;
480 
481  // Which milestones will we attempt to connect to?
482  const std::vector<DenseVertex> &neighbors = connectionStrategy_(m);
483 
484  foreach (DenseVertex n, neighbors)
485  if (si_->checkMotion(stateProperty_[m], stateProperty_[n]))
486  {
487  const double weight = distanceFunction(m, n);
488  const DenseGraph::edge_property_type properties(weight);
489 
490  boost::add_edge(m, n, properties, g_);
491  }
492 
493  nn_->add(m);
494 
495  // Need to update representative information here...
496  calculateRepresentative(m);
497 
498  std::vector<DenseVertex> interfaceNeighborhood;
499  std::set<SparseVertex> interfaceRepresentatives;
500 
501  getInterfaceNeighborRepresentatives(m, interfaceRepresentatives);
502  getInterfaceNeighborhood(m, interfaceNeighborhood);
503  addToRepresentatives(m, representativesProperty_[m], interfaceRepresentatives);
504  foreach (DenseVertex qp, interfaceNeighborhood)
505  {
506  removeFromRepresentatives(qp, representativesProperty_[qp]);
507  getInterfaceNeighborRepresentatives(qp, interfaceRepresentatives);
508  addToRepresentatives(qp, representativesProperty_[qp], interfaceRepresentatives);
509  }
510 
511  return m;
512 }
513 
515 {
516  std::lock_guard<std::mutex> _(graphMutex_);
517 
518  SparseVertex v = boost::add_vertex(s_);
519  sparseStateProperty_[v] = state;
520  sparseColorProperty_[v] = type;
521 
522  sparseDJSets_.make_set(v);
523 
524  snn_->add(v);
525  updateRepresentatives(v);
526 
527  resetFailures();
528  return v;
529 }
530 
532 {
533  const base::Cost weight(costHeuristic(v, vp));
534  const SpannerGraph::edge_property_type properties(weight);
535  std::lock_guard<std::mutex> _(graphMutex_);
536  boost::add_edge(v, vp, properties, s_);
537  sparseDJSets_.union_set(v, vp);
538 }
539 
541 {
542  const double weight = distanceFunction(v, vp);
543  const DenseGraph::edge_property_type properties(weight);
544  std::lock_guard<std::mutex> _(graphMutex_);
545  boost::add_edge(v, vp, properties, g_);
546 }
547 
548 bool ompl::geometric::SPARS::checkAddCoverage(const base::State *lastState, const std::vector<SparseVertex> &neigh)
549 {
550  // For each of these neighbors,
551  foreach (SparseVertex n, neigh)
552  // If path between is free
553  if (si_->checkMotion(lastState, sparseStateProperty_[n]))
554  // Abort out and return false
555  return false;
556  // No free paths means we add for coverage
557  addGuard(si_->cloneState(lastState), COVERAGE);
558  return true;
559 }
560 
561 bool ompl::geometric::SPARS::checkAddConnectivity(const base::State *lastState, const std::vector<SparseVertex> &neigh)
562 {
563  std::vector<SparseVertex> links;
564  // For each neighbor
565  for (std::size_t i = 0; i < neigh.size(); ++i)
566  // For each other neighbor
567  for (std::size_t j = i + 1; j < neigh.size(); ++j)
568  // If they are in different components
569  if (!sameComponent(neigh[i], neigh[j]))
570  // If the paths between are collision free
571  if (si_->checkMotion(lastState, sparseStateProperty_[neigh[i]]) &&
572  si_->checkMotion(lastState, sparseStateProperty_[neigh[j]]))
573  {
574  links.push_back(neigh[i]);
575  links.push_back(neigh[j]);
576  }
577 
578  if (!links.empty())
579  {
580  // Add the node
581  SparseVertex g = addGuard(si_->cloneState(lastState), CONNECTIVITY);
582 
583  for (unsigned long link : links)
584  // If there's no edge
585  if (!boost::edge(g, link, s_).second)
586  // And the components haven't been united by previous links
587  if (!sameComponent(link, g))
588  connectSparsePoints(g, link);
589  return true;
590  }
591  return false;
592 }
593 
594 bool ompl::geometric::SPARS::checkAddInterface(const std::vector<SparseVertex> &graphNeighborhood,
595  const std::vector<SparseVertex> &visibleNeighborhood, DenseVertex q)
596 {
597  // If we have more than 1 neighbor
598  if (visibleNeighborhood.size() > 1)
599  // If our closest neighbors are also visible
600  if (graphNeighborhood[0] == visibleNeighborhood[0] && graphNeighborhood[1] == visibleNeighborhood[1])
601  // If our two closest neighbors don't share an edge
602  if (!boost::edge(visibleNeighborhood[0], visibleNeighborhood[1], s_).second)
603  {
604  // If they can be directly connected
605  if (si_->checkMotion(sparseStateProperty_[visibleNeighborhood[0]],
606  sparseStateProperty_[visibleNeighborhood[1]]))
607  {
608  // Connect them
609  connectSparsePoints(visibleNeighborhood[0], visibleNeighborhood[1]);
610  // And report that we added to the roadmap
611  resetFailures();
612  // Report success
613  return true;
614  }
615 
616  // Add the new node to the graph, to bridge the interface
617  SparseVertex v = addGuard(si_->cloneState(stateProperty_[q]), INTERFACE);
618  connectSparsePoints(v, visibleNeighborhood[0]);
619  connectSparsePoints(v, visibleNeighborhood[1]);
620  // Report success
621  return true;
622  }
623  return false;
624 }
625 
626 bool ompl::geometric::SPARS::checkAddPath(DenseVertex q, const std::vector<DenseVertex> &neigh)
627 {
628  bool result = false;
629 
630  // Get q's representative => v
631  SparseVertex v = representativesProperty_[q];
632 
633  // Extract the representatives of neigh => n_rep
634  std::set<SparseVertex> n_rep;
635  foreach (DenseVertex qp, neigh)
636  n_rep.insert(representativesProperty_[qp]);
637 
638  std::vector<SparseVertex> Xs;
639  // for each v' in n_rep
640  for (auto it = n_rep.begin(); it != n_rep.end() && !result; ++it)
641  {
642  SparseVertex vp = *it;
643  // Identify appropriate v" candidates => vpps
644  std::vector<SparseVertex> VPPs;
645  computeVPP(v, vp, VPPs);
646 
647  foreach (SparseVertex vpp, VPPs)
648  {
649  double s_max = 0;
650  // Find the X nodes to test
651  computeX(v, vp, vpp, Xs);
652 
653  // For each x in xs
654  foreach (SparseVertex x, Xs)
655  {
656  // Compute/Retain MAXimum distance path thorugh S
657  double dist = (si_->distance(sparseStateProperty_[x], sparseStateProperty_[v]) +
658  si_->distance(sparseStateProperty_[v], sparseStateProperty_[vp])) /
659  2.0;
660  if (dist > s_max)
661  s_max = dist;
662  }
663 
664  DensePath bestDPath;
665  DenseVertex best_qpp = boost::graph_traits<DenseGraph>::null_vertex();
666  double d_min = std::numeric_limits<double>::infinity(); // Insanely big number
667  // For each vpp in vpps
668  for (std::size_t j = 0; j < VPPs.size() && !result; ++j)
669  {
670  SparseVertex vpp = VPPs[j];
671  // For each q", which are stored interface nodes on v for i(vpp,v)
672  foreach (DenseVertex qpp, interfaceListsProperty_[v][vpp])
673  {
674  // check that representatives are consistent
675  assert(representativesProperty_[qpp] == v);
676 
677  // If they happen to be the one and same node
678  if (q == qpp)
679  {
680  bestDPath.push_front(stateProperty_[q]);
681  best_qpp = qpp;
682  d_min = 0;
683  }
684  else
685  {
686  // Compute/Retain MINimum distance path on D through q, q"
687  DensePath dPath;
688  computeDensePath(q, qpp, dPath);
689  if (!dPath.empty())
690  {
691  // compute path length
692  double length = 0.0;
693  DensePath::const_iterator jt = dPath.begin();
694  for (auto it = jt + 1; it != dPath.end(); ++it)
695  {
696  length += si_->distance(*jt, *it);
697  jt = it;
698  }
699 
700  if (length < d_min)
701  {
702  d_min = length;
703  bestDPath.swap(dPath);
704  best_qpp = qpp;
705  }
706  }
707  }
708  }
709 
710  // If the spanner property is violated for these paths
711  if (s_max > stretchFactor_ * d_min)
712  {
713  // Need to augment this path with the appropriate neighbor information
714  DenseVertex na = getInterfaceNeighbor(q, vp);
715  DenseVertex nb = getInterfaceNeighbor(best_qpp, vpp);
716 
717  bestDPath.push_front(stateProperty_[na]);
718  bestDPath.push_back(stateProperty_[nb]);
719 
720  // check consistency of representatives
721  assert(representativesProperty_[na] == vp && representativesProperty_[nb] == vpp);
722 
723  // Add the dense path to the spanner
724  addPathToSpanner(bestDPath, vpp, vp);
725 
726  // Report success
727  result = true;
728  }
729  }
730  }
731  }
732  return result;
733 }
734 
736 {
737  double degree = 0.0;
738  foreach (DenseVertex v, boost::vertices(s_))
739  degree += (double)boost::out_degree(v, s_);
740  degree /= (double)boost::num_vertices(s_);
741  return degree;
742 }
743 
744 void ompl::geometric::SPARS::printDebug(std::ostream &out) const
745 {
746  out << "SPARS Debug Output: " << std::endl;
747  out << " Settings: " << std::endl;
748  out << " Max Failures: " << getMaxFailures() << std::endl;
749  out << " Dense Delta Fraction: " << getDenseDeltaFraction() << std::endl;
750  out << " Sparse Delta Fraction: " << getSparseDeltaFraction() << std::endl;
751  out << " Stretch Factor: " << getStretchFactor() << std::endl;
752  out << " Status: " << std::endl;
753  out << " Milestone Count: " << milestoneCount() << std::endl;
754  out << " Guard Count: " << guardCount() << std::endl;
755  out << " Iterations: " << getIterationCount() << std::endl;
756  out << " Average Valence: " << averageValence() << std::endl;
757  out << " Consecutive Failures: " << consecutiveFailures_ << std::endl;
758 }
759 
760 void ompl::geometric::SPARS::getSparseNeighbors(base::State *inState, std::vector<SparseVertex> &graphNeighborhood)
761 {
762  sparseStateProperty_[sparseQueryVertex_] = inState;
763 
764  graphNeighborhood.clear();
765  snn_->nearestR(sparseQueryVertex_, sparseDelta_, graphNeighborhood);
766 
767  sparseStateProperty_[sparseQueryVertex_] = nullptr;
768 }
769 
771  const std::vector<SparseVertex> &graphNeighborhood,
772  std::vector<SparseVertex> &visibleNeighborhood) const
773 {
774  visibleNeighborhood.clear();
775  // Now that we got the neighbors from the NN, we must remove any we can't see
776  for (unsigned long i : graphNeighborhood)
777  if (si_->checkMotion(inState, sparseStateProperty_[i]))
778  visibleNeighborhood.push_back(i);
779 }
780 
782 {
783  foreach (DenseVertex vp, boost::adjacent_vertices(q, g_))
784  if (representativesProperty_[vp] == rep)
785  if (distanceFunction(q, vp) <= denseDelta_)
786  return vp;
787  throw Exception(name_, "Vertex has no interface neighbor with given representative");
788 }
789 
791 {
792  // First, check to see that the path has length
793  if (dense_path.size() <= 1)
794  {
795  // The path is 0 length, so simply link the representatives
796  connectSparsePoints(vp, vpp);
797  resetFailures();
798  }
799  else
800  {
801  // We will need to construct a PathGeometric to do this.
802  geomPath_.getStates().resize(dense_path.size());
803  std::copy(dense_path.begin(), dense_path.end(), geomPath_.getStates().begin());
804 
805  // Attempt to simplify the path
806  psimp_->reduceVertices(geomPath_, geomPath_.getStateCount() * 2);
807 
808  // we are sure there are at least 2 points left on geomPath_
809 
810  std::vector<SparseVertex> added_nodes;
811  added_nodes.reserve(geomPath_.getStateCount());
812  for (std::size_t i = 0; i < geomPath_.getStateCount(); ++i)
813  {
814  // Add each guard
815  SparseVertex ng = addGuard(si_->cloneState(geomPath_.getState(i)), QUALITY);
816  added_nodes.push_back(ng);
817  }
818  // Link them up
819  for (std::size_t i = 1; i < added_nodes.size(); ++i)
820  {
821  connectSparsePoints(added_nodes[i - 1], added_nodes[i]);
822  }
823  // Don't forget to link them to their representatives
824  connectSparsePoints(added_nodes[0], vp);
825  connectSparsePoints(added_nodes[added_nodes.size() - 1], vpp);
826  }
827  geomPath_.getStates().clear();
828  return true;
829 }
830 
832 {
833  // Get all of the dense samples which may be affected by adding this node
834  std::vector<DenseVertex> dense_points;
835 
836  stateProperty_[queryVertex_] = sparseStateProperty_[v];
837 
838  nn_->nearestR(queryVertex_, sparseDelta_ + denseDelta_, dense_points);
839 
840  stateProperty_[queryVertex_] = nullptr;
841 
842  // For each of those points
843  for (unsigned long dense_point : dense_points)
844  {
845  // Remove that point from the old representative's list(s)
846  removeFromRepresentatives(dense_point, representativesProperty_[dense_point]);
847  // Update that point's representative
848  calculateRepresentative(dense_point);
849  }
850 
851  std::set<SparseVertex> interfaceRepresentatives;
852  // For each of the points
853  for (unsigned long dense_point : dense_points)
854  {
855  // Get it's representative
856  SparseVertex rep = representativesProperty_[dense_point];
857  // Extract the representatives of any interface-sharing neighbors
858  getInterfaceNeighborRepresentatives(dense_point, interfaceRepresentatives);
859  // For sanity's sake, make sure we clear ourselves out of what this new rep might think of us
860  removeFromRepresentatives(dense_point, rep);
861  // Add this vertex to it's representative's list for the other representatives
862  addToRepresentatives(dense_point, rep, interfaceRepresentatives);
863  }
864 }
865 
867 {
868  // Get the nearest neighbors within sparseDelta_
869  std::vector<SparseVertex> graphNeighborhood;
870  getSparseNeighbors(stateProperty_[q], graphNeighborhood);
871 
872  // For each neighbor
873  for (unsigned long i : graphNeighborhood)
874  if (si_->checkMotion(stateProperty_[q], sparseStateProperty_[i]))
875  {
876  // update the representative
877  representativesProperty_[q] = i;
878  // abort
879  break;
880  }
881 }
882 
883 void ompl::geometric::SPARS::addToRepresentatives(DenseVertex q, SparseVertex rep, const std::set<SparseVertex> &oreps)
884 {
885  // If this node supports no interfaces
886  if (oreps.empty())
887  {
888  // Add it to the pool of non-interface nodes
889  bool new_insert = nonInterfaceListsProperty_[rep].insert(q).second;
890 
891  // we expect this was not previously tracked
892  if (!new_insert)
893  assert(false);
894  }
895  else
896  {
897  // otherwise, for every neighbor representative
898  foreach (SparseVertex v, oreps)
899  {
900  assert(rep == representativesProperty_[q]);
901  bool new_insert = interfaceListsProperty_[rep][v].insert(q).second;
902  if (!new_insert)
903  assert(false);
904  }
905  }
906 }
907 
909 {
910  // Remove the node from the non-interface points (if there)
911  nonInterfaceListsProperty_[rep].erase(q);
912 
913  // From each of the interfaces
914  foreach (SparseVertex vpp, interfaceListsProperty_[rep] | boost::adaptors::map_keys)
915  {
916  // Remove this node from that list
917  interfaceListsProperty_[rep][vpp].erase(q);
918  }
919 }
920 
921 void ompl::geometric::SPARS::computeVPP(SparseVertex v, SparseVertex vp, std::vector<SparseVertex> &VPPs)
922 {
923  foreach (SparseVertex cvpp, boost::adjacent_vertices(v, s_))
924  if (cvpp != vp)
925  if (!boost::edge(cvpp, vp, s_).second)
926  VPPs.push_back(cvpp);
927 }
928 
929 void ompl::geometric::SPARS::computeX(SparseVertex v, SparseVertex vp, SparseVertex vpp, std::vector<SparseVertex> &Xs)
930 {
931  Xs.clear();
932  foreach (SparseVertex cx, boost::adjacent_vertices(vpp, s_))
933  if (boost::edge(cx, v, s_).second && !boost::edge(cx, vp, s_).second)
934  if (!interfaceListsProperty_[vpp][cx].empty())
935  Xs.push_back(cx);
936  Xs.push_back(vpp);
937 }
938 
940  std::set<SparseVertex> &interfaceRepresentatives)
941 {
942  interfaceRepresentatives.clear();
943 
944  // Get our representative
945  SparseVertex rep = representativesProperty_[q];
946  // For each neighbor we are connected to
947  foreach (DenseVertex n, boost::adjacent_vertices(q, g_))
948  {
949  // Get his representative
950  SparseVertex orep = representativesProperty_[n];
951  // If that representative is not our own
952  if (orep != rep)
953  // If he is within denseDelta_
954  if (distanceFunction(q, n) < denseDelta_)
955  // Include his rep in the set
956  interfaceRepresentatives.insert(orep);
957  }
958 }
959 
960 void ompl::geometric::SPARS::getInterfaceNeighborhood(DenseVertex q, std::vector<DenseVertex> &interfaceNeighborhood)
961 {
962  interfaceNeighborhood.clear();
963 
964  // Get our representative
965  SparseVertex rep = representativesProperty_[q];
966 
967  // For each neighbor we are connected to
968  foreach (DenseVertex n, boost::adjacent_vertices(q, g_))
969  // If neighbor representative is not our own
970  if (representativesProperty_[n] != rep)
971  // If he is within denseDelta_
972  if (distanceFunction(q, n) < denseDelta_)
973  // Append him to the list
974  interfaceNeighborhood.push_back(n);
975 }
976 
978 {
979  std::lock_guard<std::mutex> _(graphMutex_);
980 
981  boost::vector_property_map<SparseVertex> prev(boost::num_vertices(s_));
982 
983  try
984  {
985  // Consider using a persistent distance_map if it's slow
986  boost::astar_search(s_, start,
987  [this, goal](SparseVertex v)
988  {
989  return costHeuristic(v, goal);
990  },
991  boost::predecessor_map(prev)
992  .distance_compare([this](base::Cost c1, base::Cost c2)
993  {
994  return opt_->isCostBetterThan(c1, c2);
995  })
996  .distance_combine([this](base::Cost c1, base::Cost c2)
997  {
998  return opt_->combineCosts(c1, c2);
999  })
1000  .distance_inf(opt_->infiniteCost())
1001  .distance_zero(opt_->identityCost())
1002  .visitor(AStarGoalVisitor<SparseVertex>(goal)));
1003  }
1004  catch (AStarFoundGoal &)
1005  {
1006  }
1007 
1008  if (prev[goal] == goal)
1009  throw Exception(name_, "Could not find solution path");
1010  else
1011  {
1012  auto p(std::make_shared<PathGeometric>(si_));
1013 
1014  for (SparseVertex pos = goal; prev[pos] != pos; pos = prev[pos])
1015  p->append(sparseStateProperty_[pos]);
1016  p->append(sparseStateProperty_[start]);
1017  p->reverse();
1018 
1019  return p;
1020  }
1021 }
1022 
1024 {
1025  path.clear();
1026 
1027  boost::vector_property_map<DenseVertex> prev(boost::num_vertices(g_));
1028 
1029  try
1030  {
1031  boost::astar_search(g_, start,
1032  [this, goal](const DenseVertex a)
1033  {
1034  return distanceFunction(a, goal);
1035  },
1036  boost::predecessor_map(prev).visitor(AStarGoalVisitor<DenseVertex>(goal)));
1037  }
1038  catch (AStarFoundGoal &)
1039  {
1040  }
1041 
1042  if (prev[goal] == goal)
1043  OMPL_WARN("%s: No dense path was found?", getName().c_str());
1044  else
1045  {
1046  for (DenseVertex pos = goal; prev[pos] != pos; pos = prev[pos])
1047  path.push_front(stateProperty_[pos]);
1048  path.push_front(stateProperty_[start]);
1049  }
1050 }
1051 
1053 {
1054  Planner::getPlannerData(data);
1055 
1056  // Explicitly add start and goal states:
1057  for (unsigned long i : startM_)
1058  data.addStartVertex(base::PlannerDataVertex(sparseStateProperty_[i], (int)START));
1059 
1060  for (unsigned long i : goalM_)
1061  data.addGoalVertex(base::PlannerDataVertex(sparseStateProperty_[i], (int)GOAL));
1062 
1063  // Adding edges and all other vertices simultaneously
1064  foreach (const SparseEdge e, boost::edges(s_))
1065  {
1066  const SparseVertex v1 = boost::source(e, s_);
1067  const SparseVertex v2 = boost::target(e, s_);
1068  data.addEdge(base::PlannerDataVertex(sparseStateProperty_[v1], (int)sparseColorProperty_[v1]),
1069  base::PlannerDataVertex(sparseStateProperty_[v2], (int)sparseColorProperty_[v2]));
1070 
1071  // Add the reverse edge, since we're constructing an undirected roadmap
1072  data.addEdge(base::PlannerDataVertex(sparseStateProperty_[v2], (int)sparseColorProperty_[v1]),
1073  base::PlannerDataVertex(sparseStateProperty_[v1], (int)sparseColorProperty_[v2]));
1074  }
1075 
1076  // Make sure to add edge-less nodes as well
1077  foreach (const SparseVertex n, boost::vertices(s_))
1078  if (boost::out_degree(n, s_) == 0)
1079  data.addVertex(base::PlannerDataVertex(sparseStateProperty_[n], (int)sparseColorProperty_[n]));
1080 }
1081 
1083 {
1084  return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
1085 }
void connectDensePoints(DenseVertex v, DenseVertex vp)
Connects points in the dense graph.
Definition: SPARS.cpp:540
void calculateRepresentative(DenseVertex q)
Calculates the representative for a dense sample.
Definition: SPARS.cpp:866
PathSimplifierPtr psimp_
A path simplifier used to simplify dense paths added to S.
Definition: SPARS.h:531
unsigned getMaxFailures() const
Retrieve the maximum consecutive failure limit.
Definition: SPARS.h:298
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
A shared pointer wrapper for ompl::base::Path.
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: SPARS.cpp:171
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:199
SparseVertex addGuard(base::State *state, GuardType type)
Construct a node with the given state (state) for the spanner and store it in the nn structure.
Definition: SPARS.cpp:514
void setStretchFactor(double t)
Set the roadmap spanner stretch factor. This value represents a multiplicative upper bound on path qu...
Definition: SPARS.h:292
void freeMemory()
Free all the memory allocated by the planner.
Definition: SPARS.cpp:186
An optimization objective which corresponds to optimizing path length.
Definition of an abstract state.
Definition: State.h:50
double getDenseDeltaFraction() const
Retrieve the dense graph interface support delta fraction.
Definition: SPARS.h:304
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
Definition: SPARS.cpp:226
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
void updateRepresentatives(SparseVertex v)
Automatically updates the representatives of all dense samplse within sparseDelta_ of v.
Definition: SPARS.cpp:831
GuardType
Enumeration which specifies the reason a guard is added to the spanner.
Definition: SPARS.h:82
boost::graph_traits< SpannerGraph >::vertex_descriptor SparseVertex
A vertex in the sparse roadmap that is constructed.
Definition: SPARS.h:151
bool checkAddConnectivity(const base::State *lastState, const std::vector< SparseVertex > &neigh)
Checks the latest dense sample for connectivity, and adds appropriately.
Definition: SPARS.cpp:561
void getInterfaceNeighborRepresentatives(DenseVertex q, std::set< SparseVertex > &interfaceRepresentatives)
Gets the representatives of all interfaces that q supports.
Definition: SPARS.cpp:939
boost::graph_traits< DenseGraph >::vertex_descriptor DenseVertex
A vertex in DenseGraph.
Definition: SPARS.h:184
base::Cost costHeuristic(SparseVertex u, SparseVertex v) const
Given two vertices, returns a heuristic on the cost of the path connecting them. This method wraps Op...
Definition: SPARS.cpp:1082
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
void resetFailures()
A reset function for resetting the failures count.
Definition: SPARS.cpp:155
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
@ TIMEOUT
The planner failed to find a solution.
Definition: PlannerStatus.h:62
Make the minimal number of connections required to ensure asymptotic optimality.
DenseVertex addMilestone(base::State *state)
Construct a milestone for a given state (state) and store it in the nearest neighbors data structure.
Definition: SPARS.cpp:474
void clearQuery() override
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition: SPARS.cpp:160
bool checkAddPath(DenseVertex q, const std::vector< DenseVertex > &neigh)
Checks for adding an entire dense path to the Sparse Roadmap.
Definition: SPARS.cpp:626
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
~SPARS() override
Destructor.
Definition: SPARS.cpp:93
bool haveSolution(const std::vector< DenseVertex > &starts, const std::vector< DenseVertex > &goals, base::PathPtr &solution)
Check if there exists a solution, i.e., there exists a pair of milestones such that the first is in s...
Definition: SPARS.cpp:250
void computeVPP(DenseVertex v, DenseVertex vp, std::vector< SparseVertex > &VPPs)
Computes all nodes which qualify as a candidate v" for v and vp.
Definition: SPARS.cpp:921
base::PathPtr constructSolution(SparseVertex start, SparseVertex goal) const
Given two milestones from the same connected component, construct a path connecting them and set it a...
Definition: SPARS.cpp:977
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void filterVisibleNeighbors(base::State *inState, const std::vector< SparseVertex > &graphNeighborhood, std::vector< SparseVertex > &visibleNeighborhood) const
Get the visible neighbors.
Definition: SPARS.cpp:770
void setMaxFailures(unsigned int m)
Set the maximum consecutive failures to augment the spanner before termination. In general,...
Definition: SPARS.h:261
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:429
bool addPathToSpanner(const DensePath &dense_path, SparseVertex vp, SparseVertex vpp)
Method for actually adding a dense path to the Roadmap Spanner, S.
Definition: SPARS.cpp:790
bool checkAddCoverage(const base::State *lastState, const std::vector< SparseVertex > &neigh)
Checks the latest dense sample for the coverage property, and adds appropriately.
Definition: SPARS.cpp:548
@ INVALID_GOAL
Invalid goal state.
Definition: PlannerStatus.h:58
double getSparseDeltaFraction() const
Retrieve the sparse graph visibility range delta fraction.
Definition: SPARS.h:310
std::deque< base::State * > DensePath
Internal representation of a dense path.
Definition: SPARS.h:123
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:206
void addToRepresentatives(DenseVertex q, SparseVertex rep, const std::set< SparseVertex > &oreps)
Adds a dense sample to the appropriate lists of its representative.
Definition: SPARS.cpp:883
void getSparseNeighbors(base::State *inState, std::vector< SparseVertex > &graphNeighborhood)
Get all nodes in the sparse graph which are within sparseDelta_ of the given state.
Definition: SPARS.cpp:760
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
void removeFromRepresentatives(DenseVertex q, SparseVertex rep)
Removes the node from its representative's lists.
Definition: SPARS.cpp:908
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: SPARS.cpp:318
bool reachedFailureLimit() const
Returns true if we have reached the iteration failures limit, maxFailures_
Definition: SPARS.cpp:296
void setSparseDeltaFraction(double d)
Set the delta fraction for connection distance on the sparse spanner. This value represents the visib...
Definition: SPARS.h:280
double averageValence() const
Returns the average valence of the spanner graph.
Definition: SPARS.cpp:735
double getStretchFactor() const
Retrieve the spanner's set stretch factor.
Definition: SPARS.h:316
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition permits, construct the spanner graph.
Definition: SPARS.cpp:415
Abstract definition of goals.
Definition: Goal.h:63
#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
DenseVertex addSample(base::State *workState, const base::PlannerTerminationCondition &ptc)
Attempt to add a single sample to the roadmap.
Definition: SPARS.cpp:204
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
void computeX(DenseVertex v, DenseVertex vp, DenseVertex vpp, std::vector< SparseVertex > &Xs)
Computes all nodes which qualify as a candidate x for v, v', and v".
Definition: SPARS.cpp:929
@ EXACT_SOLUTION
The planner found an exact solution.
Definition: PlannerStatus.h:66
void setDenseDeltaFraction(double d)
Set the delta fraction for interface detection. If two nodes in the dense graph are more than a delta...
Definition: SPARS.h:269
void checkQueryStateInitialization()
Check that the query vertex is initialized (used for internal nearest neighbor searches)
Definition: SPARS.cpp:301
void getInterfaceNeighborhood(DenseVertex q, std::vector< DenseVertex > &interfaceNeighborhood)
Gets the neighbors of q who help it support an interface.
Definition: SPARS.cpp:960
DenseVertex getInterfaceNeighbor(DenseVertex q, SparseVertex rep)
Get the first neighbor of q who has representative rep and is within denseDelta_.
Definition: SPARS.cpp:781
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: SPARS.cpp:1052
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:417
boost::graph_traits< SpannerGraph >::edge_descriptor SparseEdge
An edge in the sparse roadmap that is constructed.
Definition: SPARS.h:154
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
void connectSparsePoints(SparseVertex v, SparseVertex vp)
Convenience function for creating an edge in the Spanner Roadmap.
Definition: SPARS.cpp:531
bool checkAddInterface(const std::vector< DenseVertex > &graphNeighborhood, const std::vector< DenseVertex > &visibleNeighborhood, DenseVertex q)
Checks the latest dense sample for bridging an edge-less interface.
Definition: SPARS.cpp:594
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:202
virtual bool isStartGoalPairValid(const State *, const State *) const
Since there can be multiple starting states (and multiple goal states) it is possible certain pairs a...
Definition: Goal.h:136
void computeDensePath(DenseVertex start, DenseVertex goal, DensePath &path) const
Constructs the dense path between the start and goal vertices (if connected)
Definition: SPARS.cpp:1023
void printDebug(std::ostream &out=std::cout) const
Print debug information about planner.
Definition: SPARS.cpp:744
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
static const unsigned int FIND_VALID_STATE_ATTEMPTS_WITHOUT_TERMINATION_CHECK
Maximum number of sampling attempts to find a valid state, without checking whether the allowed time ...
Abstract definition of a goal region that can be sampled.
The exception type for ompl.
Definition: Exception.h:47
bool sameComponent(SparseVertex m1, SparseVertex m2)
Check that two vertices are in the same connected component.
Definition: SPARS.cpp:313
SPARS(const base::SpaceInformationPtr &si)
Constructor.
Definition: SPARS.cpp:54
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: SPARS.cpp:98
@ INVALID_START
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
bool reachedTerminationCriterion() const
Returns true if we have reached the iteration failures limit, maxFailures_ or if a solution was added...
Definition: SPARS.cpp:291
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:59
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:196