SPARSdb.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Rutgers the State University of New Jersey, New Brunswick
5  * Copyright (c) 2014, University of Colorado, Boulder
6  * All Rights Reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Rutgers University nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Andrew Dobson, Dave Coleman */
37 
38 #include <ompl/tools/thunder/SPARSdb.h>
39 #include <ompl/geometric/planners/prm/ConnectionStrategy.h>
40 #include <ompl/base/goals/GoalSampleableRegion.h>
41 #include <ompl/tools/config/SelfConfig.h>
42 #include <ompl/util/Console.h>
43 #include <boost/graph/astar_search.hpp>
44 #include <boost/graph/incremental_components.hpp>
45 #include <boost/property_map/vector_property_map.hpp>
46 #include <boost/foreach.hpp>
47 
48 // Allow hooks for visualizing planner
49 //#define OMPL_THUNDER_DEBUG
50 
51 #define foreach BOOST_FOREACH
52 #define foreach_reverse BOOST_REVERSE_FOREACH
53 
54 // edgeWeightMap methods ////////////////////////////////////////////////////////////////////////////
55 
56 BOOST_CONCEPT_ASSERT(
57  (boost::ReadablePropertyMapConcept<ompl::geometric::SPARSdb::edgeWeightMap, ompl::geometric::SPARSdb::Edge>));
58 
60  : g_(graph), collisionStates_(collisionStates)
61 {
62 }
63 
65 {
66  // Get the status of collision checking for this edge
67  if (collisionStates_[e] == IN_COLLISION)
68  return std::numeric_limits<double>::infinity();
69 
70  return boost::get(boost::edge_weight, g_, e);
71 }
72 
73 namespace boost
74 {
76  {
77  return m.get(e);
78  }
79 }
80 
81 // CustomVisitor methods ////////////////////////////////////////////////////////////////////////////
82 
83 BOOST_CONCEPT_ASSERT(
84  (boost::AStarVisitorConcept<ompl::geometric::SPARSdb::CustomVisitor, ompl::geometric::SPARSdb::Graph>));
85 
87 {
88 }
89 
91 {
92  if (u == goal)
93  throw foundGoalException();
94 }
95 
96 // SPARSdb methods ////////////////////////////////////////////////////////////////////////////////////////
97 
99  : base::Planner(si, "SPARSdb")
100  ,
101  // Numeric variables
102  stretchFactor_(3.)
103  , sparseDeltaFraction_(.25)
104  , denseDeltaFraction_(.001)
105  , maxFailures_(5000)
107  , nearSamplePoints_((2 * si_->getStateDimension()))
108  ,
109  // Property accessors of edges
110  edgeWeightProperty_(boost::get(boost::edge_weight, g_))
112  ,
113  // Property accessors of vertices
117  ,
118  // Disjoint set accessors
119  disjointSets_(boost::get(boost::vertex_rank, g_), boost::get(boost::vertex_predecessor, g_))
120  , addedSolution_(false)
122  , iterations_(0)
123  , sparseDelta_(0.)
124  , denseDelta_(0.)
125  , verbose_(false)
126 {
129  specs_.optimizingPaths = true;
130 
131  psimp_ = std::make_shared<PathSimplifier>(si_);
132 
133  Planner::declareParam<double>("stretch_factor", this, &SPARSdb::setStretchFactor, &SPARSdb::getStretchFactor,
134  "1.1:0.1:3.0");
135  Planner::declareParam<double>("sparse_delta_fraction", this, &SPARSdb::setSparseDeltaFraction,
136  &SPARSdb::getSparseDeltaFraction, "0.0:0.01:1.0");
137  Planner::declareParam<double>("dense_delta_fraction", this, &SPARSdb::setDenseDeltaFraction,
138  &SPARSdb::getDenseDeltaFraction, "0.0:0.0001:0.1");
139  Planner::declareParam<unsigned int>("max_failures", this, &SPARSdb::setMaxFailures, &SPARSdb::getMaxFailures,
140  "100:10:3000");
141 }
142 
144 {
145  freeMemory();
146 }
147 
149 {
150  Planner::setup();
151  if (!nn_)
152  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
153  nn_->setDistanceFunction([this](const Vertex a, const Vertex b)
154  {
155  return distanceFunction(a, b);
156  });
157  double maxExt = si_->getMaximumExtent();
159  denseDelta_ = denseDeltaFraction_ * maxExt;
160 
161  if (!sampler_)
162  sampler_ = si_->allocValidStateSampler();
163 }
164 
166 {
167  Planner::setProblemDefinition(pdef);
168  clearQuery();
169 }
170 
172 {
173  startM_.clear();
174  goalM_.clear();
175  pis_.restart();
176 }
177 
179 {
180  Planner::clear();
181  clearQuery();
182  resetFailures();
183  iterations_ = 0;
184  freeMemory();
185  if (nn_)
186  nn_->clear();
187 }
188 
190 {
191  Planner::clear();
192  sampler_.reset();
193 
194  foreach (Vertex v, boost::vertices(g_))
195  {
196  foreach (InterfaceData &d, interfaceDataProperty_[v].interfaceHash | boost::adaptors::map_values)
197  d.clear(si_);
198  if (stateProperty_[v] != nullptr)
199  si_->freeState(stateProperty_[v]);
200  stateProperty_[v] = nullptr;
201  }
202  g_.clear();
203 
204  if (nn_)
205  nn_->clear();
206 }
207 
208 bool ompl::geometric::SPARSdb::getSimilarPaths(int nearestK, const base::State *start, const base::State *goal,
209  CandidateSolution &candidateSolution,
211 {
212  // TODO: nearestK unused
213 
214  // Get neighbors near start and goal. Note: potentially they are not *visible* - will test for this later
215 
216  // Start
217  OMPL_INFORM("Looking for a node near the problem start");
219  {
220  OMPL_INFORM("No graph neighbors found for start within radius %f", sparseDelta_);
221  return false;
222  }
223  if (verbose_)
224  OMPL_INFORM("Found %d nodes near start", startVertexCandidateNeighbors_.size());
225 
226  // Goal
227  OMPL_INFORM("Looking for a node near the problem goal");
228  if (!findGraphNeighbors(goal, goalVertexCandidateNeighbors_))
229  {
230  OMPL_INFORM("No graph neighbors found for goal within radius %f", sparseDelta_);
231  return false;
232  }
233  if (verbose_)
234  OMPL_INFORM("Found %d nodes near goal", goalVertexCandidateNeighbors_.size());
235 
236  // Get paths between start and goal
237  bool result =
238  getPaths(startVertexCandidateNeighbors_, goalVertexCandidateNeighbors_, start, goal, candidateSolution, ptc);
239 
240  // Error check
241  if (!result)
242  {
243  OMPL_INFORM("getSimilarPaths(): SPARSdb returned FALSE for getPaths");
244  return false;
245  }
246  if (!candidateSolution.path_)
247  {
248  OMPL_ERROR("getSimilarPaths(): SPARSdb returned solution is nullptr");
249  return false;
250  }
251 
252  // Debug output
253  if (false)
254  {
255  ompl::geometric::PathGeometric geometricSolution =
256  static_cast<ompl::geometric::PathGeometric &>(*candidateSolution.path_);
257 
258  for (std::size_t i = 0; i < geometricSolution.getStateCount(); ++i)
259  {
260  OMPL_INFORM(" getSimilarPaths(): Adding state %f to plannerData", i);
261  si_->printState(geometricSolution.getState(i), std::cout);
262  }
263  }
264 
265  return result;
266 }
267 
268 bool ompl::geometric::SPARSdb::getPaths(const std::vector<Vertex> &candidateStarts,
269  const std::vector<Vertex> &candidateGoals, const base::State *actualStart,
270  const base::State *actualGoal, CandidateSolution &candidateSolution,
272 {
273  // Try every combination of nearby start and goal pairs
274  foreach (Vertex start, candidateStarts)
275  {
276  // Check if this start is visible from the actual start
277  if (!si_->checkMotion(actualStart, stateProperty_[start]))
278  {
279  if (verbose_)
280  OMPL_WARN("FOUND CANDIDATE START THAT IS NOT VISIBLE ");
281  continue; // this is actually not visible
282  }
283 
284  foreach (Vertex goal, candidateGoals)
285  {
286  if (verbose_)
287  OMPL_INFORM(" foreach_goal: Checking motion from %d to %d", actualGoal, stateProperty_[goal]);
288 
289  // Check if our planner is out of time
290  if (ptc == true)
291  {
292  OMPL_DEBUG("getPaths function interrupted because termination condition is true.");
293  return false;
294  }
295 
296  // Check if this goal is visible from the actual goal
297  if (!si_->checkMotion(actualGoal, stateProperty_[goal]))
298  {
299  if (verbose_)
300  OMPL_INFORM("FOUND CANDIDATE GOAL THAT IS NOT VISIBLE! ");
301  continue; // this is actually not visible
302  }
303 
304  // Repeatidly search through graph for connection then check for collisions then repeat
305  if (lazyCollisionSearch(start, goal, actualStart, actualGoal, candidateSolution, ptc))
306  {
307  // Found a path
308  return true;
309  }
310  else
311  {
312  // Did not find a path
313  OMPL_INFORM("Did not find a path, looking for other start/goal combinations ");
314  }
315 
316  } // foreach
317  } // foreach
318 
319  return false;
320 }
321 
323  const base::State *actualStart, const base::State *actualGoal,
324  CandidateSolution &candidateSolution,
326 {
327  base::Goal *g = pdef_->getGoal().get(); // for checking isStartGoalPairValid
328 
329  // Vector to store candidate paths in before they are converted to PathPtrs
330  std::vector<Vertex> vertexPath;
331 
332  // decide if start and goal are connected
333  // TODO this does not compute dynamic graphcs
334  // i.e. it will say its the same components even when an edge has been disabled
335  bool same_component =
336  true; // sameComponent(start, goal); // TODO is this important? I disabled it during dev and never used it
337 
338  // Check if the chosen start and goal can be used together to satisfy problem
339  if (!same_component)
340  {
341  if (verbose_)
342  OMPL_INFORM(" Goal and start are not part of same component, skipping ");
343  return false;
344  }
345 
346  // TODO: remove this because start and goal are not either start nor goals
347  if (!g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
348  {
349  if (verbose_)
350  OMPL_INFORM(" Start and goal pair are not valid combinations, skipping ");
351  return false;
352  }
353 
354  // Make sure that the start and goal aren't so close together that they find the same vertex
355  if (start == goal)
356  {
357  if (verbose_)
358  OMPL_INFORM(" Start equals goal, skipping ");
359  return false;
360  }
361 
362  // Keep looking for paths between chosen start and goal until one is found that is valid,
363  // or no further paths can be found between them because of disabled edges
364  // this is necessary for lazy collision checking i.e. rerun after marking invalid edges we found
365  bool havePartialSolution = false;
366  while (true)
367  {
368  if (verbose_)
369  OMPL_INFORM(" while true: look for valid paths between start and goal");
370 
371  // Check if our planner is out of time
372  if (ptc == true)
373  {
374  OMPL_DEBUG("lazyCollisionSearch: function interrupted because termination condition is true.");
375  return false;
376  }
377 
378  // Attempt to find a solution from start to goal
379  if (!constructSolution(start, goal, vertexPath))
380  {
381  // We will stop looking through this start-goal combination, but perhaps this partial solution is good
382  if (verbose_)
383  OMPL_INFORM(" unable to construct solution between start and goal using astar");
384 
385  // no solution path found. check if a previous partially correct solution was found
386  if (havePartialSolution && false) // TODO: re-implement partial solution logic
387  {
388  if (verbose_)
389  OMPL_INFORM("has partial solution ");
390  // Save this candidateSolution for later
391  convertVertexPathToStatePath(vertexPath, actualStart, actualGoal, candidateSolution);
392  return false;
393  }
394 
395  if (verbose_)
396  OMPL_INFORM(" no partial solution found on this astar search, keep looking through start-goal "
397  "combos");
398 
399  // no path found what so ever
400  // return false;
401  return false;
402  }
403  havePartialSolution = true; // we have found at least one path at this point. may be invalid
404 
405  if (verbose_)
406  {
407  OMPL_INFORM(" has at least a partial solution, maybe exact solution");
408  OMPL_INFORM(" Solution has %d vertices", vertexPath.size());
409  }
410 
411  // Check if all the points in the potential solution are valid
412  if (lazyCollisionCheck(vertexPath, ptc))
413  {
414  if (verbose_)
415  {
416  OMPL_INFORM("---------- lazy collision check returned valid ");
417  }
418 
419  // the path is valid, we are done!
420  convertVertexPathToStatePath(vertexPath, actualStart, actualGoal, candidateSolution);
421  return true;
422  }
423  // else, loop with updated graph that has the invalid edges/states disabled
424  } // while
425 
426  // we never found a valid path
427  return false;
428 }
429 
431  std::vector<Vertex> &vertexPath) const
432 {
433  auto *vertexPredecessors = new Vertex[boost::num_vertices(g_)];
434  bool foundGoal = false;
435 
436  auto *vertexDistances = new double[boost::num_vertices(g_)];
437 
438  try
439  {
440  boost::astar_search(g_, // graph
441  start, // start state
442  [this, goal](const Vertex v)
443  {
444  return distanceFunction(v, goal);
445  }, // the heuristic
446  // ability to disable edges (set cost to inifinity):
447  boost::weight_map(edgeWeightMap(g_, edgeCollisionStateProperty_))
448  .predecessor_map(vertexPredecessors)
449  .distance_map(&vertexDistances[0])
450  .visitor(CustomVisitor(goal)));
451  }
453  {
454  // the custom exception from CustomVisitor
455  if (verbose_ && false)
456  {
457  OMPL_INFORM("constructSolution: Astar found goal vertex ------------------------");
458  OMPL_INFORM("distance to goal: %f", vertexDistances[goal]);
459  }
460 
461  if (vertexDistances[goal] > 1.7e+308) // terrible hack for detecting infinity
462  // double diff = d[goal] - std::numeric_limits<double>::infinity();
463  // if ((diff < std::numeric_limits<double>::epsilon()) && (-diff < std::numeric_limits<double>::epsilon()))
464  // check if the distance to goal is inifinity. if so, it is unreachable
465  // if (d[goal] >= std::numeric_limits<double>::infinity())
466  {
467  if (verbose_)
468  OMPL_INFORM("Distance to goal is infinity");
469  foundGoal = false;
470  }
471  else
472  {
473  // Only clear the vertexPath after we know we have a new solution, otherwise it might have a good
474  // previous one
475  vertexPath.clear(); // remove any old solutions
476 
477  // Trace back the shortest path in reverse and only save the states
478  Vertex v;
479  for (v = goal; v != vertexPredecessors[v]; v = vertexPredecessors[v])
480  {
481  vertexPath.push_back(v);
482  }
483  if (v != goal) // TODO explain this because i don't understand
484  {
485  vertexPath.push_back(v);
486  }
487 
488  foundGoal = true;
489  }
490  }
491 
492  delete[] vertexPredecessors;
493  delete[] vertexDistances;
494 
495  // No solution found from start to goal
496  return foundGoal;
497 }
498 
499 bool ompl::geometric::SPARSdb::lazyCollisionCheck(std::vector<Vertex> &vertexPath,
501 {
502  OMPL_DEBUG("Starting lazy collision checking");
503 
504  bool hasInvalidEdges = false;
505 
506  // Initialize
507  Vertex fromVertex = vertexPath[0];
508  Vertex toVertex;
509 
510  // Loop through every pair of states and make sure path is valid.
511  for (std::size_t toID = 1; toID < vertexPath.size(); ++toID)
512  {
513  // Increment location on path
514  toVertex = vertexPath[toID];
515 
516  // Check if our planner is out of time
517  if (ptc == true)
518  {
519  OMPL_DEBUG("Lazy collision check function interrupted because termination condition is true.");
520  return false;
521  }
522 
523  Edge thisEdge = boost::edge(fromVertex, toVertex, g_).first;
524 
525  // Has this edge already been checked before?
526  if (edgeCollisionStateProperty_[thisEdge] == NOT_CHECKED)
527  {
528  // Check path between states
529  if (!si_->checkMotion(stateProperty_[fromVertex], stateProperty_[toVertex]))
530  {
531  // Path between (from, to) states not valid, disable the edge
532  OMPL_INFORM(" DISABLING EDGE from vertex %f to vertex %f", fromVertex, toVertex);
533 
534  // Disable edge
535  edgeCollisionStateProperty_[thisEdge] = IN_COLLISION;
536  }
537  else
538  {
539  // Mark edge as free so we no longer need to check for collision
540  edgeCollisionStateProperty_[thisEdge] = FREE;
541  }
542  }
543 
544  // Check final result
545  if (edgeCollisionStateProperty_[thisEdge] == IN_COLLISION)
546  {
547  // Remember that this path is no longer valid, but keep checking remainder of path edges
548  hasInvalidEdges = true;
549  }
550 
551  // switch vertex focus
552  fromVertex = toVertex;
553  }
554 
555  OMPL_INFORM("Done lazy collision checking");
556 
557  // TODO: somewhere in the code we need to reset all edges collision status back to NOT_CHECKED for future queries
558 
559  // Only return true if nothing was found invalid
560  return !hasInvalidEdges;
561 }
562 
564 {
565  return boost::same_component(m1, m2, disjointSets_);
566 }
567 
569 {
571 }
572 
573 void ompl::geometric::SPARSdb::printDebug(std::ostream &out) const
574 {
575  out << "SPARSdb Debug Output: " << std::endl;
576  out << " Settings: " << std::endl;
577  out << " Max Failures: " << getMaxFailures() << std::endl;
578  out << " Dense Delta Fraction: " << getDenseDeltaFraction() << std::endl;
579  out << " Sparse Delta Fraction: " << getSparseDeltaFraction() << std::endl;
580  out << " Sparse Delta: " << sparseDelta_ << std::endl;
581  out << " Stretch Factor: " << getStretchFactor() << std::endl;
582  out << " Maximum Extent: " << si_->getMaximumExtent() << std::endl;
583  out << " Status: " << std::endl;
584  out << " Vertices Count: " << getNumVertices() << std::endl;
585  out << " Edges Count: " << getNumEdges() << std::endl;
586  out << " Iterations: " << getIterations() << std::endl;
587  out << " Consecutive Failures: " << consecutiveFailures_ << std::endl;
588  out << " Number of guards: " << nn_->size() << std::endl
589  << std::endl;
590 }
591 
592 bool ompl::geometric::SPARSdb::getGuardSpacingFactor(const double pathLength, int &numGuards, double &spacingFactor)
593 {
594  static const double factorHigh = 1.9;
595  static const double factorLow = 1.1;
596  double minPathLength = sparseDelta_ * factorLow;
597 
598  // Check if the path length is too short
599  if (pathLength < minPathLength)
600  {
601  OMPL_INFORM("Path length is too short to get a correct sparcing factor: length: %f, min: %f ", pathLength,
602  minPathLength);
603  spacingFactor = factorLow;
604  return true; // still attempt
605  }
606 
607  // Get initial guess using med value
608  double numGuardsFraction = pathLength / (sparseDelta_ * factorLow);
609  if (verbose_)
610  {
611  OMPL_INFORM("getGuardSpacingFactor: ");
612  OMPL_INFORM(" pathLength: %f", pathLength);
613  OMPL_INFORM(" sparseDelta: %f", sparseDelta_);
614  OMPL_INFORM(" min pathLength: %f", minPathLength);
615  OMPL_INFORM(" numGuardsFraction: %f", numGuardsFraction);
616  }
617 
618  // Round down to nearest integer
619  numGuards = numGuardsFraction;
620 
621  static std::size_t MAX_ATTEMPTS = 4;
622  for (std::size_t i = 0; i < MAX_ATTEMPTS; ++i)
623  {
624  if (verbose_)
625  OMPL_INFORM(" numGuards: %d", numGuards);
626 
627  // Find the factor to achieve this number of guards
628  spacingFactor = pathLength / (sparseDelta_ * numGuards);
629  if (verbose_)
630  OMPL_INFORM(" new spacingFactor: %f", spacingFactor);
631 
632  // Check if this factor is too low
633  if (spacingFactor < factorLow)
634  {
635  if (verbose_)
636  OMPL_INFORM(" spacing factor is too low ");
637  numGuards++;
638  continue;
639  }
640  else if (spacingFactor > factorHigh)
641  {
642  if (verbose_)
643  OMPL_INFORM(" spacing factor is too high ");
644  numGuards--;
645  continue;
646  }
647  else
648  return true; // a good value
649  }
650 
651  OMPL_ERROR("Unable to find correct spacing factor - perhaps this is a bug");
652  spacingFactor = factorLow;
653  return true; // still attempt
654 }
655 
656 bool ompl::geometric::SPARSdb::addPathToRoadmap(const base::PlannerTerminationCondition &ptc,
657  ompl::geometric::PathGeometric &solutionPath)
658 {
659  // Check that the query vertex is initialized (used for internal nearest neighbor searches)
661 
662  // Error check
663  if (solutionPath.getStateCount() < 2)
664  {
665  OMPL_ERROR("Less than 2 states were passed to addPathToRoadmap in the solution path");
666  return false;
667  }
668 
669  // Find spacing factor - 2.0 would be a perfect amount, but we leave room for rounding/interpolation errors and
670  // curves in path
671  int numGuards; // unused variable that indicates how many guards we will add
672  double spacingFactor;
673  if (!getGuardSpacingFactor(solutionPath.length(), numGuards, spacingFactor))
674  return false;
675 
676  OMPL_DEBUG("Expected number of necessary coverage guards is calculated to be %i from the original path state count "
677  "%i",
678  numGuards, solutionPath.getStateCount());
679 
680  unsigned int n = 0;
681  const int n1 = solutionPath.getStateCount() - 1;
682  for (int i = 0; i < n1; ++i)
683  n += si_->getStateSpace()->validSegmentCount(solutionPath.getState(i), solutionPath.getState(i + 1));
684 
685  solutionPath.interpolate(n);
686 
687  // Debug
688  if (verbose_)
689  {
690  OMPL_INFORM("-------------------------------------------------------");
691  OMPL_INFORM("Attempting to add %d states to roadmap", solutionPath.getStateCount());
692  OMPL_INFORM("-------------------------------------------------------");
693  }
694 
695  // Try to add the start first, but don't force it
696  addStateToRoadmap(ptc, solutionPath.getState(0));
697 
698 #ifdef OMPL_THUNDER_DEBUG
699  visualizeStateCallback(solutionPath.getState(solutionPath.getStateCount() - 1), 3, sparseDelta_);
700 #endif
701 
702  // Add solution states to SPARSdb one by one ---------------------------
703 
704  // Track which nodes we've already tried to add
705  std::vector<std::size_t> addedStateIDs;
706  // Track which nodes we will attempt to use as connectivity states
707  std::vector<std::size_t> connectivityStateIDs;
708  // std::vector<base::State*> connectivityStates;
709 
710  double distanceFromLastState = 0;
711 
712  std::size_t lastStateID = 0; // track the id in the solutionPath of the last state
713 
714  for (std::size_t i = 1; i < solutionPath.getStateCount();
715  ++i) // skip 0 and last because those are start/goal and are already added
716  {
717  distanceFromLastState = si_->distance(solutionPath.getState(i), solutionPath.getState(lastStateID));
718 
719  if (verbose_ && false)
720  {
721  OMPL_INFORM("Index %d at distance %f from last state ", i, distanceFromLastState);
722  }
723 
724  if (distanceFromLastState >= sparseDelta_ * spacingFactor)
725  {
726  if (verbose_)
727  {
728  OMPL_INFORM("Adding state %d of %d", i, solutionPath.getStateCount());
729  }
730 
731 // Show the candidate state in Rviz for path insertion of GUARDS
732 #ifdef OMPL_THUNDER_DEBUG
733  visualizeStateCallback(solutionPath.getState(i), 1, sparseDelta_);
734 #endif
735 
736  // Add a single state to the roadmap
737  if (!addStateToRoadmap(ptc, solutionPath.getState(i)))
738  {
739  if (verbose_)
740  {
741  OMPL_INFORM("Last state added to roadmap failed ");
742  }
743  }
744 
745  // Now figure out midpoint state between lastState and i
746  std::size_t midStateID = (i - lastStateID) / 2 + lastStateID;
747  connectivityStateIDs.push_back(midStateID);
748 
749  double distA = si_->distance(solutionPath.getState(lastStateID), solutionPath.getState(midStateID));
750  double distB = si_->distance(solutionPath.getState(i), solutionPath.getState(midStateID));
751  double diff = distA - distB;
752  if ((diff < std::numeric_limits<double>::epsilon()) && (-diff < std::numeric_limits<double>::epsilon()))
753  if (verbose_)
754  OMPL_WARN("DISTANCES ARE DIFFERENT ");
755 
756  // Save this state as the new last state
757  lastStateID = i;
758  // Remember which nodes we've already added / attempted to add
759  addedStateIDs.push_back(midStateID);
760  addedStateIDs.push_back(i);
761  }
762  // Close up if it doesn't do it automatically
763  else if (i == solutionPath.getStateCount() - 1)
764  {
765  if (verbose_)
766  OMPL_INFORM("Last state - do special midpoint");
767 
768  // Now figure out midpoint state between lastState and i
769  std::size_t midStateID = (i - lastStateID) / 2 + lastStateID;
770  connectivityStateIDs.push_back(midStateID);
771  addedStateIDs.push_back(midStateID);
772  if (verbose_)
773  OMPL_INFORM("Mid state is %d", midStateID);
774  }
775  }
776 
777  // Attempt to add the goal directly
778  addStateToRoadmap(ptc, solutionPath.getState(solutionPath.getStateCount() - 1));
779 
780  if (verbose_)
781  {
782  OMPL_INFORM("-------------------------------------------------------");
783  OMPL_INFORM("-------------------------------------------------------");
784  OMPL_INFORM("Adding connectivity states ----------------------------");
785  OMPL_INFORM("-------------------------------------------------------");
786  OMPL_INFORM("-------------------------------------------------------");
787  }
788 
789  for (std::size_t i = 0; i < connectivityStateIDs.size(); ++i)
790  {
791  base::State *connectivityState = solutionPath.getState(connectivityStateIDs[i]);
792 
793  if (verbose_)
794  {
795  OMPL_INFORM("Adding connectvity state ", i);
796  }
797 
798 #ifdef OMPL_THUNDER_DEBUG
799  // Show the candidate state in Rviz for path insertion of BRIDGES (CONNECTIVITY)
800  visualizeStateCallback(connectivityState, 2, sparseDelta_);
801  sleep(0.5);
802 #endif
803 
804  // Add a single state to the roadmap
805  addStateToRoadmap(ptc, connectivityState);
806  }
807 
808  // Add remaining states at random
809  if (verbose_)
810  {
811  OMPL_INFORM("-------------------------------------------------------");
812  OMPL_INFORM("-------------------------------------------------------");
813  OMPL_INFORM("Adding remaining states randomly ----------------------");
814  OMPL_INFORM("-------------------------------------------------------");
815  OMPL_INFORM("-------------------------------------------------------");
816  }
817 
818  // Create a vector of shuffled indexes
819  std::vector<std::size_t> shuffledIDs;
820  std::size_t usedIDTracker = 0;
821  for (std::size_t i = 1; i < solutionPath.getStateCount(); ++i) // skip 0 because start already added
822  {
823  // Check if we've already used this id
824  if (usedIDTracker < addedStateIDs.size() && i == addedStateIDs[usedIDTracker])
825  {
826  // skip this id
827  usedIDTracker++;
828  continue;
829  }
830 
831  shuffledIDs.push_back(i); // 1 2 3...
832  }
833 
834  std::random_shuffle(shuffledIDs.begin(), shuffledIDs.end()); // using built-in random generator:
835 
836  // Add each state randomly
837  for (unsigned long shuffledID : shuffledIDs)
838  {
839 #ifdef OMPL_THUNDER_DEBUG
840  visualizeStateCallback(solutionPath.getState(shuffledIDs[i]), 1, sparseDelta_);
841 #endif
842 
843  // Add a single state to the roadmap
844  addStateToRoadmap(ptc, solutionPath.getState(shuffledID));
845  }
846 
847  bool benchmarkLogging = true;
848  if (benchmarkLogging)
849  {
850  OMPL_DEBUG("ompl::geometric::SPARSdb: Benchmark logging enabled (slower)");
851 
852  // Return the result of inserting into database, if applicable
853  return checkStartGoalConnection(solutionPath);
854  }
855 
856  return true;
857 }
858 
859 bool ompl::geometric::SPARSdb::checkStartGoalConnection(ompl::geometric::PathGeometric &solutionPath)
860 {
861  // Make sure path has states
862  if (solutionPath.getStateCount() < 2)
863  {
864  OMPL_ERROR("Not enought states (< 2) in the solutionPath");
865  return false;
866  }
867 
868  bool error = false;
869  CandidateSolution candidateSolution;
870  do
871  {
872  base::State *actualStart = solutionPath.getState(0);
873  base::State *actualGoal = solutionPath.getState(solutionPath.getStateCount() - 1);
874 
875  /* The whole neighborhood set which has been most recently computed */
876  std::vector<Vertex> graphNeighborhood;
877  /* The visible neighborhood set which has been most recently computed */
878  std::vector<Vertex> visibleNeighborhood;
879 
880  // Get start vertex
881  findGraphNeighbors(actualStart, graphNeighborhood, visibleNeighborhood);
882  if (!visibleNeighborhood.size())
883  {
884  OMPL_ERROR("No vertexes found near start");
885  error = true;
886  break;
887  }
888  Vertex closeStart = visibleNeighborhood[0];
889 
890  // Get goal vertex
891  findGraphNeighbors(actualGoal, graphNeighborhood, visibleNeighborhood);
892  if (!visibleNeighborhood.size())
893  {
894  OMPL_ERROR("No vertexes found near goal");
895  error = true;
896  break;
897  }
898  Vertex closeGoal = visibleNeighborhood[0];
899 
900  // Check if connected
901  if (false)
902  if (!sameComponent(closeStart, closeGoal))
903  {
904  OMPL_ERROR("Start and goal are not connected!");
905  error = true;
906  break;
907  }
908 
909  // Get new path from start to goal
910  std::vector<Vertex> vertexPath;
911  if (!constructSolution(closeStart, closeGoal, vertexPath))
912  {
913  OMPL_ERROR("Unable to find path from start to goal - perhaps because of new obstacles");
914  error = true;
915  break;
916  }
917 
918  // Convert to PathGeometric
919  bool disableCollisionWarning = true; // this is just for benchmarking purposes
920  if (!convertVertexPathToStatePath(vertexPath, actualStart, actualGoal, candidateSolution,
921  disableCollisionWarning))
922  {
923  OMPL_ERROR("Unable to convert to state path");
924  error = true;
925  break;
926  }
927  } while (false);
928 
929  // Check distance of new path from old path
930  double originalLength = solutionPath.length();
931 
932  OMPL_DEBUG("Results of attempting to make insertion in SPARSdb ");
933  OMPL_DEBUG("-------------------------------------------------------");
934  OMPL_DEBUG("Original length: %f", originalLength);
935 
936  if (error)
937  {
938  OMPL_ERROR("UNABLE TO GET PATH");
939 
940  // Record this for plotting
942  }
943  else
944  {
945  double newLength = candidateSolution.getGeometricPath().length();
946  double percentIncrease = 100 - originalLength / newLength * 100;
947  OMPL_DEBUG("New length: %f", newLength);
948  OMPL_DEBUG("Percent increase: %f %%", percentIncrease);
949  }
950 
951  return !error; // return true if it inserted correctly
952 }
953 
954 bool ompl::geometric::SPARSdb::addStateToRoadmap(const base::PlannerTerminationCondition &ptc, base::State *newState)
955 {
956  bool stateAdded = false;
957  // Check that the query vertex is initialized (used for internal nearest neighbor searches)
959 
960  // Deep copy
961  base::State *qNew = si_->cloneState(newState);
962  base::State *workState = si_->allocState();
963 
964  /* The whole neighborhood set which has been most recently computed */
965  std::vector<Vertex> graphNeighborhood;
966  /* The visible neighborhood set which has been most recently computed */
967  std::vector<Vertex> visibleNeighborhood;
968 
969  ++iterations_;
970 
971  findGraphNeighbors(qNew, graphNeighborhood, visibleNeighborhood);
972 
973  if (verbose_)
974  {
975  OMPL_INFORM(" graph neighborhood: %d | visible neighborhood: %d", graphNeighborhood.size(),
976  visibleNeighborhood.size());
977 
978  foreach (Vertex v, visibleNeighborhood)
979  {
980  OMPL_INFORM("Visible neighbor is vertex %f with distance %f ", v, si_->distance(qNew, stateProperty_[v]));
981  }
982  }
983 
984  if (verbose_)
985  OMPL_INFORM(" - checkAddCoverage() Are other nodes around it visible?");
986  // Coverage criterion
987  if (!checkAddCoverage(qNew,
988  visibleNeighborhood)) // Always add a node if no other nodes around it are visible (GUARD)
989  {
990  if (verbose_)
991  OMPL_INFORM(" -- checkAddConnectivity() Does this node connect neighboring nodes that are not connected? ");
992  // Connectivity criterion
993  if (!checkAddConnectivity(qNew, visibleNeighborhood))
994  {
995  if (verbose_)
996  OMPL_INFORM(" --- checkAddInterface() Does this node's neighbor's need it to better connect them? ");
997  if (!checkAddInterface(qNew, graphNeighborhood, visibleNeighborhood))
998  {
999  if (verbose_)
1000  OMPL_INFORM(" ---- Ensure SPARS asymptotic optimality");
1001  if (visibleNeighborhood.size() > 0)
1002  {
1003  std::map<Vertex, base::State *> closeRepresentatives;
1004  if (verbose_)
1005  OMPL_INFORM(" ----- findCloseRepresentatives()");
1006 
1007  findCloseRepresentatives(workState, qNew, visibleNeighborhood[0], closeRepresentatives, ptc);
1008  if (verbose_)
1009  OMPL_INFORM("------ Found %d close representatives", closeRepresentatives.size());
1010 
1011  for (auto &closeRepresentative : closeRepresentatives)
1012  {
1013  if (verbose_)
1014  OMPL_INFORM(" ------ Looping through close representatives");
1015  updatePairPoints(visibleNeighborhood[0], qNew, closeRepresentative.first,
1016  closeRepresentative.second);
1017  updatePairPoints(closeRepresentative.first, closeRepresentative.second, visibleNeighborhood[0],
1018  qNew);
1019  }
1020  if (verbose_)
1021  OMPL_INFORM(" ------ checkAddPath()");
1022  if (checkAddPath(visibleNeighborhood[0]))
1023  {
1024  if (verbose_)
1025  {
1026  OMPL_INFORM("nearest visible neighbor added ");
1027  }
1028  }
1029 
1030  for (auto &closeRepresentative : closeRepresentatives)
1031  {
1032  if (verbose_)
1033  OMPL_INFORM(" ------- Looping through close representatives to add path");
1034  checkAddPath(closeRepresentative.first);
1035  si_->freeState(closeRepresentative.second);
1036  }
1037  if (verbose_)
1038  OMPL_INFORM("------ Done with inner most loop ");
1039  }
1040  }
1041  else // added for interface
1042  {
1043  stateAdded = true;
1044  }
1045  }
1046  else // added for connectivity
1047  {
1048  stateAdded = true;
1049  }
1050  }
1051  else // added for coverage
1052  {
1053  stateAdded = true;
1054  }
1055 
1056  if (!stateAdded)
1058 
1059  si_->freeState(workState);
1060  si_->freeState(qNew);
1061 
1062  return stateAdded;
1063 }
1064 
1066 {
1067  if (boost::num_vertices(g_) < 1)
1068  {
1069  queryVertex_ = boost::add_vertex(g_);
1070  stateProperty_[queryVertex_] = nullptr;
1071  }
1072 }
1073 
1075 {
1076  // Disabled
1078 }
1079 
1080 bool ompl::geometric::SPARSdb::checkAddCoverage(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood)
1081 {
1082  if (visibleNeighborhood.size() > 0)
1083  return false;
1084  // No free paths means we add for coverage
1085  if (verbose_)
1086  OMPL_INFORM(" --- Adding node for COVERAGE ");
1087  Vertex v = addGuard(si_->cloneState(qNew), COVERAGE);
1088  if (verbose_)
1089  OMPL_INFORM(" Added vertex %f", v);
1090 
1091  return true;
1092 }
1093 
1094 bool ompl::geometric::SPARSdb::checkAddConnectivity(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood)
1095 {
1096  // Identify visibile nodes around our new state that are unconnected (in different connected components)
1097  // and connect them
1098 
1099  std::vector<Vertex> statesInDiffConnectedComponents; // links
1100  if (visibleNeighborhood.size() >
1101  1) // if less than 2 there is no way to find a pair of nodes in different connected components
1102  {
1103  // For each neighbor
1104  for (std::size_t i = 0; i < visibleNeighborhood.size(); ++i)
1105  {
1106  // For each other neighbor
1107  for (std::size_t j = i + 1; j < visibleNeighborhood.size(); ++j)
1108  {
1109  // If they are in different components
1110  if (!sameComponent(visibleNeighborhood[i], visibleNeighborhood[j]))
1111  {
1112  statesInDiffConnectedComponents.push_back(visibleNeighborhood[i]);
1113  statesInDiffConnectedComponents.push_back(visibleNeighborhood[j]);
1114  }
1115  }
1116  }
1117 
1118  // Were any diconnected states found?
1119  if (statesInDiffConnectedComponents.size() > 0)
1120  {
1121  if (verbose_)
1122  OMPL_INFORM(" --- Adding node for CONNECTIVITY ");
1123  // Add the node
1124  Vertex newVertex = addGuard(si_->cloneState(qNew), CONNECTIVITY);
1125 
1126  for (unsigned long statesInDiffConnectedComponent : statesInDiffConnectedComponents)
1127  {
1128  // If there's no edge between the two new states
1129  // DTC: this should actually never happen - we just created the new vertex so
1130  // why would it be connected to anything?
1131  if (!boost::edge(newVertex, statesInDiffConnectedComponent, g_).second)
1132  {
1133  // The components haven't been united by previous links
1134  if (!sameComponent(statesInDiffConnectedComponent, newVertex))
1135  connectGuards(newVertex, statesInDiffConnectedComponent);
1136  }
1137  }
1138 
1139  return true;
1140  }
1141  }
1142  return false;
1143 }
1144 
1145 bool ompl::geometric::SPARSdb::checkAddInterface(const base::State *qNew, std::vector<Vertex> &graphNeighborhood,
1146  std::vector<Vertex> &visibleNeighborhood)
1147 {
1148  // If we have at least 2 neighbors
1149  if (visibleNeighborhood.size() > 1)
1150  {
1151  // If the two closest nodes are also visible
1152  if (graphNeighborhood[0] == visibleNeighborhood[0] && graphNeighborhood[1] == visibleNeighborhood[1])
1153  {
1154  // If our two closest neighbors don't share an edge
1155  if (!boost::edge(visibleNeighborhood[0], visibleNeighborhood[1], g_).second)
1156  {
1157  // If they can be directly connected
1158  if (si_->checkMotion(stateProperty_[visibleNeighborhood[0]], stateProperty_[visibleNeighborhood[1]]))
1159  {
1160  // Connect them
1161  if (verbose_)
1162  OMPL_INFORM(" --- INTERFACE: directly connected nodes ");
1163  connectGuards(visibleNeighborhood[0], visibleNeighborhood[1]);
1164  // And report that we added to the roadmap
1165  resetFailures();
1166  // Report success
1167  return true;
1168  }
1169  else
1170  {
1171  // Add the new node to the graph, to bridge the interface
1172  if (verbose_)
1173  OMPL_INFORM(" --- Adding node for INTERFACE ");
1174  Vertex v = addGuard(si_->cloneState(qNew), INTERFACE);
1175  connectGuards(v, visibleNeighborhood[0]);
1176  connectGuards(v, visibleNeighborhood[1]);
1177  if (verbose_)
1178  OMPL_INFORM(" --- INTERFACE: connected two neighbors through new interface node ");
1179  // Report success
1180  return true;
1181  }
1182  }
1183  }
1184  }
1185  return false;
1186 }
1187 
1189 {
1190  bool spannerPropertyWasViolated = false;
1191 
1192  std::vector<Vertex> rs;
1193  foreach (Vertex r, boost::adjacent_vertices(v, g_))
1194  rs.push_back(r);
1195 
1196  /* Candidate x vertices as described in the method, filled by function computeX(). */
1197  std::vector<Vertex> Xs;
1198 
1199  /* Candidate v" vertices as described in the method, filled by function computeVPP(). */
1200  std::vector<Vertex> VPPs;
1201 
1202  for (std::size_t i = 0; i < rs.size() && !spannerPropertyWasViolated; ++i)
1203  {
1204  Vertex r = rs[i];
1205  computeVPP(v, r, VPPs);
1206  foreach (Vertex rp, VPPs)
1207  {
1208  // First, compute the longest path through the graph
1209  computeX(v, r, rp, Xs);
1210  double rm_dist = 0.0;
1211  foreach (Vertex rpp, Xs)
1212  {
1213  double tmp_dist = (si_->distance(stateProperty_[r], stateProperty_[v]) +
1214  si_->distance(stateProperty_[v], stateProperty_[rpp])) /
1215  2.0;
1216  if (tmp_dist > rm_dist)
1217  rm_dist = tmp_dist;
1218  }
1219 
1220  InterfaceData &d = getData(v, r, rp);
1221 
1222  // Then, if the spanner property is violated
1223  if (rm_dist > stretchFactor_ * d.d_)
1224  {
1225  spannerPropertyWasViolated = true; // Report that we added for the path
1226  if (si_->checkMotion(stateProperty_[r], stateProperty_[rp]))
1227  connectGuards(r, rp);
1228  else
1229  {
1230  auto p(std::make_shared<PathGeometric>(si_));
1231  if (r < rp)
1232  {
1233  p->append(d.sigmaA_);
1234  p->append(d.pointA_);
1235  p->append(stateProperty_[v]);
1236  p->append(d.pointB_);
1237  p->append(d.sigmaB_);
1238  }
1239  else
1240  {
1241  p->append(d.sigmaB_);
1242  p->append(d.pointB_);
1243  p->append(stateProperty_[v]);
1244  p->append(d.pointA_);
1245  p->append(d.sigmaA_);
1246  }
1247 
1248  psimp_->reduceVertices(*p, 10);
1249  psimp_->shortcutPath(*p, 50);
1250 
1251  if (p->checkAndRepair(100).second)
1252  {
1253  Vertex prior = r;
1254  Vertex vnew;
1255  std::vector<base::State *> &states = p->getStates();
1256 
1257  foreach (base::State *st, states)
1258  {
1259  // no need to clone st, since we will destroy p; we just copy the pointer
1260  if (verbose_)
1261  OMPL_INFORM(" --- Adding node for QUALITY");
1262  vnew = addGuard(st, QUALITY);
1263 
1264  connectGuards(prior, vnew);
1265  prior = vnew;
1266  }
1267  // clear the states, so memory is not freed twice
1268  states.clear();
1269  connectGuards(prior, rp);
1270  }
1271  }
1272  }
1273  }
1274  }
1275 
1276  if (!spannerPropertyWasViolated)
1277  {
1278  if (verbose_)
1279  {
1280  OMPL_INFORM(" ------- Spanner property was NOT violated, SKIPPING");
1281  }
1282  }
1283 
1284  return spannerPropertyWasViolated;
1285 }
1286 
1288 {
1290 }
1291 
1292 void ompl::geometric::SPARSdb::findGraphNeighbors(base::State *st, std::vector<Vertex> &graphNeighborhood,
1293  std::vector<Vertex> &visibleNeighborhood)
1294 {
1295  visibleNeighborhood.clear();
1297  nn_->nearestR(queryVertex_, sparseDelta_, graphNeighborhood);
1298  if (verbose_ && false)
1299  OMPL_INFORM("Finding nearest nodes in NN tree within radius %f", sparseDelta_);
1300  stateProperty_[queryVertex_] = nullptr;
1301 
1302  // Now that we got the neighbors from the NN, we must remove any we can't see
1303  for (unsigned long i : graphNeighborhood)
1304  if (si_->checkMotion(st, stateProperty_[i]))
1305  visibleNeighborhood.push_back(i);
1306 }
1307 
1308 bool ompl::geometric::SPARSdb::findGraphNeighbors(const base::State *state, std::vector<Vertex> &graphNeighborhood)
1309 {
1310  base::State *stateCopy = si_->cloneState(state);
1311 
1312  // Don't check for visibility
1313  graphNeighborhood.clear();
1314  stateProperty_[queryVertex_] = stateCopy;
1315 
1316  // Double the range of sparseDelta_ up to 3 times until at least 1 neighbor is found
1317  std::size_t expandNeighborhoodSearchAttempts = 3;
1318  double neighborSearchRadius;
1319  static const double EXPAND_NEIGHBORHOOD_RATE =
1320  0.25; // speed to which we look outside the original sparse delta neighborhood
1321  for (std::size_t i = 0; i < expandNeighborhoodSearchAttempts; ++i)
1322  {
1323  neighborSearchRadius = sparseDelta_ + i * EXPAND_NEIGHBORHOOD_RATE * sparseDelta_;
1324  if (verbose_)
1325  {
1326  OMPL_INFORM("-------------------------------------------------------");
1327  OMPL_INFORM("Attempt %d to find neighborhood at radius %f", i + 1, neighborSearchRadius);
1328  OMPL_INFORM("-------------------------------------------------------");
1329  }
1330 
1331  nn_->nearestR(queryVertex_, neighborSearchRadius, graphNeighborhood);
1332 
1333  // Check if at least one neighbor found
1334  if (graphNeighborhood.size() > 0)
1335  break;
1336  }
1337  stateProperty_[queryVertex_] = nullptr;
1338 
1339  // Check if no neighbors found
1340  if (!graphNeighborhood.size())
1341  {
1342  return false;
1343  }
1344  return true;
1345 }
1346 
1348 {
1349  std::vector<Vertex> hold;
1350  nn_->nearestR(v, sparseDelta_, hold);
1351 
1352  std::vector<Vertex> neigh;
1353  for (unsigned long i : hold)
1354  if (si_->checkMotion(stateProperty_[v], stateProperty_[i]))
1355  neigh.push_back(i);
1356 
1357  foreach (Vertex vp, neigh)
1358  connectGuards(v, vp);
1359 }
1360 
1362 {
1363  std::vector<Vertex> nbh;
1365  nn_->nearestR(queryVertex_, sparseDelta_, nbh);
1366  stateProperty_[queryVertex_] = nullptr;
1367 
1368  if (verbose_)
1369  OMPL_INFORM(" ------- findGraphRepresentative found %d nearest neighbors of distance %f", nbh.size(),
1370  sparseDelta_);
1371 
1372  Vertex result = boost::graph_traits<Graph>::null_vertex();
1373 
1374  for (std::size_t i = 0; i < nbh.size(); ++i)
1375  {
1376  if (verbose_)
1377  OMPL_INFORM(" -------- Checking motion of graph rep candidate %d", i);
1378  if (si_->checkMotion(st, stateProperty_[nbh[i]]))
1379  {
1380  if (verbose_)
1381  OMPL_INFORM(" --------- VALID ");
1382  result = nbh[i];
1383  break;
1384  }
1385  }
1386  return result;
1387 }
1388 
1390  const Vertex qRep,
1391  std::map<Vertex, base::State *> &closeRepresentatives,
1393 {
1394  // Properly clear the vector by also deleting previously sampled unused states
1395  for (auto &closeRepresentative : closeRepresentatives)
1396  si_->freeState(closeRepresentative.second);
1397  closeRepresentatives.clear();
1398 
1399  // denseDelta_ = 0.25 * sparseDelta_;
1400  nearSamplePoints_ /= 10; // HACK - this makes it look for the same number of samples as dimensions
1401 
1402  if (verbose_)
1403  OMPL_INFORM(" ----- nearSamplePoints: %f, denseDelta: %f", nearSamplePoints_, denseDelta_);
1404 
1405  // Then, begin searching the space around new potential state qNew
1406  for (unsigned int i = 0; i < nearSamplePoints_ && ptc == false; ++i)
1407  {
1408  do
1409  {
1410  sampler_->sampleNear(workState, qNew, denseDelta_);
1411 
1412 #ifdef OMPL_THUNDER_DEBUG
1413  visualizeStateCallback(workState, 3, sparseDelta_);
1414  sleep(0.1);
1415 #endif
1416 
1417  if (verbose_)
1418  {
1419  OMPL_INFORM(" ------ findCloseRepresentatives sampled state ");
1420 
1421  if (!si_->isValid(workState))
1422  {
1423  OMPL_INFORM(" ------ isValid ");
1424  }
1425  if (si_->distance(qNew, workState) > denseDelta_)
1426  {
1427  OMPL_INFORM(" ------ Distance too far ");
1428  }
1429  if (!si_->checkMotion(qNew, workState))
1430  {
1431  OMPL_INFORM(" ------ Motion invalid ");
1432  }
1433  }
1434 
1435  } while ((!si_->isValid(workState) || si_->distance(qNew, workState) > denseDelta_ ||
1436  !si_->checkMotion(qNew, workState)) &&
1437  ptc == false);
1438 
1439  // if we were not successful at sampling a desirable state, we are out of time
1440  if (ptc == true)
1441  {
1442  if (verbose_)
1443  OMPL_INFORM(" ------ We are out of time ");
1444  break;
1445  }
1446 
1447  if (verbose_)
1448  OMPL_INFORM(" ------ Find graph representative ");
1449 
1450  // Compute who his graph neighbors are
1451  Vertex representative = findGraphRepresentative(workState);
1452 
1453  // Assuming this sample is actually seen by somebody (which he should be in all likelihood)
1454  if (representative != boost::graph_traits<Graph>::null_vertex())
1455  {
1456  if (verbose_)
1457  OMPL_INFORM(" ------ Representative is not null ");
1458 
1459  // If his representative is different than qNew
1460  if (qRep != representative)
1461  {
1462  if (verbose_)
1463  OMPL_INFORM(" ------ qRep != representative ");
1464 
1465  // And we haven't already tracked this representative
1466  if (closeRepresentatives.find(representative) == closeRepresentatives.end())
1467  {
1468  if (verbose_)
1469  OMPL_INFORM(" ------ Track the representative");
1470  // Track the representativen
1471  closeRepresentatives[representative] = si_->cloneState(workState);
1472  }
1473  }
1474  else
1475  {
1476  if (verbose_)
1477  OMPL_INFORM(" ------ qRep == representative, no good ");
1478  }
1479  }
1480  else
1481  {
1482  if (verbose_)
1483  OMPL_INFORM(" ------ Rep is null ");
1484 
1485  // This guy can't be seen by anybody, so we should take this opportunity to add him
1486  if (verbose_)
1487  OMPL_INFORM(" --- Adding node for COVERAGE");
1488  addGuard(si_->cloneState(workState), COVERAGE);
1489 
1490  if (verbose_)
1491  {
1492  OMPL_INFORM(" ------ STOP EFFORS TO ADD A DENSE PATH");
1493  }
1494 
1495  // We should also stop our efforts to add a dense path
1496  for (auto &closeRepresentative : closeRepresentatives)
1497  si_->freeState(closeRepresentative.second);
1498  closeRepresentatives.clear();
1499  break;
1500  }
1501  } // for loop
1502 }
1503 
1505 {
1506  // First of all, we need to compute all candidate r'
1507  std::vector<Vertex> VPPs;
1508  computeVPP(rep, r, VPPs);
1509 
1510  // Then, for each pair Pv(r,r')
1511  foreach (Vertex rp, VPPs)
1512  // Try updating the pair info
1513  distanceCheck(rep, q, r, s, rp);
1514 }
1515 
1516 void ompl::geometric::SPARSdb::computeVPP(Vertex v, Vertex vp, std::vector<Vertex> &VPPs)
1517 {
1518  VPPs.clear();
1519  foreach (Vertex cvpp, boost::adjacent_vertices(v, g_))
1520  if (cvpp != vp)
1521  if (!boost::edge(cvpp, vp, g_).second)
1522  VPPs.push_back(cvpp);
1523 }
1524 
1525 void ompl::geometric::SPARSdb::computeX(Vertex v, Vertex vp, Vertex vpp, std::vector<Vertex> &Xs)
1526 {
1527  Xs.clear();
1528 
1529  foreach (Vertex cx, boost::adjacent_vertices(vpp, g_))
1530  if (boost::edge(cx, v, g_).second && !boost::edge(cx, vp, g_).second)
1531  {
1532  InterfaceData &d = getData(v, vpp, cx);
1533  if ((vpp < cx && d.pointA_) || (cx < vpp && d.pointB_))
1534  Xs.push_back(cx);
1535  }
1536  Xs.push_back(vpp);
1537 }
1538 
1540 {
1541  if (vp < vpp)
1542  return VertexPair(vp, vpp);
1543  else if (vpp < vp)
1544  return VertexPair(vpp, vp);
1545  else
1546  throw Exception(name_, "Trying to get an index where the pairs are the same point!");
1547 }
1548 
1550 {
1551  return interfaceDataProperty_[v].interfaceHash[index(vp, vpp)];
1552 }
1553 
1555  Vertex rp)
1556 {
1557  // Get the info for the current representative-neighbors pair
1558  InterfaceData &d = getData(rep, r, rp);
1559 
1560  if (r < rp) // FIRST points represent r (the guy discovered through sampling)
1561  {
1562  if (d.pointA_ == nullptr) // If the point we're considering replacing (P_v(r,.)) isn't there
1563  // Then we know we're doing better, so add it
1564  d.setFirst(q, s, si_);
1565  else // Otherwise, he is there,
1566  {
1567  if (d.pointB_ == nullptr) // But if the other guy doesn't exist, we can't compare.
1568  {
1569  // Should probably keep the one that is further away from rep? Not known what to do in this case.
1570  // TODO: is this not part of the algorithm?
1571  }
1572  else // We know both of these points exist, so we can check some distances
1573  if (si_->distance(q, d.pointB_) < si_->distance(d.pointA_, d.pointB_))
1574  // Distance with the new point is good, so set it.
1575  d.setFirst(q, s, si_);
1576  }
1577  }
1578  else // SECOND points represent r (the guy discovered through sampling)
1579  {
1580  if (d.pointB_ == nullptr) // If the point we're considering replacing (P_V(.,r)) isn't there...
1581  // Then we must be doing better, so add it
1582  d.setSecond(q, s, si_);
1583  else // Otherwise, he is there
1584  {
1585  if (d.pointA_ == nullptr) // But if the other guy doesn't exist, we can't compare.
1586  {
1587  // Should we be doing something cool here?
1588  }
1589  else if (si_->distance(q, d.pointA_) < si_->distance(d.pointB_, d.pointA_))
1590  // Distance with the new point is good, so set it
1591  d.setSecond(q, s, si_);
1592  }
1593  }
1594 
1595  // Lastly, save what we have discovered
1596  interfaceDataProperty_[rep].interfaceHash[index(r, rp)] = d;
1597 }
1598 
1600 {
1602 
1603  std::vector<Vertex> hold;
1604  nn_->nearestR(queryVertex_, sparseDelta_, hold);
1605 
1606  stateProperty_[queryVertex_] = nullptr;
1607 
1608  // For each of the vertices
1609  foreach (Vertex v, hold)
1610  {
1611  foreach (VertexPair r, interfaceDataProperty_[v].interfaceHash | boost::adaptors::map_keys)
1612  interfaceDataProperty_[v].interfaceHash[r].clear(si_);
1613  }
1614 }
1615 
1617 {
1618  Vertex m = boost::add_vertex(g_);
1619  stateProperty_[m] = state;
1620  colorProperty_[m] = type;
1621 
1622  // assert(si_->isValid(state));
1623  abandonLists(state);
1624 
1625  disjointSets_.make_set(m);
1626  nn_->add(m);
1627  resetFailures();
1628 
1629  if (verbose_)
1630  {
1631  OMPL_INFORM(" ---- addGuard() of type %f", type);
1632  }
1633 #ifdef OMPL_THUNDER_DEBUG
1634  visualizeStateCallback(state, 4, sparseDelta_); // Candidate node has already (just) been added
1635  sleep(0.1);
1636 #endif
1637 
1638  return m;
1639 }
1640 
1642 {
1643  // OMPL_INFORM("connectGuards called ---------------------------------------------------------------- ");
1644  assert(v <= getNumVertices());
1645  assert(vp <= getNumVertices());
1646 
1647  if (verbose_)
1648  {
1649  OMPL_INFORM(" ------- connectGuards/addEdge: Connecting vertex %f to vertex %f", v, vp);
1650  }
1651 
1652  // Create the new edge
1653  Edge e = (boost::add_edge(v, vp, g_)).first;
1654 
1655  // Add associated properties to the edge
1656  edgeWeightProperty_[e] = distanceFunction(v, vp); // TODO: use this value with astar
1657  edgeCollisionStateProperty_[e] = NOT_CHECKED;
1658 
1659  // Add the edge to the incrementeal connected components datastructure
1660  disjointSets_.union_set(v, vp);
1661 
1662 // Debug in Rviz
1663 #ifdef OMPL_THUNDER_DEBUG
1664  visualizeEdgeCallback(stateProperty_[v], stateProperty_[vp]);
1665  sleep(0.8);
1666 #endif
1667 }
1668 
1669 bool ompl::geometric::SPARSdb::convertVertexPathToStatePath(std::vector<Vertex> &vertexPath,
1670  const base::State *actualStart,
1671  const base::State *actualGoal,
1672  CandidateSolution &candidateSolution,
1673  bool disableCollisionWarning)
1674 {
1675  if (!vertexPath.size())
1676  return false;
1677 
1678  auto pathGeometric(std::make_shared<ompl::geometric::PathGeometric>(si_));
1679  candidateSolution.isApproximate_ = false; // assume path is valid
1680 
1681  // Add original start if it is different than the first state
1682  if (actualStart != stateProperty_[vertexPath.back()])
1683  {
1684  pathGeometric->append(actualStart);
1685 
1686  // Add the edge status
1687  // the edge from actualStart to start is always valid otherwise we would not have used that start
1688  candidateSolution.edgeCollisionStatus_.push_back(FREE);
1689  }
1690 
1691  // Reverse the vertexPath and convert to state path
1692  for (std::size_t i = vertexPath.size(); i > 0; --i)
1693  {
1694  pathGeometric->append(stateProperty_[vertexPath[i - 1]]);
1695 
1696  // Add the edge status
1697  if (i > 1) // skip the last vertex (its reversed)
1698  {
1699  Edge thisEdge = boost::edge(vertexPath[i - 1], vertexPath[i - 2], g_).first;
1700 
1701  // Check if any edges in path are not free (then it an approximate path)
1702  if (edgeCollisionStateProperty_[thisEdge] == IN_COLLISION)
1703  {
1704  candidateSolution.isApproximate_ = true;
1705  candidateSolution.edgeCollisionStatus_.push_back(IN_COLLISION);
1706  }
1707  else if (edgeCollisionStateProperty_[thisEdge] == NOT_CHECKED)
1708  {
1709  if (!disableCollisionWarning)
1710  OMPL_ERROR("A chosen path has an edge that has not been checked for collision. This should not "
1711  "happen");
1712  candidateSolution.edgeCollisionStatus_.push_back(NOT_CHECKED);
1713  }
1714  else
1715  {
1716  candidateSolution.edgeCollisionStatus_.push_back(FREE);
1717  }
1718  }
1719  }
1720 
1721  // Add original goal if it is different than the last state
1722  if (actualGoal != stateProperty_[vertexPath.front()])
1723  {
1724  pathGeometric->append(actualGoal);
1725 
1726  // Add the edge status
1727  // the edge from actualGoal to goal is always valid otherwise we would not have used that goal
1728  candidateSolution.edgeCollisionStatus_.push_back(FREE);
1729  }
1730 
1731  candidateSolution.path_ = pathGeometric;
1732 
1733  return true;
1734 }
1735 
1737 {
1738  Planner::getPlannerData(data);
1739 
1740  // Explicitly add start and goal states:
1741  for (unsigned long i : startM_)
1743 
1744  for (unsigned long i : goalM_)
1746 
1747  // I'm curious:
1748  if (goalM_.size() > 0)
1749  {
1750  throw Exception(name_, "SPARS2 has goal states?");
1751  }
1752  if (startM_.size() > 0)
1753  {
1754  throw Exception(name_, "SPARS2 has start states?");
1755  }
1756 
1757  // If there are even edges here
1758  if (boost::num_edges(g_) > 0)
1759  {
1760  // Adding edges and all other vertices simultaneously
1761  foreach (const Edge e, boost::edges(g_))
1762  {
1763  const Vertex v1 = boost::source(e, g_);
1764  const Vertex v2 = boost::target(e, g_);
1765 
1766  // TODO save weights!
1769 
1770  // OMPL_INFORM("Adding edge from vertex of type %d to vertex of type %d", colorProperty_[v1],
1771  // colorProperty_[v2]);
1772  }
1773  }
1774  // else
1775  // OMPL_INFORM("%s: There are no edges in the graph!", getName().c_str());
1776 
1777  // Make sure to add edge-less nodes as well
1778  foreach (const Vertex n, boost::vertices(g_))
1779  if (boost::out_degree(n, g_) == 0)
1781 
1782  data.properties["iterations INTEGER"] = boost::lexical_cast<std::string>(iterations_);
1783 }
1784 
1786 {
1787  // Check that the query vertex is initialized (used for internal nearest neighbor searches)
1789 
1790  // Add all vertices
1791  if (verbose_)
1792  {
1793  OMPL_INFORM("SPARS::setPlannerData: numVertices=%d", data.numVertices());
1794  }
1795  OMPL_INFORM("Loading PlannerData into SPARSdb");
1796 
1797  std::vector<Vertex> idToVertex;
1798 
1799  // Temp disable verbose mode for loading database
1800  bool wasVerbose = verbose_;
1801  verbose_ = false;
1802 
1803  OMPL_INFORM("Loading vertices:");
1804  // Add the nodes to the graph
1805  for (std::size_t vertexID = 0; vertexID < data.numVertices(); ++vertexID)
1806  {
1807  // Get the state from loaded planner data
1808  const base::State *oldState = data.getVertex(vertexID).getState();
1809  base::State *state = si_->cloneState(oldState);
1810 
1811  // Get the tag, which in this application represents the vertex type
1812  GuardType type = static_cast<GuardType>(data.getVertex(vertexID).getTag());
1813 
1814  // ADD GUARD
1815  idToVertex.push_back(addGuard(state, type));
1816  }
1817 
1818  OMPL_INFORM("Loading edges:");
1819  // Add the corresponding edges to the graph
1820  std::vector<unsigned int> edgeList;
1821  for (std::size_t fromVertex = 0; fromVertex < data.numVertices(); ++fromVertex)
1822  {
1823  edgeList.clear();
1824 
1825  // Get the edges
1826  data.getEdges(fromVertex, edgeList); // returns num of edges
1827 
1828  Vertex m = idToVertex[fromVertex];
1829 
1830  // Process edges
1831  for (unsigned int toVertex : edgeList)
1832  {
1833  Vertex n = idToVertex[toVertex];
1834 
1835  // Add the edge to the graph
1836  const base::Cost weight(0);
1837  if (verbose_ && false)
1838  {
1839  OMPL_INFORM(" Adding edge from vertex id %d to id %d into edgeList", fromVertex, toVertex);
1840  OMPL_INFORM(" Vertex %d to %d", m, n);
1841  }
1842  connectGuards(m, n);
1843  }
1844  } // for
1845 
1846  // Re-enable verbose mode, if necessary
1847  verbose_ = wasVerbose;
1848 }
1849 
1851 {
1852  foreach (const Edge e, boost::edges(g_))
1853  edgeCollisionStateProperty_[e] = NOT_CHECKED; // each edge has an unknown state
1854 }
double d_
Last known distance between the two interfaces supported by points_ and sigmas.
Definition: SPARSdb.h:122
long unsigned int getIterations() const
Get the number of iterations the algorithm performed.
Definition: SPARSdb.h:557
double getDenseDeltaFraction() const
Retrieve the dense graph interface support delta.
Definition: SPARSdb.h:429
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:212
double denseDeltaFraction_
Maximum range for allowing two samples to support an interface as a fraction of maximum extent...
Definition: SPARSdb.h:735
void setDenseDeltaFraction(double d)
Sets interface support tolerance as a fraction of max. extent.
Definition: SPARSdb.h:409
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
boost::property_map< Graph, vertex_color_t >::type colorProperty_
Access to the colors for the vertices.
Definition: SPARSdb.h:759
Interface information storage class, which does bookkeeping for criterion four.
Definition: SPARSdb.h:111
void examine_vertex(Vertex u, const Graph &g) const
Definition: SPARSdb.cpp:90
bool reachedFailureLimit() const
Returns whether we have reached the iteration failures limit, maxFailures_.
Definition: SPARSdb.cpp:568
A shared pointer wrapper for ompl::base::ProblemDefinition.
double length() const override
Compute the length of a geometric path (sum of lengths of segments that make up the path) ...
void findCloseRepresentatives(base::State *workState, 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: SPARSdb.cpp:1389
bool convertVertexPathToStatePath(std::vector< Vertex > &vertexPath, const base::State *actualStart, const base::State *actualGoal, CandidateSolution &candidateSolution, bool disableCollisionWarning=false)
Convert astar results to correctly ordered path.
Definition: SPARSdb.cpp:1669
SPARSdb(const base::SpaceInformationPtr &si)
Constructor.
Definition: SPARSdb.cpp:98
The planner failed to find a solution.
Definition: PlannerStatus.h:62
boost::disjoint_sets< boost::property_map< Graph, boost::vertex_rank_t >::type, boost::property_map< Graph, boost::vertex_predecessor_t >::type > disjointSets_
Data structure that maintains the connected components.
Definition: SPARSdb.h:766
void setMaxFailures(unsigned int m)
Sets the maximum failures until termination.
Definition: SPARSdb.h:417
void printDebug(std::ostream &out=std::cout) const
Print debug information about planner.
Definition: SPARSdb.cpp:573
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:206
Graph g_
Connectivity graph.
Definition: SPARSdb.h:716
void findGraphNeighbors(base::State *state, std::vector< Vertex > &graphNeighborhood, std::vector< Vertex > &visibleNeighborhood)
Finds visible nodes in the graph near state.
Definition: SPARSdb.cpp:1292
boost::property_map< Graph, boost::edge_weight_t >::type edgeWeightProperty_
Access to the weights of each Edge.
Definition: SPARSdb.h:750
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
void clear(const base::SpaceInformationPtr &si)
Clears the given interface data.
Definition: SPARSdb.h:135
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: SPARSdb.cpp:1080
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...
bool lazyCollisionCheck(std::vector< Vertex > &vertexPath, const base::PlannerTerminationCondition &ptc)
Check recalled path for collision and disable as needed.
Definition: SPARSdb.cpp:499
Abstract definition of goals.
Definition: Goal.h:62
unsigned int getNumVertices() const
Get the number of vertices in the sparse roadmap.
Definition: SPARSdb.h:518
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
bool verbose_
Option to enable debugging output.
Definition: SPARSdb.h:790
boost::property_map< Graph, vertex_interface_data_t >::type interfaceDataProperty_
Access to the interface pair information for the vertices.
Definition: SPARSdb.h:762
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
bool addedSolution_
A flag indicating that a solution has been added during solve()
Definition: SPARSdb.h:771
base::State * pointA_
States which lie inside the visibility region of a vertex and support an interface.
Definition: SPARSdb.h:114
boost::graph_traits< Graph >::edge_descriptor Edge
Edge in Graph.
Definition: SPARSdb.h:305
bool checkAddPath(Vertex v)
Checks vertex v for short paths through its region and adds when appropriate.
Definition: SPARSdb.cpp:1188
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
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: SPARSdb.cpp:1554
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added...
double sparseDeltaFraction_
Maximum visibility range for nodes in the graph as a fraction of maximum extent.
Definition: SPARSdb.h:731
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: SPARSdb.cpp:1145
Struct for passing around partially solved solutions.
Definition: SPARSdb.h:240
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: SPARSdb.cpp:148
Vertex queryVertex_
Vertex for performing nearest neighbor queries.
Definition: SPARSdb.h:725
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: SPARSdb.cpp:1525
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:418
base::State * getState(unsigned int index)
Get the state located at index along the path.
void resetFailures()
A reset function for resetting the failures count.
Definition: SPARSdb.cpp:1287
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist...
void freeMemory()
Free all the memory allocated by the planner.
Definition: SPARSdb.cpp:189
unsigned int maxFailures_
The number of consecutive failures to add to the graph before termination.
Definition: SPARSdb.h:738
bool getSimilarPaths(int nearestK, const base::State *start, const base::State *goal, CandidateSolution &candidateSolution, const base::PlannerTerminationCondition &ptc)
Search the roadmap for the best path close to the given start and goal states that is valid...
Definition: SPARSdb.cpp:208
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
unsigned int numPathInsertionFailures_
Track how many solutions fail to have connectivity at end.
Definition: SPARSdb.h:741
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: SPARSdb.h:704
base::State * sigmaA_
States which lie just outside the visibility region of a vertex and support an interface.
Definition: SPARSdb.h:118
Vertex findGraphRepresentative(base::State *st)
Finds the representative of the input state, st.
Definition: SPARSdb.cpp:1361
void checkQueryStateInitialization()
Check that the query vertex is initialized (used for internal nearest neighbor searches) ...
Definition: SPARSdb.cpp:1065
std::vector< Vertex > startVertexCandidateNeighbors_
Used by getSimilarPaths.
Definition: SPARSdb.h:786
std::pair< VertexIndexType, VertexIndexType > VertexPair
Pair of vertices which support an interface.
Definition: SPARSdb.h:107
edgeWeightMap(const Graph &graph, const EdgeCollisionStateMap &collisionStates)
Definition: SPARSdb.cpp:59
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void interpolate(unsigned int count)
Insert a number of states in a path so that the path is made up of exactly count states. States are inserted uniformly (more states on longer segments). Changes are performed only if a path has less than count states.
long unsigned int iterations_
A counter for the number of iterations of the algorithm.
Definition: SPARSdb.h:777
boost::graph_traits< Graph >::vertex_descriptor Vertex
Vertex in Graph.
Definition: SPARSdb.h:302
VertexPair index(Vertex vp, Vertex vpp)
Rectifies indexing order for accessing the vertex data.
Definition: SPARSdb.cpp:1539
void setProblemDefinition(const base::ProblemDefinitionPtr &pdef) override
Set the problem definition for the planner. The problem needs to be set before calling solve()...
Definition: SPARSdb.cpp:165
boost::property_map< Graph, edge_collision_state_t >::type EdgeCollisionStateMap
Access map that stores the lazy collision checking status of each edge.
Definition: SPARSdb.h:311
void clearEdgeCollisionStates()
Clear all past edge state information about in collision or not.
Definition: SPARSdb.cpp:1850
void setPlannerData(const base::PlannerData &data)
Set the sparse graph from file.
Definition: SPARSdb.cpp:1785
~SPARSdb() override
Destructor.
Definition: SPARSdb.cpp:143
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...
base::ValidStateSamplerPtr sampler_
Sampler user for generating valid samples in the state space.
Definition: SPARSdb.h:710
A shared pointer wrapper for ompl::base::SpaceInformation.
bool getPaths(const std::vector< Vertex > &candidateStarts, const std::vector< Vertex > &candidateGoals, const base::State *actualStart, const base::State *actualGoal, CandidateSolution &candidateSolution, const base::PlannerTerminationCondition &ptc)
Check if there exists a solution, i.e., there exists a pair of milestones such that the first is in s...
Definition: SPARSdb.cpp:268
unsigned int nearSamplePoints_
Number of sample points to use when trying to detect interfaces.
Definition: SPARSdb.h:744
void computeVPP(Vertex v, Vertex vp, std::vector< Vertex > &VPPs)
Computes all nodes which qualify as a candidate v" for v and vp.
Definition: SPARSdb.cpp:1516
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...
bool checkAddConnectivity(const base::State *qNew, std::vector< Vertex > &visibleNeighborhood)
Checks to see if the sample needs to be added to ensure connectivity.
Definition: SPARSdb.cpp:1094
Definition of an abstract state.
Definition: State.h:49
void setSparseDeltaFraction(double D)
Sets vertex visibility range as a fraction of max. extent.
Definition: SPARSdb.h:401
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition: SPARSdb.h:756
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:421
double stretchFactor_
Stretch Factor as per graph spanner literature (multiplicative bound on path quality) ...
Definition: SPARSdb.h:728
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: SPARSdb.h:176
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:427
double sparseDelta_
Maximum visibility range for nodes in the graph.
Definition: SPARSdb.h:780
The exception type for ompl.
Definition: Exception.h:46
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
virtual const State * getState() const
Retrieve the state associated with this vertex.
Definition: PlannerData.h:80
unsigned int consecutiveFailures_
A counter for the number of consecutive failed iterations of the algorithm.
Definition: SPARSdb.h:774
std::vector< Vertex > goalM_
Array of goal milestones.
Definition: SPARSdb.h:722
void approachGraph(Vertex v)
Approaches the graph from a given vertex.
Definition: SPARSdb.cpp:1347
unsigned int getNumEdges() const
Get the number of edges in the sparse roadmap.
Definition: SPARSdb.h:524
EdgeCollisionStateMap edgeCollisionStateProperty_
Access to the collision checking state of each Edge.
Definition: SPARSdb.h:753
std::string name_
The name of this planner.
Definition: Planner.h:424
InterfaceData & getData(Vertex v, Vertex vp, Vertex vpp)
Retrieves the Vertex data associated with v,vp,vpp.
Definition: SPARSdb.cpp:1549
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, VertexProperties, EdgeProperties > Graph
Definition: SPARSdb.h:299
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: SPARSdb.cpp:563
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: SPARSdb.cpp:1504
std::vector< Vertex > startM_
Array of start milestones.
Definition: SPARSdb.h:719
PathSimplifierPtr psimp_
A path simplifier used to simplify dense paths added to the graph.
Definition: SPARSdb.h:747
bool constructSolution(const Vertex start, const Vertex goal, std::vector< Vertex > &vertexPath) const
Given two milestones from the same connected component, construct a path connecting them and set it a...
Definition: SPARSdb.cpp:430
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:216
unsigned int getEdges(unsigned int v, std::vector< unsigned int > &edgeList) const
Returns a list of the vertex indexes directly connected to vertex with index v (outgoing edges)...
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: SPARSdb.cpp:1616
unsigned int getMaxFailures() const
Retrieve the maximum consecutive failure limit.
Definition: SPARSdb.h:423
void restart()
Forget how many states were returned by nextStart() and nextGoal() and return all states again...
Definition: Planner.cpp:171
std::shared_ptr< NearestNeighbors< Vertex > > nn_
Nearest neighbors data structure.
Definition: SPARSdb.h:713
double getStretchFactor() const
Retrieve the spanner&#39;s set stretch factor.
Definition: SPARSdb.h:441
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: SPARSdb.cpp:1074
Definition of a geometric path.
Definition: PathGeometric.h:60
void abandonLists(base::State *st)
When a new guard is added at state st, finds all guards who must abandon their interface information ...
Definition: SPARSdb.cpp:1599
bool lazyCollisionSearch(const Vertex &start, const Vertex &goal, const base::State *actualStart, const base::State *actualGoal, CandidateSolution &candidateSolution, const base::PlannerTerminationCondition &ptc)
Repeatidly search through graph for connection then check for collisions then repeat.
Definition: SPARSdb.cpp:322
void connectGuards(Vertex v, Vertex vp)
Connect two guards in the roadmap.
Definition: SPARSdb.cpp:1641
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
void clearQuery()
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition: SPARSdb.cpp:171
virtual int getTag() const
Returns the integer tag associated with this vertex.
Definition: PlannerData.h:70
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: SPARSdb.cpp:1736
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: SPARSdb.h:161
void setStretchFactor(double t)
Sets the stretch factor.
Definition: SPARSdb.h:395
GuardType
Enumeration which specifies the reason a guard is added to the spanner.
Definition: SPARSdb.h:89
double getSparseDeltaFraction() const
Retrieve the sparse graph visibility range delta.
Definition: SPARSdb.h:435
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
double denseDelta_
Maximum range for allowing two samples to support an interface.
Definition: SPARSdb.h:783
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
Definition: PlannerData.h:406
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: SPARSdb.cpp:178
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