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