SPARStwo.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/SPARStwo.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 <boost/graph/astar_search.hpp>
43 #include <boost/graph/incremental_components.hpp>
44 #include <boost/property_map/vector_property_map.hpp>
45 #include <boost/foreach.hpp>
46 #include <thread>
47 
48 #include "GoalVisitor.hpp"
49 
50 #define foreach BOOST_FOREACH
51 #define foreach_reverse BOOST_REVERSE_FOREACH
52 
54  : base::Planner(si, "SPARStwo")
55  , nearSamplePoints_((2 * si_->getStateDimension()))
56  , stateProperty_(boost::get(vertex_state_t(), g_))
57  , weightProperty_(boost::get(boost::edge_weight, g_))
58  , colorProperty_(boost::get(vertex_color_t(), g_))
59  , interfaceDataProperty_(boost::get(vertex_interface_data_t(), g_))
60  , disjointSets_(boost::get(boost::vertex_rank, g_), boost::get(boost::vertex_predecessor, g_))
61 {
64  specs_.optimizingPaths = true;
65  specs_.multithreaded = true;
66 
67  psimp_ = std::make_shared<PathSimplifier>(si_);
68 
69  Planner::declareParam<double>("stretch_factor", this, &SPARStwo::setStretchFactor, &SPARStwo::getStretchFactor,
70  "1.1:0.1:3.0");
71  Planner::declareParam<double>("sparse_delta_fraction", this, &SPARStwo::setSparseDeltaFraction,
72  &SPARStwo::getSparseDeltaFraction, "0.0:0.01:1.0");
73  Planner::declareParam<double>("dense_delta_fraction", this, &SPARStwo::setDenseDeltaFraction,
74  &SPARStwo::getDenseDeltaFraction, "0.0:0.0001:0.1");
75  Planner::declareParam<unsigned int>("max_failures", this, &SPARStwo::setMaxFailures, &SPARStwo::getMaxFailures,
76  "100:10:3000");
77 
78  addPlannerProgressProperty("iterations INTEGER", [this]
79  {
80  return getIterationCount();
81  });
82  addPlannerProgressProperty("best cost REAL", [this]
83  {
84  return getBestCost();
85  });
86 }
87 
89 {
90  freeMemory();
91 }
92 
94 {
95  Planner::setup();
96  if (!nn_)
97  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
98  nn_->setDistanceFunction([this](const Vertex a, const Vertex b)
99  {
100  return distanceFunction(a, b);
101  });
102  double maxExt = si_->getMaximumExtent();
104  denseDelta_ = denseDeltaFraction_ * maxExt;
105 
106  // Setup optimization objective
107  //
108  // If no optimization objective was specified, then default to
109  // optimizing path length as computed by the distance() function
110  // in the state space.
111  if (pdef_)
112  {
113  if (pdef_->hasOptimizationObjective())
114  {
115  opt_ = pdef_->getOptimizationObjective();
116  if (dynamic_cast<base::PathLengthOptimizationObjective *>(opt_.get()) == nullptr)
117  OMPL_WARN("%s: Asymptotic optimality has only been proven with path length optimizaton; convergence "
118  "for other optimizaton objectives is not guaranteed.",
119  getName().c_str());
120  }
121  else
122  opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
123  }
124  else
125  {
126  OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
127  setup_ = false;
128  }
129 }
130 
132 {
133  Planner::setProblemDefinition(pdef);
134  clearQuery();
135 }
136 
138 {
139  startM_.clear();
140  goalM_.clear();
141  pis_.restart();
142 }
143 
145 {
146  Planner::clear();
147  clearQuery();
148  resetFailures();
149  iterations_ = 0;
150  bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
151  freeMemory();
152  if (nn_)
153  nn_->clear();
154 }
155 
157 {
158  Planner::clear();
159  sampler_.reset();
160 
161  foreach (Vertex v, boost::vertices(g_))
162  {
163  foreach (InterfaceData &d, interfaceDataProperty_[v] | boost::adaptors::map_values)
164  d.clear(si_);
165  if (stateProperty_[v] != nullptr)
166  si_->freeState(stateProperty_[v]);
167  stateProperty_[v] = nullptr;
168  }
169  g_.clear();
170 
171  if (nn_)
172  nn_->clear();
173 }
174 
175 bool ompl::geometric::SPARStwo::haveSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals,
176  base::PathPtr &solution)
177 {
178  base::Goal *g = pdef_->getGoal().get();
179  base::Cost sol_cost(opt_->infiniteCost());
180  foreach (Vertex start, starts)
181  foreach (Vertex goal, goals)
182  {
183  // we lock because the connected components algorithm is incremental and may change disjointSets_
184  graphMutex_.lock();
185  bool same_component = sameComponent(start, goal);
186  graphMutex_.unlock();
187 
188  if (same_component && g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
189  {
190  base::PathPtr p = constructSolution(start, goal);
191  if (p)
192  {
193  base::Cost pathCost = p->cost(opt_);
194  if (opt_->isCostBetterThan(pathCost, bestCost_))
195  bestCost_ = pathCost;
196  // Check if optimization objective is satisfied
197  if (opt_->isSatisfied(pathCost))
198  {
199  solution = p;
200  return true;
201  }
202  if (opt_->isCostBetterThan(pathCost, sol_cost))
203  {
204  solution = p;
205  sol_cost = pathCost;
206  }
207  }
208  }
209  }
210  return false;
211 }
212 
214 {
215  return boost::same_component(m1, m2, disjointSets_);
216 }
217 
219 {
221 }
222 
224 {
226 }
227 
229 {
230  if (stopOnMaxFail)
231  {
232  resetFailures();
233  base::PlannerTerminationCondition ptcOrFail([this, &ptc]
234  {
235  return ptc || reachedFailureLimit();
236  });
237  constructRoadmap(ptcOrFail);
238  }
239  else
240  constructRoadmap(ptc);
241 }
242 
244 {
246 
247  if (!isSetup())
248  setup();
249  if (!sampler_)
250  sampler_ = si_->allocValidStateSampler();
251 
252  base::State *qNew = si_->allocState();
253  base::State *workState = si_->allocState();
254 
255  /* The whole neighborhood set which has been most recently computed */
256  std::vector<Vertex> graphNeighborhood;
257  /* The visible neighborhood set which has been most recently computed */
258  std::vector<Vertex> visibleNeighborhood;
259 
260  bestCost_ = opt_->infiniteCost();
261  while (!ptc)
262  {
263  ++iterations_;
265 
266  // Generate a single sample, and attempt to connect it to nearest neighbors.
267  if (!sampler_->sample(qNew))
268  continue;
269 
270  findGraphNeighbors(qNew, graphNeighborhood, visibleNeighborhood);
271 
272  if (!checkAddCoverage(qNew, visibleNeighborhood))
273  if (!checkAddConnectivity(qNew, visibleNeighborhood))
274  if (!checkAddInterface(qNew, graphNeighborhood, visibleNeighborhood))
275  {
276  if (!visibleNeighborhood.empty())
277  {
278  std::map<Vertex, base::State *> closeRepresentatives;
279  findCloseRepresentatives(workState, qNew, visibleNeighborhood[0], closeRepresentatives, ptc);
280  for (auto &closeRepresentative : closeRepresentatives)
281  {
282  updatePairPoints(visibleNeighborhood[0], qNew, closeRepresentative.first,
283  closeRepresentative.second);
284  updatePairPoints(closeRepresentative.first, closeRepresentative.second,
285  visibleNeighborhood[0], qNew);
286  }
287  checkAddPath(visibleNeighborhood[0]);
288  for (auto &closeRepresentative : closeRepresentatives)
289  {
290  checkAddPath(closeRepresentative.first);
291  si_->freeState(closeRepresentative.second);
292  }
293  }
294  }
295  }
296  si_->freeState(workState);
297  si_->freeState(qNew);
298 }
299 
301 {
302  std::lock_guard<std::mutex> _(graphMutex_);
303  if (boost::num_vertices(g_) < 1)
304  {
305  queryVertex_ = boost::add_vertex(g_);
306  stateProperty_[queryVertex_] = nullptr;
307  }
308 }
309 
311 {
312  checkValidity();
314 
315  auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
316 
317  if (goal == nullptr)
318  {
319  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
321  }
322 
323  // Add the valid start states as milestones
324  while (const base::State *st = pis_.nextStart())
325  startM_.push_back(addGuard(si_->cloneState(st), START));
326  if (startM_.empty())
327  {
328  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
330  }
331 
332  if (!goal->couldSample())
333  {
334  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
336  }
337 
338  // Add the valid goal states as milestones
339  while (const base::State *st = (goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal()))
340  goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
341  if (goalM_.empty())
342  {
343  OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
345  }
346 
347  unsigned int nrStartStates = boost::num_vertices(g_) - 1; // don't count query vertex
348  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nrStartStates);
349 
350  // Reset addedSolution_ member
351  addedSolution_ = false;
352  resetFailures();
353  base::PathPtr sol;
354  base::PlannerTerminationCondition ptcOrFail([this, &ptc]
355  {
356  return ptc || reachedFailureLimit();
357  });
358  std::thread slnThread([this, &ptcOrFail, &sol]
359  {
360  checkForSolution(ptcOrFail, sol);
361  });
362 
363  // Construct planner termination condition which also takes M into account
364  base::PlannerTerminationCondition ptcOrStop([this, &ptc]
365  {
366  return ptc || reachedTerminationCriterion();
367  });
368  constructRoadmap(ptcOrStop);
369 
370  // Ensure slnThread is ceased before exiting solve
371  slnThread.join();
372 
373  OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
374 
375  if (sol)
376  pdef_->addSolutionPath(sol, false, -1.0, getName());
377 
378  // Return true if any solution was found.
380 }
381 
383 {
384  auto *goal = static_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
385  while (!ptc && !addedSolution_)
386  {
387  // Check for any new goal states
388  if (goal->maxSampleCount() > goalM_.size())
389  {
390  const base::State *st = pis_.nextGoal();
391  if (st != nullptr)
392  goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
393  }
394 
395  // Check for a solution
397  // Sleep for 1ms
398  if (!addedSolution_)
399  std::this_thread::sleep_for(std::chrono::milliseconds(1));
400  }
401 }
402 
403 bool ompl::geometric::SPARStwo::checkAddCoverage(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood)
404 {
405  if (!visibleNeighborhood.empty())
406  return false;
407  // No free paths means we add for coverage
408  addGuard(si_->cloneState(qNew), COVERAGE);
409  return true;
410 }
411 
412 bool ompl::geometric::SPARStwo::checkAddConnectivity(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood)
413 {
414  std::vector<Vertex> links;
415  if (visibleNeighborhood.size() > 1)
416  {
417  // For each neighbor
418  for (std::size_t i = 0; i < visibleNeighborhood.size(); ++i)
419  // For each other neighbor
420  for (std::size_t j = i + 1; j < visibleNeighborhood.size(); ++j)
421  // If they are in different components
422  if (!sameComponent(visibleNeighborhood[i], visibleNeighborhood[j]))
423  {
424  links.push_back(visibleNeighborhood[i]);
425  links.push_back(visibleNeighborhood[j]);
426  }
427 
428  if (!links.empty())
429  {
430  // Add the node
431  Vertex g = addGuard(si_->cloneState(qNew), CONNECTIVITY);
432 
433  for (unsigned long link : links)
434  // If there's no edge
435  if (!boost::edge(g, link, g_).second)
436  // And the components haven't been united by previous links
437  if (!sameComponent(link, g))
438  connectGuards(g, link);
439  return true;
440  }
441  }
442  return false;
443 }
444 
445 bool ompl::geometric::SPARStwo::checkAddInterface(const base::State *qNew, std::vector<Vertex> &graphNeighborhood,
446  std::vector<Vertex> &visibleNeighborhood)
447 {
448  // If we have more than 1 or 0 neighbors
449  if (visibleNeighborhood.size() > 1)
450  if (graphNeighborhood[0] == visibleNeighborhood[0] && graphNeighborhood[1] == visibleNeighborhood[1])
451  // If our two closest neighbors don't share an edge
452  if (!boost::edge(visibleNeighborhood[0], visibleNeighborhood[1], g_).second)
453  {
454  // If they can be directly connected
455  if (si_->checkMotion(stateProperty_[visibleNeighborhood[0]], stateProperty_[visibleNeighborhood[1]]))
456  {
457  // Connect them
458  connectGuards(visibleNeighborhood[0], visibleNeighborhood[1]);
459  // And report that we added to the roadmap
460  resetFailures();
461  // Report success
462  return true;
463  }
464 
465  // Add the new node to the graph, to bridge the interface
466  Vertex v = addGuard(si_->cloneState(qNew), INTERFACE);
467  connectGuards(v, visibleNeighborhood[0]);
468  connectGuards(v, visibleNeighborhood[1]);
469  // Report success
470  return true;
471  }
472  return false;
473 }
474 
476 {
477  bool ret = false;
478 
479  std::vector<Vertex> rs;
480  foreach (Vertex r, boost::adjacent_vertices(v, g_))
481  rs.push_back(r);
482 
483  /* Candidate x vertices as described in the method, filled by function computeX(). */
484  std::vector<Vertex> Xs;
485 
486  /* Candidate v" vertices as described in the method, filled by function computeVPP(). */
487  std::vector<Vertex> VPPs;
488 
489  for (std::size_t i = 0; i < rs.size() && !ret; ++i)
490  {
491  Vertex r = rs[i];
492  computeVPP(v, r, VPPs);
493  foreach (Vertex rp, VPPs)
494  {
495  // First, compute the longest path through the graph
496  computeX(v, r, rp, Xs);
497  double rm_dist = 0.0;
498  foreach (Vertex rpp, Xs)
499  {
500  double tmp_dist = (si_->distance(stateProperty_[r], stateProperty_[v]) +
501  si_->distance(stateProperty_[v], stateProperty_[rpp])) /
502  2.0;
503  if (tmp_dist > rm_dist)
504  rm_dist = tmp_dist;
505  }
506 
507  InterfaceData &d = getData(v, r, rp);
508 
509  // Then, if the spanner property is violated
510  if (rm_dist > stretchFactor_ * d.d_)
511  {
512  ret = true; // Report that we added for the path
513  if (si_->checkMotion(stateProperty_[r], stateProperty_[rp]))
514  connectGuards(r, rp);
515  else
516  {
517  auto p(std::make_shared<PathGeometric>(si_));
518  if (r < rp)
519  {
520  p->append(d.sigmaA_);
521  p->append(d.pointA_);
522  p->append(stateProperty_[v]);
523  p->append(d.pointB_);
524  p->append(d.sigmaB_);
525  }
526  else
527  {
528  p->append(d.sigmaB_);
529  p->append(d.pointB_);
530  p->append(stateProperty_[v]);
531  p->append(d.pointA_);
532  p->append(d.sigmaA_);
533  }
534 
535  psimp_->reduceVertices(*p, 10);
536  psimp_->shortcutPath(*p, 50);
537 
538  if (p->checkAndRepair(100).second)
539  {
540  Vertex prior = r;
541  Vertex vnew;
542  std::vector<base::State *> &states = p->getStates();
543 
544  foreach (base::State *st, states)
545  {
546  // no need to clone st, since we will destroy p; we just copy the pointer
547  vnew = addGuard(st, QUALITY);
548 
549  connectGuards(prior, vnew);
550  prior = vnew;
551  }
552  // clear the states, so memory is not freed twice
553  states.clear();
554  connectGuards(prior, rp);
555  }
556  }
557  }
558  }
559  }
560 
561  return ret;
562 }
563 
565 {
567 }
568 
569 void ompl::geometric::SPARStwo::findGraphNeighbors(base::State *st, std::vector<Vertex> &graphNeighborhood,
570  std::vector<Vertex> &visibleNeighborhood)
571 {
572  visibleNeighborhood.clear();
574  nn_->nearestR(queryVertex_, sparseDelta_, graphNeighborhood);
575  stateProperty_[queryVertex_] = nullptr;
576 
577  // Now that we got the neighbors from the NN, we must remove any we can't see
578  for (unsigned long i : graphNeighborhood)
579  if (si_->checkMotion(st, stateProperty_[i]))
580  visibleNeighborhood.push_back(i);
581 }
582 
584 {
585  std::vector<Vertex> hold;
586  nn_->nearestR(v, sparseDelta_, hold);
587 
588  std::vector<Vertex> neigh;
589  for (unsigned long i : hold)
590  if (si_->checkMotion(stateProperty_[v], stateProperty_[i]))
591  neigh.push_back(i);
592 
593  foreach (Vertex vp, neigh)
594  connectGuards(v, vp);
595 }
596 
598 {
599  std::vector<Vertex> nbh;
601  nn_->nearestR(queryVertex_, sparseDelta_, nbh);
602  stateProperty_[queryVertex_] = nullptr;
603 
604  Vertex result = boost::graph_traits<Graph>::null_vertex();
605 
606  for (unsigned long i : nbh)
607  if (si_->checkMotion(st, stateProperty_[i]))
608  {
609  result = i;
610  break;
611  }
612  return result;
613 }
614 
616  const Vertex qRep,
617  std::map<Vertex, base::State *> &closeRepresentatives,
619 {
620  for (auto &closeRepresentative : closeRepresentatives)
621  si_->freeState(closeRepresentative.second);
622  closeRepresentatives.clear();
623 
624  // Then, begin searching the space around him
625  for (unsigned int i = 0; i < nearSamplePoints_; ++i)
626  {
627  do
628  {
629  sampler_->sampleNear(workArea, qNew, denseDelta_);
630  } while ((!si_->isValid(workArea) || si_->distance(qNew, workArea) > denseDelta_ ||
631  !si_->checkMotion(qNew, workArea)) &&
632  !ptc);
633 
634  // if we were not successful at sampling a desirable state, we are out of time
635  if (ptc)
636  break;
637 
638  // Compute who his graph neighbors are
639  Vertex representative = findGraphRepresentative(workArea);
640 
641  // Assuming this sample is actually seen by somebody (which he should be in all likelihood)
642  if (representative != boost::graph_traits<Graph>::null_vertex())
643  {
644  // If his representative is different than qNew
645  if (qRep != representative)
646  // And we haven't already tracked this representative
647  if (closeRepresentatives.find(representative) == closeRepresentatives.end())
648  // Track the representative
649  closeRepresentatives[representative] = si_->cloneState(workArea);
650  }
651  else
652  {
653  // This guy can't be seen by anybody, so we should take this opportunity to add him
654  addGuard(si_->cloneState(workArea), COVERAGE);
655 
656  // We should also stop our efforts to add a dense path
657  for (auto &closeRepresentative : closeRepresentatives)
658  si_->freeState(closeRepresentative.second);
659  closeRepresentatives.clear();
660  break;
661  }
662  }
663 }
664 
666 {
667  // First of all, we need to compute all candidate r'
668  std::vector<Vertex> VPPs;
669  computeVPP(rep, r, VPPs);
670 
671  // Then, for each pair Pv(r,r')
672  foreach (Vertex rp, VPPs)
673  // Try updating the pair info
674  distanceCheck(rep, q, r, s, rp);
675 }
676 
677 void ompl::geometric::SPARStwo::computeVPP(Vertex v, Vertex vp, std::vector<Vertex> &VPPs)
678 {
679  VPPs.clear();
680  foreach (Vertex cvpp, boost::adjacent_vertices(v, g_))
681  if (cvpp != vp)
682  if (!boost::edge(cvpp, vp, g_).second)
683  VPPs.push_back(cvpp);
684 }
685 
686 void ompl::geometric::SPARStwo::computeX(Vertex v, Vertex vp, Vertex vpp, std::vector<Vertex> &Xs)
687 {
688  Xs.clear();
689 
690  foreach (Vertex cx, boost::adjacent_vertices(vpp, g_))
691  if (boost::edge(cx, v, g_).second && !boost::edge(cx, vp, g_).second)
692  {
693  InterfaceData &d = getData(v, vpp, cx);
694  if ((vpp < cx && (d.pointA_ != nullptr)) || (cx < vpp && (d.pointB_ != nullptr)))
695  Xs.push_back(cx);
696  }
697  Xs.push_back(vpp);
698 }
699 
701 {
702  if (vp < vpp)
703  return VertexPair(vp, vpp);
704  if (vpp < vp)
705  return VertexPair(vpp, vp);
706  else
707  throw Exception(name_, "Trying to get an index where the pairs are the same point!");
708 }
709 
711 {
712  return interfaceDataProperty_[v][index(vp, vpp)];
713 }
714 
716  Vertex rp)
717 {
718  // Get the info for the current representative-neighbors pair
719  InterfaceData &d = getData(rep, r, rp);
720 
721  if (r < rp) // FIRST points represent r (the guy discovered through sampling)
722  {
723  if (d.pointA_ == nullptr) // If the point we're considering replacing (P_v(r,.)) isn't there
724  // Then we know we're doing better, so add it
725  d.setFirst(q, s, si_);
726  else // Otherwise, he is there,
727  {
728  if (d.pointB_ == nullptr) // But if the other guy doesn't exist, we can't compare.
729  {
730  // Should probably keep the one that is further away from rep? Not known what to do in this case.
731  // \todo: is this not part of the algorithm?
732  }
733  else // We know both of these points exist, so we can check some distances
734  if (si_->distance(q, d.pointB_) < si_->distance(d.pointA_, d.pointB_))
735  // Distance with the new point is good, so set it.
736  d.setFirst(q, s, si_);
737  }
738  }
739  else // SECOND points represent r (the guy discovered through sampling)
740  {
741  if (d.pointB_ == nullptr) // If the point we're considering replacing (P_V(.,r)) isn't there...
742  // Then we must be doing better, so add it
743  d.setSecond(q, s, si_);
744  else // Otherwise, he is there
745  {
746  if (d.pointA_ == nullptr) // But if the other guy doesn't exist, we can't compare.
747  {
748  // Should we be doing something cool here?
749  }
750  else if (si_->distance(q, d.pointA_) < si_->distance(d.pointB_, d.pointA_))
751  // Distance with the new point is good, so set it
752  d.setSecond(q, s, si_);
753  }
754  }
755 
756  // Lastly, save what we have discovered
757  interfaceDataProperty_[rep][index(r, rp)] = d;
758 }
759 
761 {
763 
764  std::vector<Vertex> hold;
765  nn_->nearestR(queryVertex_, sparseDelta_, hold);
766 
767  stateProperty_[queryVertex_] = nullptr;
768 
769  // For each of the vertices
770  foreach (Vertex v, hold)
771  {
772  foreach (VertexPair r, interfaceDataProperty_[v] | boost::adaptors::map_keys)
773  interfaceDataProperty_[v][r].clear(si_);
774  }
775 }
776 
778 {
779  std::lock_guard<std::mutex> _(graphMutex_);
780 
781  Vertex m = boost::add_vertex(g_);
782  stateProperty_[m] = state;
783  colorProperty_[m] = type;
784 
785  assert(si_->isValid(state));
786  abandonLists(state);
787 
788  disjointSets_.make_set(m);
789  nn_->add(m);
790  resetFailures();
791 
792  return m;
793 }
794 
796 {
797  assert(v <= milestoneCount());
798  assert(vp <= milestoneCount());
799 
800  const base::Cost weight(costHeuristic(v, vp));
801  const Graph::edge_property_type properties(weight);
802  std::lock_guard<std::mutex> _(graphMutex_);
803  boost::add_edge(v, vp, properties, g_);
804  disjointSets_.union_set(v, vp);
805 }
806 
808 {
809  std::lock_guard<std::mutex> _(graphMutex_);
810 
811  boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
812 
813  try
814  {
815  boost::astar_search(g_, start,
816  [this, goal](Vertex v)
817  {
818  return costHeuristic(v, goal);
819  },
820  boost::predecessor_map(prev)
821  .distance_compare([this](base::Cost c1, base::Cost c2)
822  {
823  return opt_->isCostBetterThan(c1, c2);
824  })
825  .distance_combine([this](base::Cost c1, base::Cost c2)
826  {
827  return opt_->combineCosts(c1, c2);
828  })
829  .distance_inf(opt_->infiniteCost())
830  .distance_zero(opt_->identityCost())
831  .visitor(AStarGoalVisitor<Vertex>(goal)));
832  }
833  catch (AStarFoundGoal &)
834  {
835  }
836 
837  if (prev[goal] == goal)
838  throw Exception(name_, "Could not find solution path");
839  else
840  {
841  auto p(std::make_shared<PathGeometric>(si_));
842  for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
843  p->append(stateProperty_[pos]);
844  p->append(stateProperty_[start]);
845  p->reverse();
846 
847  return p;
848  }
849 }
850 
851 void ompl::geometric::SPARStwo::printDebug(std::ostream &out) const
852 {
853  out << "SPARStwo Debug Output: " << std::endl;
854  out << " Settings: " << std::endl;
855  out << " Max Failures: " << getMaxFailures() << std::endl;
856  out << " Dense Delta Fraction: " << getDenseDeltaFraction() << std::endl;
857  out << " Sparse Delta Fraction: " << getSparseDeltaFraction() << std::endl;
858  out << " Stretch Factor: " << getStretchFactor() << std::endl;
859  out << " Status: " << std::endl;
860  out << " Milestone Count: " << milestoneCount() << std::endl;
861  out << " Iterations: " << getIterationCount() << std::endl;
862  out << " Consecutive Failures: " << consecutiveFailures_ << std::endl;
863 }
864 
866 {
867  Planner::getPlannerData(data);
868 
869  // Explicitly add start and goal states:
870  for (unsigned long i : startM_)
872 
873  for (unsigned long i : goalM_)
875 
876  // If there are even edges here
877  if (boost::num_edges(g_) > 0)
878  {
879  // Adding edges and all other vertices simultaneously
880  foreach (const Edge e, boost::edges(g_))
881  {
882  const Vertex v1 = boost::source(e, g_);
883  const Vertex v2 = boost::target(e, g_);
886 
887  // Add the reverse edge, since we're constructing an undirected roadmap
890  }
891  }
892  else
893  OMPL_INFORM("%s: There are no edges in the graph!", getName().c_str());
894 
895  // Make sure to add edge-less nodes as well
896  foreach (const Vertex n, boost::vertices(g_))
897  if (boost::out_degree(n, g_) == 0)
899 }
900 
902 {
903  return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
904 }
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:203
void printDebug(std::ostream &out=std::cout) const
Print debug information about planner.
Definition: SPARStwo.cpp:851
bool sameComponent(Vertex m1, Vertex m2)
Check if two milestones (m1 and m2) are part of the same connected component. This is not a const fun...
Definition: SPARStwo.cpp:213
void addPlannerProgressProperty(const std::string &progressPropertyName, const PlannerProgressProperty &prop)
Add a planner progress property called progressPropertyName with a property querying function prop to...
Definition: Planner.h:399
double sparseDeltaFraction_
Maximum visibility range for nodes in the graph as a fraction of maximum extent.
Definition: SPARStwo.h:475
Graph g_
Connectivity graph.
Definition: SPARStwo.h:460
base::State * pointA_
States which lie inside the visibility region of a vertex and support an interface.
Definition: SPARStwo.h:101
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
void findCloseRepresentatives(base::State *workArea, const base::State *qNew, Vertex qRep, std::map< Vertex, base::State *> &closeRepresentatives, const base::PlannerTerminationCondition &ptc)
Finds representatives of samples near qNew_ which are not his representative.
Definition: SPARStwo.cpp:615
base::ValidStateSamplerPtr sampler_
Sampler user for generating valid samples in the state space.
Definition: SPARStwo.h:454
void setProblemDefinition(const base::ProblemDefinitionPtr &pdef) override
Set the problem definition for the planner. The problem needs to be set before calling solve()...
Definition: SPARStwo.cpp:131
A shared pointer wrapper for ompl::base::ProblemDefinition.
void setDenseDeltaFraction(double d)
Sets interface support tolerance as a fraction of max. extent.
Definition: SPARStwo.h:245
The planner failed to find a solution.
Definition: PlannerStatus.h:62
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:197
void findGraphNeighbors(base::State *st, std::vector< Vertex > &graphNeighborhood, std::vector< Vertex > &visibleNeighborhood)
Finds visible nodes in the graph near st.
Definition: SPARStwo.cpp:569
double denseDeltaFraction_
Maximum range for allowing two samples to support an interface as a fraction of maximum extent...
Definition: SPARStwo.h:479
long unsigned int iterations_
A counter for the number of iterations of the algorithm.
Definition: SPARStwo.h:533
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: SPARStwo.cpp:144
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available. Because sampling of goal states may also produce invalid goals, this function takes an argument that specifies whether a termination condition has been reached. If the termination condition evaluates to true the function terminates even if no valid goal has been found.
Definition: Planner.cpp:264
void setStretchFactor(double t)
Sets the stretch factor.
Definition: SPARStwo.h:231
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
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...
double getDenseDeltaFraction() const
Retrieve the dense graph interface support delta.
Definition: SPARStwo.h:265
void resetFailures()
A reset function for resetting the failures count.
Definition: SPARStwo.cpp:564
void distanceCheck(Vertex rep, const base::State *q, Vertex r, const base::State *s, Vertex rp)
Performs distance checking for the candidate new state, q against the current information.
Definition: SPARStwo.cpp:715
Abstract definition of goals.
Definition: Goal.h:62
SPARStwo(const base::SpaceInformationPtr &si)
Constructor.
Definition: SPARStwo.cpp:53
base::Cost costHeuristic(Vertex u, Vertex v) const
Given two vertices, returns a heuristic on the cost of the path connecting them. This method wraps Op...
Definition: SPARStwo.cpp:901
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Vertex findGraphRepresentative(base::State *st)
Finds the representative of the input state, st.
Definition: SPARStwo.cpp:597
base::Cost bestCost_
Best cost found so far by algorithm.
Definition: SPARStwo.h:535
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added...
std::vector< Vertex > startM_
Array of start milestones.
Definition: SPARStwo.h:463
double getStretchFactor() const
Retrieve the spanner&#39;s set stretch factor.
Definition: SPARStwo.h:277
void clearQuery()
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition: SPARStwo.cpp:137
std::vector< Vertex > goalM_
Array of goal milestones.
Definition: SPARStwo.h:466
void setSparseDeltaFraction(double D)
Sets vertex visibility range as a fraction of max. extent.
Definition: SPARStwo.h:237
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:409
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:200
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: SPARStwo.cpp:93
base::OptimizationObjectivePtr opt_
Objective cost function for PRM graph edges.
Definition: SPARStwo.h:524
base::State * sigmaA_
States which lie just outside the visibility region of a vertex and support an interface.
Definition: SPARStwo.h:105
bool checkAddCoverage(const base::State *qNew, std::vector< Vertex > &visibleNeighborhood)
Checks to see if the sample needs to be added to ensure coverage of the space.
Definition: SPARStwo.cpp:403
unsigned int consecutiveFailures_
A counter for the number of consecutive failed iterations of the algorithm.
Definition: SPARStwo.h:512
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:429
unsigned int milestoneCount() const
Get the number of vertices in the sparse roadmap.
Definition: SPARStwo.h:333
double denseDelta_
Maximum range for allowing two samples to support an interface.
Definition: SPARStwo.h:518
Abstract definition of a goal region that can be sampled.
bool checkAddConnectivity(const base::State *qNew, std::vector< Vertex > &visibleNeighborhood)
Checks to see if the sample needs to be added to ensure connectivity.
Definition: SPARStwo.cpp:412
void computeVPP(Vertex v, Vertex vp, std::vector< Vertex > &VPPs)
Computes all nodes which qualify as a candidate v" for v and vp.
Definition: SPARStwo.cpp:677
bool addedSolution_
A flag indicating that a solution has been added during solve()
Definition: SPARStwo.h:509
void setSecond(const base::State *p, const base::State *s, const base::SpaceInformationPtr &si)
Sets information for the second interface (i.e. interface with larger index vertex).
Definition: SPARStwo.h:156
Vertex addGuard(base::State *state, GuardType type)
Construct a guard for a given state (state) and store it in the nearest neighbors data structure...
Definition: SPARStwo.cpp:777
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition permits, construct the spanner graph.
Definition: SPARStwo.cpp:243
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
The planner found an exact solution.
Definition: PlannerStatus.h:66
Interface information storage class, which does bookkeeping for criterion four.
Definition: SPARStwo.h:98
double sparseDelta_
Maximum visibility range for nodes in the graph.
Definition: SPARStwo.h:515
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: SPARStwo.cpp:310
bool checkAddInterface(const base::State *qNew, std::vector< Vertex > &graphNeighborhood, std::vector< Vertex > &visibleNeighborhood)
Checks to see if the current sample reveals the existence of an interface, and if so...
Definition: SPARStwo.cpp:445
base::PathPtr constructSolution(Vertex start, Vertex goal) const
Given two milestones from the same connected component, construct a path connecting them and set it a...
Definition: SPARStwo.cpp:807
void approachGraph(Vertex v)
Approaches the graph from a given vertex.
Definition: SPARStwo.cpp:583
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
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...
boost::disjoint_sets< boost::property_map< Graph, boost::vertex_rank_t >::type, boost::property_map< Graph, boost::vertex_predecessor_t >::type > disjointSets_
Data structure that maintains the connected components.
Definition: SPARStwo.h:504
A shared pointer wrapper for ompl::base::SpaceInformation.
bool reachedTerminationCriterion() const
Returns true if we have reached the iteration failures limit, maxFailures_ or if a solution was added...
Definition: SPARStwo.cpp:223
bool haveSolution(const std::vector< Vertex > &starts, const std::vector< Vertex > &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: SPARStwo.cpp:175
void clear(const base::SpaceInformationPtr &si)
Clears the given interface data.
Definition: SPARStwo.h:115
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...
Definition of an abstract state.
Definition: State.h:49
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set...
Definition: Planner.cpp:101
PathSimplifierPtr psimp_
A path simplifier used to simplify dense paths added to the graph.
Definition: SPARStwo.h:491
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:412
std::mutex graphMutex_
Mutex to guard access to the Graph member (g_)
Definition: SPARStwo.h:521
void setMaxFailures(unsigned int m)
Sets the maximum failures until termination.
Definition: SPARStwo.h:253
unsigned int maxFailures_
The number of consecutive failures to add to the graph before termination.
Definition: SPARStwo.h:482
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: SPARStwo.cpp:865
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:418
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:227
The exception type for ompl.
Definition: Exception.h:46
boost::graph_traits< Graph >::edge_descriptor Edge
Edge in Graph.
Definition: SPARStwo.h:220
double distanceFunction(const Vertex a, const Vertex b) const
Compute distance between two milestones (this is simply distance between the states of the milestones...
Definition: SPARStwo.h:448
InterfaceData & getData(Vertex v, Vertex vp, Vertex vpp)
Retrieves the Vertex data associated with v,vp,vpp.
Definition: SPARStwo.cpp:710
GuardType
Enumeration which specifies the reason a guard is added to the spanner.
Definition: SPARStwo.h:81
void connectGuards(Vertex v, Vertex vp)
Connect two guards in the roadmap.
Definition: SPARStwo.cpp:795
boost::property_map< Graph, vertex_color_t >::type colorProperty_
Access to the colors for the vertices.
Definition: SPARStwo.h:497
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
Definition: SPARStwo.cpp:382
bool reachedFailureLimit() const
Returns whether we have reached the iteration failures limit, maxFailures_.
Definition: SPARStwo.cpp:218
boost::graph_traits< Graph >::vertex_descriptor Vertex
Vertex in Graph.
Definition: SPARStwo.h:217
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
bool isSetup() const
Check if setup() was called for this planner.
Definition: Planner.cpp:108
std::string name_
The name of this planner.
Definition: Planner.h:415
~SPARStwo() override
Destructor.
Definition: SPARStwo.cpp:88
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:207
std::pair< VertexIndexType, VertexIndexType > VertexPair
Pair of vertices which support an interface.
Definition: SPARStwo.h:95
void updatePairPoints(Vertex rep, const base::State *q, Vertex r, const base::State *s)
High-level method which updates pair point information for repV_ with neighbor r. ...
Definition: SPARStwo.cpp:665
void restart()
Forget how many states were returned by nextStart() and nextGoal() and return all states again...
Definition: Planner.cpp:170
void checkQueryStateInitialization()
Check that the query vertex is initialized (used for internal nearest neighbor searches) ...
Definition: SPARStwo.cpp:300
double d_
Last known distance between the two interfaces supported by points_ and sigmas.
Definition: SPARStwo.h:109
double stretchFactor_
Stretch Factor as per graph spanner literature (multiplicative bound on path quality) ...
Definition: SPARStwo.h:472
bool checkAddPath(Vertex v)
Checks vertex v for short paths through its region and adds when appropriate.
Definition: SPARStwo.cpp:475
void freeMemory()
Free all the memory allocated by the planner.
Definition: SPARStwo.cpp:156
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:406
void abandonLists(base::State *st)
When a new guard is added at state st, finds all guards who must abandon their interface information ...
Definition: SPARStwo.cpp:760
double getSparseDeltaFraction() const
Retrieve the sparse graph visibility range delta.
Definition: SPARStwo.h:271
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition: SPARStwo.h:488
VertexPair index(Vertex vp, Vertex vpp)
Rectifies indexing order for accessing the vertex data.
Definition: SPARStwo.cpp:700
unsigned int getMaxFailures() const
Retrieve the maximum consecutive failure limit.
Definition: SPARStwo.h:259
Vertex queryVertex_
Vertex for performing nearest neighbor queries.
Definition: SPARStwo.h:469
boost::property_map< Graph, vertex_interface_data_t >::type interfaceDataProperty_
Access to the interface pair information for the vertices.
Definition: SPARStwo.h:500
void computeX(Vertex v, Vertex vp, Vertex vpp, std::vector< Vertex > &Xs)
Computes all nodes which qualify as a candidate x for v, v&#39;, and v".
Definition: SPARStwo.cpp:686
void setFirst(const base::State *p, const base::State *s, const base::SpaceInformationPtr &si)
Sets information for the first interface (i.e. interface with smaller index vertex).
Definition: SPARStwo.h:141
std::shared_ptr< NearestNeighbors< Vertex > > nn_
Nearest neighbors data structure.
Definition: SPARStwo.h:457
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
unsigned int nearSamplePoints_
Number of sample points to use when trying to detect interfaces.
Definition: SPARStwo.h:485
A shared pointer wrapper for ompl::base::Path.
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
Definition: GoalTypes.h:56
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68