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();
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");
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 {
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
574  << 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)
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::random_shuffle(shuffledIDs.begin(), shuffledIDs.end()); // using built-in random generator:
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 enought 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
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)
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)
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 {
1275 }
1276 
1277 void ompl::geometric::SPARSdb::findGraphNeighbors(base::State *st, std::vector<Vertex> &graphNeighborhood,
1278  std::vector<Vertex> &visibleNeighborhood)
1279 {
1280  visibleNeighborhood.clear();
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;
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 {
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_)
1728 
1729  for (unsigned long i : goalM_)
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!
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)
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)
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 }
double d_
Last known distance between the two interfaces supported by points_ and sigmas.
Definition: SPARSdb.h:122
long unsigned int getIterations() const
Get the number of iterations the algorithm performed.
Definition: SPARSdb.h:550
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
double denseDeltaFraction_
Maximum range for allowing two samples to support an interface as a fraction of maximum extent...
Definition: SPARSdb.h:728
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:1374
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
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:1277
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:1065
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
unsigned int getNumVertices() const
Get the number of vertices in the sparse roadmap.
Definition: SPARSdb.h:511
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
bool verbose_
Option to enable debugging output.
Definition: SPARSdb.h:783
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:1173
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:1539
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added...
double sparseDeltaFraction_
Maximum visibility range for nodes in the graph as a fraction of maximum extent.
Definition: SPARSdb.h:724
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
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
Vertex queryVertex_
Vertex for performing nearest neighbor queries.
Definition: SPARSdb.h:718
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:1510
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:409
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:1272
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
unsigned int maxFailures_
The number of consecutive failures to add to the graph before termination.
Definition: SPARSdb.h:731
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
unsigned int numPathInsertionFailures_
Track how many solutions fail to have connectivity at end.
Definition: SPARSdb.h:734
double distanceFunction(const Vertex a, const Vertex b) const
Compute distance between two milestones (this is simply distance between the states of the milestones...
Definition: SPARSdb.h:697
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:1346
void checkQueryStateInitialization()
Check that the query vertex is initialized (used for internal nearest neighbor searches) ...
Definition: SPARSdb.cpp:1050
std::vector< Vertex > startVertexCandidateNeighbors_
Used by getSimilarPaths.
Definition: SPARSdb.h:779
std::pair< VertexIndexType, VertexIndexType > VertexPair
Pair of vertices which support an interface.
Definition: SPARSdb.h:107
edgeWeightMap(const Graph &graph, const EdgeCollisionStateMap &collisionStates)
Definition: SPARSdb.cpp:59
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void interpolate(unsigned int count)
Insert a number of states in a path so that the path is made up of exactly count states. States are inserted uniformly (more states on longer segments). Changes are performed only if a path has less than count states.
long unsigned int iterations_
A counter for the number of iterations of the algorithm.
Definition: SPARSdb.h:770
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:1524
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:1835
void setPlannerData(const base::PlannerData &data)
Set the sparse graph from file.
Definition: SPARSdb.cpp:1770
~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...
base::ValidStateSamplerPtr sampler_
Sampler user for generating valid samples in the state space.
Definition: SPARSdb.h:703
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:1501
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:1079
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
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:412
double stretchFactor_
Stretch Factor as per graph spanner literature (multiplicative bound on path quality) ...
Definition: SPARSdb.h:721
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
double sparseDelta_
Maximum visibility range for nodes in the graph.
Definition: SPARSdb.h:773
The exception type for ompl.
Definition: Exception.h:46
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
virtual const State * getState() const
Retrieve the state associated with this vertex.
Definition: PlannerData.h:80
unsigned int consecutiveFailures_
A counter for the number of consecutive failed iterations of the algorithm.
Definition: SPARSdb.h:767
std::vector< Vertex > goalM_
Array of goal milestones.
Definition: SPARSdb.h:715
void approachGraph(Vertex v)
Approaches the graph from a given vertex.
Definition: SPARSdb.cpp:1332
unsigned int getNumEdges() const
Get the number of edges in the sparse roadmap.
Definition: SPARSdb.h:517
EdgeCollisionStateMap edgeCollisionStateProperty_
Access to the collision checking state of each Edge.
Definition: SPARSdb.h:746
std::string name_
The name of this planner.
Definition: Planner.h:415
InterfaceData & getData(Vertex v, Vertex vp, Vertex vpp)
Retrieves the Vertex data associated with v,vp,vpp.
Definition: SPARSdb.cpp:1534
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:1489
std::vector< Vertex > startM_
Array of start milestones.
Definition: SPARSdb.h:712
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:1601
unsigned int getMaxFailures() const
Retrieve the maximum consecutive failure limit.
Definition: SPARSdb.h:416
void restart()
Forget how many states were returned by nextStart() and nextGoal() and return all states again...
Definition: Planner.cpp:170
std::shared_ptr< NearestNeighbors< Vertex > > nn_
Nearest neighbors data structure.
Definition: SPARSdb.h:706
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:1059
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:1584
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:1626
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:1721
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
double denseDelta_
Maximum range for allowing two samples to support an interface.
Definition: SPARSdb.h:776
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
Definition: PlannerData.h:406
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: SPARSdb.cpp: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