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