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();
103  sparseDelta_ = sparseDeltaFraction_ * maxExt;
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 {
220  return consecutiveFailures_ >= maxFailures_;
221 }
222 
224 {
225  return consecutiveFailures_ >= maxFailures_ || addedSolution_;
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 {
245  checkQueryStateInitialization();
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_;
264  ++consecutiveFailures_;
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();
313  checkQueryStateInitialization();
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
396  addedSolution_ = haveSolution(startM_, goalM_, 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 {
566  consecutiveFailures_ = 0;
567 }
568 
569 void ompl::geometric::SPARStwo::findGraphNeighbors(base::State *st, std::vector<Vertex> &graphNeighborhood,
570  std::vector<Vertex> &visibleNeighborhood)
571 {
572  visibleNeighborhood.clear();
573  stateProperty_[queryVertex_] = st;
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;
600  stateProperty_[queryVertex_] = st;
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 {
762  stateProperty_[queryVertex_] = st;
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_)
871  data.addStartVertex(base::PlannerDataVertex(stateProperty_[i], (int)START));
872 
873  for (unsigned long i : goalM_)
874  data.addGoalVertex(base::PlannerDataVertex(stateProperty_[i], (int)GOAL));
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_);
884  data.addEdge(base::PlannerDataVertex(stateProperty_[v1], (int)colorProperty_[v1]),
885  base::PlannerDataVertex(stateProperty_[v2], (int)colorProperty_[v2]));
886 
887  // Add the reverse edge, since we're constructing an undirected roadmap
888  data.addEdge(base::PlannerDataVertex(stateProperty_[v2], (int)colorProperty_[v2]),
889  base::PlannerDataVertex(stateProperty_[v1], (int)colorProperty_[v1]));
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)
898  data.addVertex(base::PlannerDataVertex(stateProperty_[n], (int)colorProperty_[n]));
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
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
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
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: SPARStwo.cpp:144
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
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added...
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
void setSparseDeltaFraction(double D)
Sets vertex visibility range as a fraction of max. extent.
Definition: SPARStwo.h:237
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::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
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
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
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
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...
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
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
void setMaxFailures(unsigned int m)
Sets the maximum failures until termination.
Definition: SPARStwo.h:253
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
The exception type for ompl.
Definition: Exception.h:46
boost::graph_traits< Graph >::edge_descriptor Edge
Edge in Graph.
Definition: SPARStwo.h:220
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
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
~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 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
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
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
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
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
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