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