Loading...
Searching...
No Matches
SPARSdb.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Rutgers the State University of New Jersey, New Brunswick
5 * Copyright (c) 2014, University of Colorado, Boulder
6 * All Rights Reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Rutgers University nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *********************************************************************/
35
36/* Author: Andrew Dobson, Dave Coleman */
37
38#include <ompl/base/goals/GoalSampleableRegion.h>
39#include <ompl/geometric/planners/prm/ConnectionStrategy.h>
40#include <ompl/tools/config/SelfConfig.h>
41#include <ompl/tools/thunder/SPARSdb.h>
42#include <ompl/util/Console.h>
43#include <boost/foreach.hpp>
44#include <boost/graph/astar_search.hpp>
45#include <boost/graph/incremental_components.hpp>
46#include <boost/property_map/vector_property_map.hpp>
47#include <random>
48
49// Allow hooks for visualizing planner
50//#define OMPL_THUNDER_DEBUG
51
52#define foreach BOOST_FOREACH
53#define foreach_reverse BOOST_REVERSE_FOREACH
54
55// edgeWeightMap methods ////////////////////////////////////////////////////////////////////////////
56
57BOOST_CONCEPT_ASSERT(
58 (boost::ReadablePropertyMapConcept<ompl::geometric::SPARSdb::edgeWeightMap, ompl::geometric::SPARSdb::Edge>));
59
61 : g_(graph), collisionStates_(collisionStates)
62{
63}
64
66{
67 // Get the status of collision checking for this edge
68 if (collisionStates_[e] == IN_COLLISION)
69 return std::numeric_limits<double>::infinity();
70
71 return boost::get(boost::edge_weight, g_, e);
72}
73
74namespace boost
75{
77 {
78 return m.get(e);
79 }
80}
81
82// CustomVisitor methods ////////////////////////////////////////////////////////////////////////////
83
84BOOST_CONCEPT_ASSERT(
85 (boost::AStarVisitorConcept<ompl::geometric::SPARSdb::CustomVisitor, ompl::geometric::SPARSdb::Graph>));
86
90
92{
93 if (u == goal)
94 throw foundGoalException();
95}
96
97// SPARSdb methods ////////////////////////////////////////////////////////////////////////////////////////
98
99ompl::geometric::SPARSdb::SPARSdb(const base::SpaceInformationPtr &si)
100 : base::Planner(si, "SPARSdb")
101 // Numeric variables
102 , nearSamplePoints_((2 * si_->getStateDimension()))
103 // Property accessors of edges
104 , edgeWeightProperty_(boost::get(boost::edge_weight, g_))
106 // Property accessors of vertices
107 , stateProperty_(boost::get(vertex_state_t(), g_))
108 , colorProperty_(boost::get(vertex_color_t(), g_))
110 // Disjoint set accessors
111 , disjointSets_(boost::get(boost::vertex_rank, g_), boost::get(boost::vertex_predecessor, g_))
112{
113 specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
114 specs_.approximateSolutions = false;
115 specs_.optimizingPaths = true;
116
117 psimp_ = std::make_shared<PathSimplifier>(si_);
118
119 Planner::declareParam<double>("stretch_factor", this, &SPARSdb::setStretchFactor, &SPARSdb::getStretchFactor,
120 "1.1:0.1:3.0");
121 Planner::declareParam<double>("sparse_delta_fraction", this, &SPARSdb::setSparseDeltaFraction,
122 &SPARSdb::getSparseDeltaFraction, "0.0:0.01:1.0");
123 Planner::declareParam<double>("dense_delta_fraction", this, &SPARSdb::setDenseDeltaFraction,
124 &SPARSdb::getDenseDeltaFraction, "0.0:0.0001:0.1");
125 Planner::declareParam<unsigned int>("max_failures", this, &SPARSdb::setMaxFailures, &SPARSdb::getMaxFailures,
126 "100:10:3000");
127}
128
133
135{
136 Planner::setup();
137 if (!nn_)
139 nn_->setDistanceFunction([this](const Vertex a, const Vertex b)
140 {
141 return distanceFunction(a, b);
142 });
143 double maxExt = si_->getMaximumExtent();
146
147 if (!sampler_)
148 sampler_ = si_->allocValidStateSampler();
149}
150
151void ompl::geometric::SPARSdb::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
152{
153 Planner::setProblemDefinition(pdef);
154 clearQuery();
155}
156
158{
159 startM_.clear();
160 goalM_.clear();
161 pis_.restart();
162}
163
165{
166 Planner::clear();
167 clearQuery();
169 iterations_ = 0;
170 freeMemory();
171 if (nn_)
172 nn_->clear();
173}
174
176{
177 Planner::clear();
178 sampler_.reset();
179
180 foreach (Vertex v, boost::vertices(g_))
181 {
182 foreach (InterfaceData &d, interfaceDataProperty_[v].interfaceHash | boost::adaptors::map_values)
183 d.clear(si_);
184 if (stateProperty_[v] != nullptr)
185 si_->freeState(stateProperty_[v]);
186 stateProperty_[v] = nullptr;
187 }
188 g_.clear();
189
190 if (nn_)
191 nn_->clear();
192}
193
194bool ompl::geometric::SPARSdb::getSimilarPaths(int /*nearestK*/, const base::State *start, const base::State *goal,
195 CandidateSolution &candidateSolution,
197{
198 // TODO: nearestK unused
199
200 // Get neighbors near start and goal. Note: potentially they are not *visible* - will test for this later
201
202 // Start
203 OMPL_INFORM("Looking for a node near the problem start");
205 {
206 OMPL_INFORM("No graph neighbors found for start within radius %f", sparseDelta_);
207 return false;
208 }
209 if (verbose_)
210 OMPL_INFORM("Found %d nodes near start", startVertexCandidateNeighbors_.size());
211
212 // Goal
213 OMPL_INFORM("Looking for a node near the problem goal");
214 if (!findGraphNeighbors(goal, goalVertexCandidateNeighbors_))
215 {
216 OMPL_INFORM("No graph neighbors found for goal within radius %f", sparseDelta_);
217 return false;
218 }
219 if (verbose_)
220 OMPL_INFORM("Found %d nodes near goal", goalVertexCandidateNeighbors_.size());
221
222 // Get paths between start and goal
223 bool result =
224 getPaths(startVertexCandidateNeighbors_, goalVertexCandidateNeighbors_, start, goal, candidateSolution, ptc);
225
226 // Error check
227 if (!result)
228 {
229 OMPL_INFORM("getSimilarPaths(): SPARSdb returned FALSE for getPaths");
230 return false;
231 }
232 if (!candidateSolution.path_)
233 {
234 OMPL_ERROR("getSimilarPaths(): SPARSdb returned solution is nullptr");
235 return false;
236 }
237
238 // Debug output
239 if (false)
240 {
241 ompl::geometric::PathGeometric geometricSolution =
242 static_cast<ompl::geometric::PathGeometric &>(*candidateSolution.path_);
243
244 for (std::size_t i = 0; i < geometricSolution.getStateCount(); ++i)
245 {
246 OMPL_INFORM(" getSimilarPaths(): Adding state %f to plannerData", i);
247 si_->printState(geometricSolution.getState(i), std::cout);
248 }
249 }
250
251 return result;
252}
253
254bool ompl::geometric::SPARSdb::getPaths(const std::vector<Vertex> &candidateStarts,
255 const std::vector<Vertex> &candidateGoals, const base::State *actualStart,
256 const base::State *actualGoal, CandidateSolution &candidateSolution,
258{
259 // Try every combination of nearby start and goal pairs
260 foreach (Vertex start, candidateStarts)
261 {
262 // Check if this start is visible from the actual start
263 if (!si_->checkMotion(actualStart, stateProperty_[start]))
264 {
265 if (verbose_)
266 OMPL_WARN("FOUND CANDIDATE START THAT IS NOT VISIBLE ");
267 continue; // this is actually not visible
268 }
269
270 foreach (Vertex goal, candidateGoals)
271 {
272 if (verbose_)
273 OMPL_INFORM(" foreach_goal: Checking motion from %d to %d", actualGoal, stateProperty_[goal]);
274
275 // Check if our planner is out of time
276 if (ptc == true)
277 {
278 OMPL_DEBUG("getPaths function interrupted because termination condition is true.");
279 return false;
280 }
281
282 // Check if this goal is visible from the actual goal
283 if (!si_->checkMotion(actualGoal, stateProperty_[goal]))
284 {
285 if (verbose_)
286 OMPL_INFORM("FOUND CANDIDATE GOAL THAT IS NOT VISIBLE! ");
287 continue; // this is actually not visible
288 }
289
290 // Repeatidly search through graph for connection then check for collisions then repeat
291 if (lazyCollisionSearch(start, goal, actualStart, actualGoal, candidateSolution, ptc))
292 {
293 // Found a path
294 return true;
295 }
296 else
297 {
298 // Did not find a path
299 OMPL_INFORM("Did not find a path, looking for other start/goal combinations ");
300 }
301
302 } // foreach
303 } // foreach
304
305 return false;
306}
307
309 const base::State *actualStart, const base::State *actualGoal,
310 CandidateSolution &candidateSolution,
312{
313 base::Goal *g = pdef_->getGoal().get(); // for checking isStartGoalPairValid
314
315 // Vector to store candidate paths in before they are converted to PathPtrs
316 std::vector<Vertex> vertexPath;
317
318 // decide if start and goal are connected
319 // TODO this does not compute dynamic graphcs
320 // i.e. it will say its the same components even when an edge has been disabled
321 bool same_component =
322 true; // sameComponent(start, goal); // TODO is this important? I disabled it during dev and never used it
323
324 // Check if the chosen start and goal can be used together to satisfy problem
325 if (!same_component)
326 {
327 if (verbose_)
328 OMPL_INFORM(" Goal and start are not part of same component, skipping ");
329 return false;
330 }
331
332 // TODO: remove this because start and goal are not either start nor goals
334 {
335 if (verbose_)
336 OMPL_INFORM(" Start and goal pair are not valid combinations, skipping ");
337 return false;
338 }
339
340 // Make sure that the start and goal aren't so close together that they find the same vertex
341 if (start == goal)
342 {
343 if (verbose_)
344 OMPL_INFORM(" Start equals goal, skipping ");
345 return false;
346 }
347
348 // Keep looking for paths between chosen start and goal until one is found that is valid,
349 // or no further paths can be found between them because of disabled edges
350 // this is necessary for lazy collision checking i.e. rerun after marking invalid edges we found
351 bool havePartialSolution = false;
352 while (true)
353 {
354 if (verbose_)
355 OMPL_INFORM(" while true: look for valid paths between start and goal");
356
357 // Check if our planner is out of time
358 if (ptc == true)
359 {
360 OMPL_DEBUG("lazyCollisionSearch: function interrupted because termination condition is true.");
361 return false;
362 }
363
364 // Attempt to find a solution from start to goal
365 if (!constructSolution(start, goal, vertexPath))
366 {
367 // We will stop looking through this start-goal combination, but perhaps this partial solution is good
368 if (verbose_)
369 OMPL_INFORM(" unable to construct solution between start and goal using astar");
370
371 // no solution path found. check if a previous partially correct solution was found
372 if (havePartialSolution && false) // TODO: re-implement partial solution logic
373 {
374 if (verbose_)
375 OMPL_INFORM("has partial solution ");
376 // Save this candidateSolution for later
377 convertVertexPathToStatePath(vertexPath, actualStart, actualGoal, candidateSolution);
378 return false;
379 }
380
381 if (verbose_)
382 OMPL_INFORM(" no partial solution found on this astar search, keep looking through start-goal "
383 "combos");
384
385 // no path found what so ever
386 // return false;
387 return false;
388 }
389 havePartialSolution = true; // we have found at least one path at this point. may be invalid
390
391 if (verbose_)
392 {
393 OMPL_INFORM(" has at least a partial solution, maybe exact solution");
394 OMPL_INFORM(" Solution has %d vertices", vertexPath.size());
395 }
396
397 // Check if all the points in the potential solution are valid
398 if (lazyCollisionCheck(vertexPath, ptc))
399 {
400 if (verbose_)
401 {
402 OMPL_INFORM("---------- lazy collision check returned valid ");
403 }
404
405 // the path is valid, we are done!
406 convertVertexPathToStatePath(vertexPath, actualStart, actualGoal, candidateSolution);
407 return true;
408 }
409 // else, loop with updated graph that has the invalid edges/states disabled
410 } // while
411
412 // we never found a valid path
413 return false;
414}
415
417 std::vector<Vertex> &vertexPath) const
418{
419 auto *vertexPredecessors = new Vertex[boost::num_vertices(g_)];
420 bool foundGoal = false;
421
422 auto *vertexDistances = new double[boost::num_vertices(g_)];
423
424 try
425 {
426 boost::astar_search(g_, // graph
427 start, // start state
428 [this, goal](const Vertex v)
429 {
430 return distanceFunction(v, goal);
431 }, // the heuristic
432 // ability to disable edges (set cost to inifinity):
433 boost::weight_map(edgeWeightMap(g_, edgeCollisionStateProperty_))
434 .predecessor_map(vertexPredecessors)
435 .distance_map(&vertexDistances[0])
436 .visitor(CustomVisitor(goal)));
437 }
439 {
440 // the custom exception from CustomVisitor
441 if (verbose_ && false)
442 {
443 OMPL_INFORM("constructSolution: Astar found goal vertex ------------------------");
444 OMPL_INFORM("distance to goal: %f", vertexDistances[goal]);
445 }
446
447 if (vertexDistances[goal] > 1.7e+308) // terrible hack for detecting infinity
448 // double diff = d[goal] - std::numeric_limits<double>::infinity();
449 // if ((diff < std::numeric_limits<double>::epsilon()) && (-diff < std::numeric_limits<double>::epsilon()))
450 // check if the distance to goal is inifinity. if so, it is unreachable
451 // if (d[goal] >= std::numeric_limits<double>::infinity())
452 {
453 if (verbose_)
454 OMPL_INFORM("Distance to goal is infinity");
455 foundGoal = false;
456 }
457 else
458 {
459 // Only clear the vertexPath after we know we have a new solution, otherwise it might have a good
460 // previous one
461 vertexPath.clear(); // remove any old solutions
462
463 // Trace back a shortest path in reverse and only save the states
464 Vertex v;
465 for (v = goal; v != vertexPredecessors[v]; v = vertexPredecessors[v])
466 {
467 vertexPath.push_back(v);
468 }
469 if (v != goal) // TODO explain this because i don't understand
470 {
471 vertexPath.push_back(v);
472 }
473
474 foundGoal = true;
475 }
476 }
477
478 delete[] vertexPredecessors;
479 delete[] vertexDistances;
480
481 // No solution found from start to goal
482 return foundGoal;
483}
484
485bool ompl::geometric::SPARSdb::lazyCollisionCheck(std::vector<Vertex> &vertexPath,
487{
488 OMPL_DEBUG("Starting lazy collision checking");
489
490 bool hasInvalidEdges = false;
491
492 // Initialize
493 Vertex fromVertex = vertexPath[0];
494 Vertex toVertex;
495
496 // Loop through every pair of states and make sure path is valid.
497 for (std::size_t toID = 1; toID < vertexPath.size(); ++toID)
498 {
499 // Increment location on path
500 toVertex = vertexPath[toID];
501
502 // Check if our planner is out of time
503 if (ptc == true)
504 {
505 OMPL_DEBUG("Lazy collision check function interrupted because termination condition is true.");
506 return false;
507 }
508
509 Edge thisEdge = boost::edge(fromVertex, toVertex, g_).first;
510
511 // Has this edge already been checked before?
512 if (edgeCollisionStateProperty_[thisEdge] == NOT_CHECKED)
513 {
514 // Check path between states
515 if (!si_->checkMotion(stateProperty_[fromVertex], stateProperty_[toVertex]))
516 {
517 // Path between (from, to) states not valid, disable the edge
518 OMPL_INFORM(" DISABLING EDGE from vertex %f to vertex %f", fromVertex, toVertex);
519
520 // Disable edge
521 edgeCollisionStateProperty_[thisEdge] = IN_COLLISION;
522 }
523 else
524 {
525 // Mark edge as free so we no longer need to check for collision
526 edgeCollisionStateProperty_[thisEdge] = FREE;
527 }
528 }
529
530 // Check final result
531 if (edgeCollisionStateProperty_[thisEdge] == IN_COLLISION)
532 {
533 // Remember that this path is no longer valid, but keep checking remainder of path edges
534 hasInvalidEdges = true;
535 }
536
537 // switch vertex focus
538 fromVertex = toVertex;
539 }
540
541 OMPL_INFORM("Done lazy collision checking");
542
543 // TODO: somewhere in the code we need to reset all edges collision status back to NOT_CHECKED for future queries
544
545 // Only return true if nothing was found invalid
546 return !hasInvalidEdges;
547}
548
550{
551 return boost::same_component(m1, m2, disjointSets_);
552}
553
558
559void ompl::geometric::SPARSdb::printDebug(std::ostream &out) const
560{
561 out << "SPARSdb Debug Output: " << std::endl;
562 out << " Settings: " << std::endl;
563 out << " Max Failures: " << getMaxFailures() << std::endl;
564 out << " Dense Delta Fraction: " << getDenseDeltaFraction() << std::endl;
565 out << " Sparse Delta Fraction: " << getSparseDeltaFraction() << std::endl;
566 out << " Sparse Delta: " << sparseDelta_ << std::endl;
567 out << " Stretch Factor: " << getStretchFactor() << std::endl;
568 out << " Maximum Extent: " << si_->getMaximumExtent() << std::endl;
569 out << " Status: " << std::endl;
570 out << " Vertices Count: " << getNumVertices() << std::endl;
571 out << " Edges Count: " << getNumEdges() << std::endl;
572 out << " Iterations: " << getIterations() << std::endl;
573 out << " Consecutive Failures: " << consecutiveFailures_ << std::endl;
574 out << " Number of guards: " << nn_->size() << std::endl << std::endl;
575}
576
577bool 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
641bool ompl::geometric::SPARSdb::addPathToRoadmap(const base::PlannerTerminationCondition &ptc,
642 ompl::geometric::PathGeometric &solutionPath)
643{
644 // Check that the query vertex is initialized (used for internal nearest neighbor searches)
645 checkQueryStateInitialization();
646
647 // Error check
648 if (solutionPath.getStateCount() < 2)
649 {
650 OMPL_ERROR("Less than 2 states were passed to addPathToRoadmap in the solution path");
651 return false;
652 }
653
654 // Find spacing factor - 2.0 would be a perfect amount, but we leave room for rounding/interpolation errors and
655 // curves in path
656 int numGuards; // unused variable that indicates how many guards we will add
657 double spacingFactor;
658 if (!getGuardSpacingFactor(solutionPath.length(), numGuards, spacingFactor))
659 return false;
660
661 OMPL_DEBUG("Expected number of necessary coverage guards is calculated to be %i from the original path state count "
662 "%i",
663 numGuards, solutionPath.getStateCount());
664
665 unsigned int n = 0;
666 const int n1 = solutionPath.getStateCount() - 1;
667 for (int i = 0; i < n1; ++i)
668 n += si_->getStateSpace()->validSegmentCount(solutionPath.getState(i), solutionPath.getState(i + 1));
669
670 solutionPath.interpolate(n);
671
672 // Debug
673 if (verbose_)
674 {
675 OMPL_INFORM("-------------------------------------------------------");
676 OMPL_INFORM("Attempting to add %d states to roadmap", solutionPath.getStateCount());
677 OMPL_INFORM("-------------------------------------------------------");
678 }
679
680 // Try to add the start first, but don't force it
681 addStateToRoadmap(ptc, solutionPath.getState(0));
682
683#ifdef OMPL_THUNDER_DEBUG
684 visualizeStateCallback(solutionPath.getState(solutionPath.getStateCount() - 1), 3, sparseDelta_);
685#endif
686
687 // Add solution states to SPARSdb one by one ---------------------------
688
689 // Track which nodes we've already tried to add
690 std::vector<std::size_t> addedStateIDs;
691 // Track which nodes we will attempt to use as connectivity states
692 std::vector<std::size_t> connectivityStateIDs;
693 // std::vector<base::State*> connectivityStates;
694
695 double distanceFromLastState = 0;
696
697 std::size_t lastStateID = 0; // track the id in the solutionPath of the last state
698
699 for (std::size_t i = 1; i < solutionPath.getStateCount();
700 ++i) // skip 0 and last because those are start/goal and are already added
701 {
702 distanceFromLastState = si_->distance(solutionPath.getState(i), solutionPath.getState(lastStateID));
703
704 if (verbose_ && false)
705 {
706 OMPL_INFORM("Index %d at distance %f from last state ", i, distanceFromLastState);
707 }
708
709 if (distanceFromLastState >= sparseDelta_ * spacingFactor)
710 {
711 if (verbose_)
712 {
713 OMPL_INFORM("Adding state %d of %d", i, solutionPath.getStateCount());
714 }
715
716// Show the candidate state in Rviz for path insertion of GUARDS
717#ifdef OMPL_THUNDER_DEBUG
718 visualizeStateCallback(solutionPath.getState(i), 1, sparseDelta_);
719#endif
720
721 // Add a single state to the roadmap
722 if (!addStateToRoadmap(ptc, solutionPath.getState(i)))
723 {
724 if (verbose_)
725 {
726 OMPL_INFORM("Last state added to roadmap failed ");
727 }
728 }
729
730 // Now figure out midpoint state between lastState and i
731 std::size_t midStateID = (i - lastStateID) / 2 + lastStateID;
732 connectivityStateIDs.push_back(midStateID);
733
734 double distA = si_->distance(solutionPath.getState(lastStateID), solutionPath.getState(midStateID));
735 double distB = si_->distance(solutionPath.getState(i), solutionPath.getState(midStateID));
736 double diff = distA - distB;
737 if ((diff < std::numeric_limits<double>::epsilon()) && (-diff < std::numeric_limits<double>::epsilon()))
738 if (verbose_)
739 OMPL_WARN("DISTANCES ARE DIFFERENT ");
740
741 // Save this state as the new last state
742 lastStateID = i;
743 // Remember which nodes we've already added / attempted to add
744 addedStateIDs.push_back(midStateID);
745 addedStateIDs.push_back(i);
746 }
747 // Close up if it doesn't do it automatically
748 else if (i == solutionPath.getStateCount() - 1)
749 {
750 if (verbose_)
751 OMPL_INFORM("Last state - do special midpoint");
752
753 // Now figure out midpoint state between lastState and i
754 std::size_t midStateID = (i - lastStateID) / 2 + lastStateID;
755 connectivityStateIDs.push_back(midStateID);
756 addedStateIDs.push_back(midStateID);
757 if (verbose_)
758 OMPL_INFORM("Mid state is %d", midStateID);
759 }
760 }
761
762 // Attempt to add the goal directly
763 addStateToRoadmap(ptc, solutionPath.getState(solutionPath.getStateCount() - 1));
764
765 if (verbose_)
766 {
767 OMPL_INFORM("-------------------------------------------------------");
768 OMPL_INFORM("-------------------------------------------------------");
769 OMPL_INFORM("Adding connectivity states ----------------------------");
770 OMPL_INFORM("-------------------------------------------------------");
771 OMPL_INFORM("-------------------------------------------------------");
772 }
773
774 for (std::size_t i = 0; i < connectivityStateIDs.size(); ++i)
775 {
776 base::State *connectivityState = solutionPath.getState(connectivityStateIDs[i]);
777
778 if (verbose_)
779 {
780 OMPL_INFORM("Adding connectvity state ", i);
781 }
782
783#ifdef OMPL_THUNDER_DEBUG
784 // Show the candidate state in Rviz for path insertion of BRIDGES (CONNECTIVITY)
785 visualizeStateCallback(connectivityState, 2, sparseDelta_);
786 sleep(0.5);
787#endif
788
789 // Add a single state to the roadmap
790 addStateToRoadmap(ptc, connectivityState);
791 }
792
793 // Add remaining states at random
794 if (verbose_)
795 {
796 OMPL_INFORM("-------------------------------------------------------");
797 OMPL_INFORM("-------------------------------------------------------");
798 OMPL_INFORM("Adding remaining states randomly ----------------------");
799 OMPL_INFORM("-------------------------------------------------------");
800 OMPL_INFORM("-------------------------------------------------------");
801 }
802
803 // Create a vector of shuffled indexes
804 std::vector<std::size_t> shuffledIDs;
805 std::size_t usedIDTracker = 0;
806 for (std::size_t i = 1; i < solutionPath.getStateCount(); ++i) // skip 0 because start already added
807 {
808 // Check if we've already used this id
809 if (usedIDTracker < addedStateIDs.size() && i == addedStateIDs[usedIDTracker])
810 {
811 // skip this id
812 usedIDTracker++;
813 continue;
814 }
815
816 shuffledIDs.push_back(i); // 1 2 3...
817 }
818
819 std::shuffle(shuffledIDs.begin(), shuffledIDs.end(), std::mt19937(std::random_device()()));
820
821 // Add each state randomly
822 for (unsigned long shuffledID : shuffledIDs)
823 {
824#ifdef OMPL_THUNDER_DEBUG
825 visualizeStateCallback(solutionPath.getState(shuffledIDs[i]), 1, sparseDelta_);
826#endif
827
828 // Add a single state to the roadmap
829 addStateToRoadmap(ptc, solutionPath.getState(shuffledID));
830 }
831
832 bool benchmarkLogging = true;
833 if (benchmarkLogging)
834 {
835 OMPL_DEBUG("ompl::geometric::SPARSdb: Benchmark logging enabled (slower)");
836
837 // Return the result of inserting into database, if applicable
838 return checkStartGoalConnection(solutionPath);
839 }
840
841 return true;
842}
843
844bool ompl::geometric::SPARSdb::checkStartGoalConnection(ompl::geometric::PathGeometric &solutionPath)
845{
846 // Make sure path has states
847 if (solutionPath.getStateCount() < 2)
848 {
849 OMPL_ERROR("Not enough states (< 2) in the solutionPath");
850 return false;
851 }
852
853 bool error = false;
854 CandidateSolution candidateSolution;
855 do
856 {
857 base::State *actualStart = solutionPath.getState(0);
858 base::State *actualGoal = solutionPath.getState(solutionPath.getStateCount() - 1);
859
860 /* The whole neighborhood set which has been most recently computed */
861 std::vector<Vertex> graphNeighborhood;
862 /* The visible neighborhood set which has been most recently computed */
863 std::vector<Vertex> visibleNeighborhood;
864
865 // Get start vertex
866 findGraphNeighbors(actualStart, graphNeighborhood, visibleNeighborhood);
867 if (!visibleNeighborhood.size())
868 {
869 OMPL_ERROR("No vertexes found near start");
870 error = true;
871 break;
872 }
873 Vertex closeStart = visibleNeighborhood[0];
874
875 // Get goal vertex
876 findGraphNeighbors(actualGoal, graphNeighborhood, visibleNeighborhood);
877 if (!visibleNeighborhood.size())
878 {
879 OMPL_ERROR("No vertexes found near goal");
880 error = true;
881 break;
882 }
883 Vertex closeGoal = visibleNeighborhood[0];
884
885 // Check if connected
886 if (false)
887 if (!sameComponent(closeStart, closeGoal))
888 {
889 OMPL_ERROR("Start and goal are not connected!");
890 error = true;
891 break;
892 }
893
894 // Get new path from start to goal
895 std::vector<Vertex> vertexPath;
896 if (!constructSolution(closeStart, closeGoal, vertexPath))
897 {
898 OMPL_ERROR("Unable to find path from start to goal - perhaps because of new obstacles");
899 error = true;
900 break;
901 }
902
903 // Convert to PathGeometric
904 bool disableCollisionWarning = true; // this is just for benchmarking purposes
905 if (!convertVertexPathToStatePath(vertexPath, actualStart, actualGoal, candidateSolution,
906 disableCollisionWarning))
907 {
908 OMPL_ERROR("Unable to convert to state path");
909 error = true;
910 break;
911 }
912 } while (false);
913
914 // Check distance of new path from old path
915 double originalLength = solutionPath.length();
916
917 OMPL_DEBUG("Results of attempting to make insertion in SPARSdb ");
918 OMPL_DEBUG("-------------------------------------------------------");
919 OMPL_DEBUG("Original length: %f", originalLength);
920
921 if (error)
922 {
923 OMPL_ERROR("UNABLE TO GET PATH");
924
925 // Record this for plotting
926 numPathInsertionFailures_++;
927 }
928 else
929 {
930 double newLength = candidateSolution.getGeometricPath().length();
931 double percentIncrease = 100 - originalLength / newLength * 100;
932 OMPL_DEBUG("New length: %f", newLength);
933 OMPL_DEBUG("Percent increase: %f %%", percentIncrease);
934 }
935
936 return !error; // return true if it inserted correctly
937}
938
939bool ompl::geometric::SPARSdb::addStateToRoadmap(const base::PlannerTerminationCondition &ptc, base::State *newState)
940{
941 bool stateAdded = false;
942 // Check that the query vertex is initialized (used for internal nearest neighbor searches)
943 checkQueryStateInitialization();
944
945 // Deep copy
946 base::State *qNew = si_->cloneState(newState);
947 base::State *workState = si_->allocState();
948
949 /* The whole neighborhood set which has been most recently computed */
950 std::vector<Vertex> graphNeighborhood;
951 /* The visible neighborhood set which has been most recently computed */
952 std::vector<Vertex> visibleNeighborhood;
953
954 ++iterations_;
955
956 findGraphNeighbors(qNew, graphNeighborhood, visibleNeighborhood);
957
958 if (verbose_)
959 {
960 OMPL_INFORM(" graph neighborhood: %d | visible neighborhood: %d", graphNeighborhood.size(),
961 visibleNeighborhood.size());
962
963 foreach (Vertex v, visibleNeighborhood)
964 {
965 OMPL_INFORM("Visible neighbor is vertex %f with distance %f ", v, si_->distance(qNew, stateProperty_[v]));
966 }
967 }
968
969 if (verbose_)
970 OMPL_INFORM(" - checkAddCoverage() Are other nodes around it visible?");
971 // Coverage criterion
972 if (!checkAddCoverage(qNew,
973 visibleNeighborhood)) // Always add a node if no other nodes around it are visible (GUARD)
974 {
975 if (verbose_)
976 OMPL_INFORM(" -- checkAddConnectivity() Does this node connect neighboring nodes that are not connected? ");
977 // Connectivity criterion
978 if (!checkAddConnectivity(qNew, visibleNeighborhood))
979 {
980 if (verbose_)
981 OMPL_INFORM(" --- checkAddInterface() Does this node's neighbor's need it to better connect them? ");
982 if (!checkAddInterface(qNew, graphNeighborhood, visibleNeighborhood))
983 {
984 if (verbose_)
985 OMPL_INFORM(" ---- Ensure SPARS asymptotic optimality");
986 if (visibleNeighborhood.size() > 0)
987 {
988 std::map<Vertex, base::State *> closeRepresentatives;
989 if (verbose_)
990 OMPL_INFORM(" ----- findCloseRepresentatives()");
991
992 findCloseRepresentatives(workState, qNew, visibleNeighborhood[0], closeRepresentatives, ptc);
993 if (verbose_)
994 OMPL_INFORM("------ Found %d close representatives", closeRepresentatives.size());
995
996 for (auto &closeRepresentative : closeRepresentatives)
997 {
998 if (verbose_)
999 OMPL_INFORM(" ------ Looping through close representatives");
1000 updatePairPoints(visibleNeighborhood[0], qNew, closeRepresentative.first,
1001 closeRepresentative.second);
1002 updatePairPoints(closeRepresentative.first, closeRepresentative.second, visibleNeighborhood[0],
1003 qNew);
1004 }
1005 if (verbose_)
1006 OMPL_INFORM(" ------ checkAddPath()");
1007 if (checkAddPath(visibleNeighborhood[0]))
1008 {
1009 if (verbose_)
1010 {
1011 OMPL_INFORM("nearest visible neighbor added ");
1012 }
1013 }
1014
1015 for (auto &closeRepresentative : closeRepresentatives)
1016 {
1017 if (verbose_)
1018 OMPL_INFORM(" ------- Looping through close representatives to add path");
1019 checkAddPath(closeRepresentative.first);
1020 si_->freeState(closeRepresentative.second);
1021 }
1022 if (verbose_)
1023 OMPL_INFORM("------ Done with inner most loop ");
1024 }
1025 }
1026 else // added for interface
1027 {
1028 stateAdded = true;
1029 }
1030 }
1031 else // added for connectivity
1032 {
1033 stateAdded = true;
1034 }
1035 }
1036 else // added for coverage
1037 {
1038 stateAdded = true;
1039 }
1040
1041 if (!stateAdded)
1042 ++consecutiveFailures_;
1043
1044 si_->freeState(workState);
1045 si_->freeState(qNew);
1046
1047 return stateAdded;
1048}
1049
1051{
1052 if (boost::num_vertices(g_) < 1)
1053 {
1054 queryVertex_ = boost::add_vertex(g_);
1055 stateProperty_[queryVertex_] = nullptr;
1056 }
1057}
1058
1064
1065bool 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
1079bool 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
1130bool 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
1276
1277void 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
1293bool 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
1501void 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
1510void 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
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
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}
The exception type for ompl.
Definition Exception.h:47
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
Abstract definition of goals.
Definition Goal.h:63
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
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition PlannerData.h:59
virtual const State * getState() const
Retrieve the state associated with this vertex.
Definition PlannerData.h:80
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
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...
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...
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
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)....
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist,...
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...
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerInputStates pis_
Utility class to extract valid input states.
Definition Planner.h:416
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition Planner.h:422
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition Planner.h:413
std::string name_
The name of this planner.
Definition Planner.h:419
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
Definition of an abstract state.
Definition State.h:50
Definition of a geometric path.
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
void interpolate(unsigned int count)
Insert a number of states in a path so that the path is made up of exactly count states....
double length() const override
Compute the length of a geometric path (sum of lengths of segments that make up the path)
base::State * getState(unsigned int index)
Get the state located at index along the path.
void examine_vertex(Vertex u, const Graph &g) const
Definition SPARSdb.cpp:91
edgeWeightMap(const Graph &graph, const EdgeCollisionStateMap &collisionStates)
Definition SPARSdb.cpp:60
Vertex queryVertex_
Vertex for performing nearest neighbor queries.
Definition SPARSdb.h:717
double getStretchFactor() const
Retrieve the spanner's set stretch factor.
Definition SPARSdb.h:433
bool convertVertexPathToStatePath(std::vector< Vertex > &vertexPath, const base::State *actualStart, const base::State *actualGoal, CandidateSolution &candidateSolution, bool disableCollisionWarning=false)
Convert astar results to correctly ordered path.
Definition SPARSdb.cpp:1654
bool 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
std::vector< Vertex > goalM_
Array of goal milestones.
Definition SPARSdb.h:714
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition SPARSdb.cpp:1721
unsigned int maxFailures_
The number of consecutive failures to add to the graph before termination.
Definition SPARSdb.h:730
double sparseDeltaFraction_
Maximum visibility range for nodes in the graph as a fraction of maximum extent.
Definition SPARSdb.h:723
bool getSimilarPaths(int nearestK, const base::State *start, const base::State *goal, CandidateSolution &candidateSolution, const base::PlannerTerminationCondition &ptc)
Search the roadmap for the best path close to the given start and goal states that is valid.
Definition SPARSdb.cpp:194
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
void setStretchFactor(double t)
Sets the stretch factor.
Definition SPARSdb.h:387
std::vector< Vertex > startVertexCandidateNeighbors_
Used by getSimilarPaths.
Definition SPARSdb.h:778
double getSparseDeltaFraction() const
Retrieve the sparse graph visibility range delta.
Definition SPARSdb.h:427
boost::graph_traits< Graph >::edge_descriptor Edge
Edge in Graph.
Definition SPARSdb.h:297
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
boost::graph_traits< Graph >::vertex_descriptor Vertex
Vertex in Graph.
Definition SPARSdb.h:294
double sparseDelta_
Maximum visibility range for nodes in the graph.
Definition SPARSdb.h:772
unsigned int getNumEdges() const
Get the number of edges in the sparse roadmap.
Definition SPARSdb.h:516
unsigned int consecutiveFailures_
A counter for the number of consecutive failed iterations of the algorithm.
Definition SPARSdb.h:766
void clearQuery() override
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition SPARSdb.cpp:157
~SPARSdb() override
Destructor.
Definition SPARSdb.cpp:129
VertexPair index(Vertex vp, Vertex vpp)
Rectifies indexing order for accessing the vertex data.
Definition SPARSdb.cpp:1524
bool constructSolution(Vertex start, Vertex goal, std::vector< Vertex > &vertexPath) const
Given two milestones from the same connected component, construct a path connecting them and set it a...
Definition SPARSdb.cpp:416
bool lazyCollisionSearch(const Vertex &start, const Vertex &goal, const base::State *actualStart, const base::State *actualGoal, CandidateSolution &candidateSolution, const base::PlannerTerminationCondition &ptc)
Repeatidly search through graph for connection then check for collisions then repeat.
Definition SPARSdb.cpp:308
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
void resetFailures()
A reset function for resetting the failures count.
Definition SPARSdb.cpp:1272
base::ValidStateSamplerPtr sampler_
Sampler user for generating valid samples in the state space.
Definition SPARSdb.h:702
void setSparseDeltaFraction(double D)
Sets vertex visibility range as a fraction of max. extent.
Definition SPARSdb.h:393
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:303
bool reachedFailureLimit() const
Returns whether we have reached the iteration failures limit, maxFailures_.
Definition SPARSdb.cpp:554
std::vector< Vertex > startM_
Array of start milestones.
Definition SPARSdb.h:711
bool checkAddPath(Vertex v)
Checks vertex v for short paths through its region and adds when appropriate.
Definition SPARSdb.cpp:1173
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:758
boost::property_map< Graph, vertex_interface_data_t >::type interfaceDataProperty_
Access to the interface pair information for the vertices.
Definition SPARSdb.h:754
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
boost::property_map< Graph, boost::edge_weight_t >::type edgeWeightProperty_
Access to the weights of each Edge.
Definition SPARSdb.h:742
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:696
EdgeCollisionStateMap edgeCollisionStateProperty_
Access to the collision checking state of each Edge.
Definition SPARSdb.h:745
long unsigned int getIterations() const
Get the number of iterations the algorithm performed.
Definition SPARSdb.h:549
double denseDeltaFraction_
Maximum range for allowing two samples to support an interface as a fraction of maximum extent.
Definition SPARSdb.h:727
boost::property_map< Graph, vertex_color_t >::type colorProperty_
Access to the colors for the vertices.
Definition SPARSdb.h:751
void computeX(Vertex v, Vertex vp, Vertex vpp, std::vector< Vertex > &Xs)
Computes all nodes which qualify as a candidate x for v, v', and v".
Definition SPARSdb.cpp:1510
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition SPARSdb.cpp:134
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, VertexProperties, EdgeProperties > Graph
Definition SPARSdb.h:289
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition SPARSdb.cpp:164
InterfaceData & getData(Vertex v, Vertex vp, Vertex vpp)
Retrieves the Vertex data associated with v,vp,vpp.
Definition SPARSdb.cpp:1534
void printDebug(std::ostream &out=std::cout) const
Print debug information about planner.
Definition SPARSdb.cpp:559
void checkQueryStateInitialization()
Check that the query vertex is initialized (used for internal nearest neighbor searches)
Definition SPARSdb.cpp:1050
PathSimplifierPtr psimp_
A path simplifier used to simplify dense paths added to the graph.
Definition SPARSdb.h:739
void setMaxFailures(unsigned int m)
Sets the maximum failures until termination.
Definition SPARSdb.h:409
void approachGraph(Vertex v)
Approaches the graph from a given vertex.
Definition SPARSdb.cpp:1332
void setDenseDeltaFraction(double d)
Sets interface support tolerance as a fraction of max. extent.
Definition SPARSdb.h:401
bool lazyCollisionCheck(std::vector< Vertex > &vertexPath, const base::PlannerTerminationCondition &ptc)
Check recalled path for collision and disable as needed.
Definition SPARSdb.cpp:485
double stretchFactor_
Stretch Factor as per graph spanner literature (multiplicative bound on path quality)
Definition SPARSdb.h:720
void findGraphNeighbors(base::State *state, std::vector< Vertex > &graphNeighborhood, std::vector< Vertex > &visibleNeighborhood)
Finds visible nodes in the graph near state.
Definition SPARSdb.cpp:1277
double denseDelta_
Maximum range for allowing two samples to support an interface.
Definition SPARSdb.h:775
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
Graph g_
Connectivity graph.
Definition SPARSdb.h:708
unsigned int getMaxFailures() const
Retrieve the maximum consecutive failure limit.
Definition SPARSdb.h:415
std::shared_ptr< NearestNeighbors< Vertex > > nn_
Nearest neighbors data structure.
Definition SPARSdb.h:705
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
void freeMemory()
Free all the memory allocated by the planner.
Definition SPARSdb.cpp:175
unsigned int nearSamplePoints_
Number of sample points to use when trying to detect interfaces.
Definition SPARSdb.h:736
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
Vertex findGraphRepresentative(base::State *st)
Finds the representative of the input state, st.
Definition SPARSdb.cpp:1346
bool sameComponent(Vertex m1, Vertex m2)
Check if two milestones (m1 and m2) are part of the same connected component. This is not a const fun...
Definition SPARSdb.cpp:549
std::pair< VertexIndexType, VertexIndexType > VertexPair
Pair of vertices which support an interface.
Definition SPARSdb.h:107
void computeVPP(Vertex v, Vertex vp, std::vector< Vertex > &VPPs)
Computes all nodes which qualify as a candidate v" for v and vp.
Definition SPARSdb.cpp:1501
void updatePairPoints(Vertex rep, const base::State *q, Vertex r, const base::State *s)
High-level method which updates pair point information for repV_ with neighbor r.
Definition SPARSdb.cpp:1489
void clearEdgeCollisionStates()
Clear all past edge state information about in collision or not.
Definition SPARSdb.cpp:1835
double getDenseDeltaFraction() const
Retrieve the dense graph interface support delta.
Definition SPARSdb.h:421
long unsigned int iterations_
A counter for the number of iterations of the algorithm.
Definition SPARSdb.h:769
bool getPaths(const std::vector< Vertex > &candidateStarts, const std::vector< Vertex > &candidateGoals, const base::State *actualStart, const base::State *actualGoal, CandidateSolution &candidateSolution, const base::PlannerTerminationCondition &ptc)
Check if there exists a solution, i.e., there exists a pair of milestones such that the first is in s...
Definition SPARSdb.cpp:254
unsigned int getNumVertices() const
Get the number of vertices in the sparse roadmap.
Definition SPARSdb.h:510
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition SPARSdb.h:748
bool verbose_
Option to enable debugging output.
Definition SPARSdb.h:782
void setPlannerData(const base::PlannerData &data)
Set the sparse graph from file.
Definition SPARSdb.cpp:1770
GuardType
Enumeration which specifies the reason a guard is added to the spanner.
Definition SPARSdb.h:90
SPARSdb(const base::SpaceInformationPtr &si)
Constructor.
Definition SPARSdb.cpp:99
void connectGuards(Vertex v, Vertex vp)
Connect two guards in the roadmap.
Definition SPARSdb.cpp:1626
static NearestNeighbors< _T > * getDefaultNearestNeighbors(const base::Planner *planner)
Select a default nearest neighbor datastructure for the given space.
Definition SelfConfig.h:106
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition Console.h:70
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition Console.h:66
This namespace contains sampling based planning routines shared by both planning under geometric cons...
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition GoalTypes.h:56
A class to store the exit status of Planner::solve()
@ TIMEOUT
The planner failed to find a solution.
Struct for passing around partially solved solutions.
Definition SPARSdb.h:233
Interface information storage class, which does bookkeeping for criterion four.
Definition SPARSdb.h:112
double d_
Last known distance between the two interfaces supported by points_ and sigmas.
Definition SPARSdb.h:122
void clear(const base::SpaceInformationPtr &si)
Clears the given interface data.
Definition SPARSdb.h:128
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
base::State * pointA_
States which lie inside the visibility region of a vertex and support an interface.
Definition SPARSdb.h:114
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
base::State * sigmaA_
States which lie just outside the visibility region of a vertex and support an interface.
Definition SPARSdb.h:118