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