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