Loading...
Searching...
No Matches
SPARS.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2013, Rutgers the State University of New Jersey, New Brunswick
5* All Rights Reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of Rutgers University nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Author: Andrew Dobson */
36
37#include "ompl/geometric/planners/prm/SPARS.h"
38#include "ompl/geometric/planners/prm/ConnectionStrategy.h"
39#include "ompl/base/goals/GoalSampleableRegion.h"
40#include "ompl/base/objectives/PathLengthOptimizationObjective.h"
41#include "ompl/tools/config/SelfConfig.h"
42#include "ompl/tools/config/MagicConstants.h"
43#include <thread>
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 <boost/foreach.hpp>
48
49#include "GoalVisitor.hpp"
50
51#define foreach BOOST_FOREACH
52#define foreach_reverse BOOST_REVERSE_FOREACH
53
54ompl::geometric::SPARS::SPARS(const base::SpaceInformationPtr &si)
55 : base::Planner(si, "SPARS")
56 , geomPath_(si)
57 , stateProperty_(boost::get(vertex_state_t(), g_))
58 , sparseStateProperty_(boost::get(vertex_state_t(), s_))
59 , sparseColorProperty_(boost::get(vertex_color_t(), s_))
60 , representativesProperty_(boost::get(vertex_representative_t(), g_))
61 , nonInterfaceListsProperty_(boost::get(vertex_list_t(), s_))
62 , interfaceListsProperty_(boost::get(vertex_interface_list_t(), s_))
63 , weightProperty_(boost::get(boost::edge_weight, g_))
64 , sparseDJSets_(boost::get(boost::vertex_rank, s_), boost::get(boost::vertex_predecessor, s_))
65{
69 specs_.multithreaded = true;
70
71 psimp_ = std::make_shared<PathSimplifier>(si_);
72 psimp_->freeStates(false);
73
74 Planner::declareParam<double>("stretch_factor", this, &SPARS::setStretchFactor, &SPARS::getStretchFactor, "1.1:0.1:"
75 "3.0");
76 Planner::declareParam<double>("sparse_delta_fraction", this, &SPARS::setSparseDeltaFraction,
77 &SPARS::getSparseDeltaFraction, "0.0:0.01:1.0");
78 Planner::declareParam<double>("dense_delta_fraction", this, &SPARS::setDenseDeltaFraction,
79 &SPARS::getDenseDeltaFraction, "0.0:0.0001:0.1");
80 Planner::declareParam<unsigned int>("max_failures", this, &SPARS::setMaxFailures, &SPARS::getMaxFailures, "100:10:"
81 "3000");
82
83 addPlannerProgressProperty("iterations INTEGER", [this]
84 {
85 return getIterationCount();
86 });
87 addPlannerProgressProperty("best cost REAL", [this]
88 {
89 return getBestCost();
90 });
91}
92
94{
95 freeMemory();
96}
97
99{
100 Planner::setup();
101 if (!nn_)
102 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<DenseVertex>(this));
103 nn_->setDistanceFunction([this](const DenseVertex a, const DenseVertex b)
104 {
105 return distanceFunction(a, b);
106 });
107 if (!snn_)
108 snn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<SparseVertex>(this));
109 snn_->setDistanceFunction([this](const SparseVertex a, const SparseVertex b)
110 {
111 return sparseDistanceFunction(a, b);
112 });
113 if (!connectionStrategy_)
114 connectionStrategy_ = KStarStrategy<DenseVertex>(
115 [this]
116 {
117 return milestoneCount();
118 },
119 nn_, si_->getStateDimension());
120 double maxExt = si_->getMaximumExtent();
121 sparseDelta_ = sparseDeltaFraction_ * maxExt;
122 denseDelta_ = denseDeltaFraction_ * maxExt;
123
124 // Setup optimization objective
125 //
126 // If no optimization objective was specified, then default to
127 // optimizing path length as computed by the distance() function
128 // in the state space.
129 if (pdef_)
130 {
131 if (pdef_->hasOptimizationObjective())
132 {
133 opt_ = pdef_->getOptimizationObjective();
134 if (dynamic_cast<base::PathLengthOptimizationObjective *>(opt_.get()) == nullptr)
135 OMPL_WARN("%s: Asymptotic optimality has only been proven with path length optimizaton; convergence "
136 "for other optimizaton objectives is not guaranteed.",
137 getName().c_str());
138 }
139 else
140 opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
141 }
142 else
143 {
144 OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
145 setup_ = false;
146 }
147}
148
149void ompl::geometric::SPARS::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
150{
151 Planner::setProblemDefinition(pdef);
152 clearQuery();
153}
154
156{
157 consecutiveFailures_ = 0;
158}
159
161{
162 startM_.clear();
163 goalM_.clear();
164 pis_.restart();
165
166 // Clear past solutions if there are any
167 if (pdef_)
168 pdef_->clearSolutionPaths();
169}
170
172{
173 Planner::clear();
174 sampler_.reset();
175 freeMemory();
176 if (nn_)
177 nn_->clear();
178 if (snn_)
179 snn_->clear();
180 clearQuery();
181 resetFailures();
182 iterations_ = 0;
183 bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
184}
185
187{
188 foreach (DenseVertex v, boost::vertices(g_))
189 if (stateProperty_[v] != nullptr)
190 {
191 si_->freeState(stateProperty_[v]);
192 stateProperty_[v] = nullptr;
193 }
194 foreach (SparseVertex n, boost::vertices(s_))
195 if (sparseStateProperty_[n] != nullptr)
196 {
197 si_->freeState(sparseStateProperty_[n]);
198 sparseStateProperty_[n] = nullptr;
199 }
200 s_.clear();
201 g_.clear();
202}
203
206{
207 DenseVertex result = boost::graph_traits<DenseGraph>::null_vertex();
208
209 // search for a valid state
210 bool found = false;
211 while (!found && !ptc)
212 {
213 unsigned int attempts = 0;
214 do
215 {
216 found = sampler_->sample(workState);
217 attempts++;
219 }
220
221 if (found)
222 result = addMilestone(si_->cloneState(workState));
223 return result;
224}
225
227{
228 auto *goal = static_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
229 while (!ptc && !addedSolution_)
230 {
231 // Check for any new goal states
232 if (goal->maxSampleCount() > goalM_.size())
233 {
234 const base::State *st = pis_.nextGoal();
235 if (st != nullptr)
236 {
237 addMilestone(si_->cloneState(st));
238 goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
239 }
240 }
241
242 // Check for a solution
243 addedSolution_ = haveSolution(startM_, goalM_, solution);
244 // Sleep for 1ms
245 if (!addedSolution_)
246 std::this_thread::sleep_for(std::chrono::milliseconds(1));
247 }
248}
249
250bool ompl::geometric::SPARS::haveSolution(const std::vector<DenseVertex> &starts, const std::vector<DenseVertex> &goals,
251 base::PathPtr &solution)
252{
253 base::Goal *g = pdef_->getGoal().get();
254 base::Cost sol_cost(opt_->infiniteCost());
255 foreach (DenseVertex start, starts)
256 {
257 foreach (DenseVertex goal, goals)
258 {
259 // we lock because the connected components algorithm is incremental and may change disjointSets_
260 graphMutex_.lock();
261 bool same_component = sameComponent(start, goal);
262 graphMutex_.unlock();
263
264 if (same_component && g->isStartGoalPairValid(sparseStateProperty_[goal], sparseStateProperty_[start]))
265 {
266 base::PathPtr p = constructSolution(start, goal);
267 if (p)
268 {
269 base::Cost pathCost = p->cost(opt_);
270 if (opt_->isCostBetterThan(pathCost, bestCost_))
271 bestCost_ = pathCost;
272 // Check if optimization objective is satisfied
273 if (opt_->isSatisfied(pathCost))
274 {
275 solution = p;
276 return true;
277 }
278 if (opt_->isCostBetterThan(pathCost, sol_cost))
279 {
280 solution = p;
281 sol_cost = pathCost;
282 }
283 }
284 }
285 }
286 }
287
288 return false;
289}
290
292{
293 return consecutiveFailures_ >= maxFailures_ || addedSolution_;
294}
295
297{
298 return consecutiveFailures_ >= maxFailures_;
299}
300
302{
303 std::lock_guard<std::mutex> _(graphMutex_);
304 if (boost::num_vertices(g_) < 1)
305 {
306 sparseQueryVertex_ = boost::add_vertex(s_);
307 queryVertex_ = boost::add_vertex(g_);
308 sparseStateProperty_[sparseQueryVertex_] = nullptr;
309 stateProperty_[queryVertex_] = nullptr;
310 }
311}
312
314{
315 return boost::same_component(m1, m2, sparseDJSets_);
316}
317
319{
320 checkValidity();
321 checkQueryStateInitialization();
322
323 auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
324
325 if (goal == nullptr)
326 {
327 OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
329 }
330
331 // Add the valid start states as milestones
332 while (const base::State *st = pis_.nextStart())
333 {
334 addMilestone(si_->cloneState(st));
335 startM_.push_back(addGuard(si_->cloneState(st), START));
336 }
337 if (startM_.empty())
338 {
339 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
341 }
342
343 if (goalM_.empty() && !goal->couldSample())
344 {
345 OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
347 }
348
349 // Add the valid goal states as milestones
350 while (const base::State *st = (goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal()))
351 {
352 addMilestone(si_->cloneState(st));
353 goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
354 }
355 if (goalM_.empty())
356 {
357 OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
359 }
360
361 unsigned int nrStartStatesDense = boost::num_vertices(g_) - 1; // don't count query vertex
362 unsigned int nrStartStatesSparse = boost::num_vertices(s_) - 1; // don't count query vertex
363 OMPL_INFORM("%s: Starting planning with %u dense states, %u sparse states", getName().c_str(), nrStartStatesDense,
364 nrStartStatesSparse);
365
366 // Reset addedSolution_ member
367 addedSolution_ = false;
368 resetFailures();
369 base::PathPtr sol;
370 base::PlannerTerminationCondition ptcOrFail([this, &ptc]
371 {
372 return ptc || reachedFailureLimit();
373 });
374 std::thread slnThread([this, &ptcOrFail, &sol]
375 {
376 checkForSolution(ptcOrFail, sol);
377 });
378
379 // Construct planner termination condition which also takes maxFailures_ and addedSolution_ into account
380 base::PlannerTerminationCondition ptcOrStop([this, &ptc]
381 {
382 return ptc || reachedTerminationCriterion();
383 });
384 constructRoadmap(ptcOrStop);
385
386 // Ensure slnThread is ceased before exiting solve
387 slnThread.join();
388
389 if (sol)
390 pdef_->addSolutionPath(sol, false, -1.0, getName());
391
392 OMPL_INFORM("%s: Created %u dense states, %u sparse states", getName().c_str(),
393 (unsigned int)(boost::num_vertices(g_) - nrStartStatesDense),
394 (unsigned int)(boost::num_vertices(s_) - nrStartStatesSparse));
395
396 // Return true if any solution was found.
398}
399
401{
402 if (stopOnMaxFail)
403 {
404 resetFailures();
405 base::PlannerTerminationCondition ptcOrFail([this, &ptc]
406 {
407 return ptc || reachedFailureLimit();
408 });
409 constructRoadmap(ptcOrFail);
410 }
411 else
412 constructRoadmap(ptc);
413}
414
416{
417 checkQueryStateInitialization();
418
419 if (!isSetup())
420 setup();
421 if (!sampler_)
422 sampler_ = si_->allocValidStateSampler();
423
424 base::State *workState = si_->allocState();
425
426 /* The whole neighborhood set which has been most recently computed */
427 std::vector<SparseVertex> graphNeighborhood;
428
429 /* The visible neighborhood set which has been most recently computed */
430 std::vector<SparseVertex> visibleNeighborhood;
431
432 /* Storage for the interface neighborhood, populated by getInterfaceNeighborhood() */
433 std::vector<DenseVertex> interfaceNeighborhood;
434
435 bestCost_ = opt_->infiniteCost();
436 while (!ptc)
437 {
438 iterations_++;
439
440 // Generate a single sample, and attempt to connect it to nearest neighbors.
441 DenseVertex q = addSample(workState, ptc);
442 if (q == boost::graph_traits<DenseGraph>::null_vertex())
443 continue;
444
445 // Now that we've added to D, try adding to S
446 // Start by figuring out who our neighbors are
447 getSparseNeighbors(workState, graphNeighborhood);
448 filterVisibleNeighbors(workState, graphNeighborhood, visibleNeighborhood);
449 // Check for addition for Coverage
450 if (!checkAddCoverage(workState, graphNeighborhood))
451 // If not for Coverage, then Connectivity
452 if (!checkAddConnectivity(workState, graphNeighborhood))
453 // Check for the existence of an interface
454 if (!checkAddInterface(graphNeighborhood, visibleNeighborhood, q))
455 {
456 // Then check to see if it's on an interface
457 getInterfaceNeighborhood(q, interfaceNeighborhood);
458 if (!interfaceNeighborhood.empty())
459 {
460 // Check for addition for spanner prop
461 if (!checkAddPath(q, interfaceNeighborhood))
462 // All of the tests have failed. Report failure for the sample
463 ++consecutiveFailures_;
464 }
465 else
466 // There's no interface here, so drop it
467 ++consecutiveFailures_;
468 }
469 }
470
471 si_->freeState(workState);
472}
473
475{
476 std::lock_guard<std::mutex> _(graphMutex_);
477
478 DenseVertex m = boost::add_vertex(g_);
479 stateProperty_[m] = state;
480
481 // Which milestones will we attempt to connect to?
482 const std::vector<DenseVertex> &neighbors = connectionStrategy_(m);
483
484 foreach (DenseVertex n, neighbors)
485 if (si_->checkMotion(stateProperty_[m], stateProperty_[n]))
486 {
487 const double weight = distanceFunction(m, n);
488 const DenseGraph::edge_property_type properties(weight);
489
490 boost::add_edge(m, n, properties, g_);
491 }
492
493 nn_->add(m);
494
495 // Need to update representative information here...
496 calculateRepresentative(m);
497
498 std::vector<DenseVertex> interfaceNeighborhood;
499 std::set<SparseVertex> interfaceRepresentatives;
500
501 getInterfaceNeighborRepresentatives(m, interfaceRepresentatives);
502 getInterfaceNeighborhood(m, interfaceNeighborhood);
503 addToRepresentatives(m, representativesProperty_[m], interfaceRepresentatives);
504 foreach (DenseVertex qp, interfaceNeighborhood)
505 {
506 removeFromRepresentatives(qp, representativesProperty_[qp]);
507 getInterfaceNeighborRepresentatives(qp, interfaceRepresentatives);
508 addToRepresentatives(qp, representativesProperty_[qp], interfaceRepresentatives);
509 }
510
511 return m;
512}
513
515{
516 std::lock_guard<std::mutex> _(graphMutex_);
517
518 SparseVertex v = boost::add_vertex(s_);
519 sparseStateProperty_[v] = state;
520 sparseColorProperty_[v] = type;
521
522 sparseDJSets_.make_set(v);
523
524 snn_->add(v);
525 updateRepresentatives(v);
526
527 resetFailures();
528 return v;
529}
530
532{
533 const base::Cost weight(costHeuristic(v, vp));
534 const SpannerGraph::edge_property_type properties(weight);
535 std::lock_guard<std::mutex> _(graphMutex_);
536 boost::add_edge(v, vp, properties, s_);
537 sparseDJSets_.union_set(v, vp);
538}
539
541{
542 const double weight = distanceFunction(v, vp);
543 const DenseGraph::edge_property_type properties(weight);
544 std::lock_guard<std::mutex> _(graphMutex_);
545 boost::add_edge(v, vp, properties, g_);
546}
547
548bool ompl::geometric::SPARS::checkAddCoverage(const base::State *lastState, const std::vector<SparseVertex> &neigh)
549{
550 // For each of these neighbors,
551 foreach (SparseVertex n, neigh)
552 // If path between is free
553 if (si_->checkMotion(lastState, sparseStateProperty_[n]))
554 // Abort out and return false
555 return false;
556 // No free paths means we add for coverage
557 addGuard(si_->cloneState(lastState), COVERAGE);
558 return true;
559}
560
561bool ompl::geometric::SPARS::checkAddConnectivity(const base::State *lastState, const std::vector<SparseVertex> &neigh)
562{
563 std::vector<SparseVertex> links;
564 // For each neighbor
565 for (std::size_t i = 0; i < neigh.size(); ++i)
566 // For each other neighbor
567 for (std::size_t j = i + 1; j < neigh.size(); ++j)
568 // If they are in different components
569 if (!sameComponent(neigh[i], neigh[j]))
570 // If the paths between are collision free
571 if (si_->checkMotion(lastState, sparseStateProperty_[neigh[i]]) &&
572 si_->checkMotion(lastState, sparseStateProperty_[neigh[j]]))
573 {
574 links.push_back(neigh[i]);
575 links.push_back(neigh[j]);
576 }
577
578 if (!links.empty())
579 {
580 // Add the node
581 SparseVertex g = addGuard(si_->cloneState(lastState), CONNECTIVITY);
582
583 for (unsigned long link : links)
584 // If there's no edge
585 if (!boost::edge(g, link, s_).second)
586 // And the components haven't been united by previous links
587 if (!sameComponent(link, g))
588 connectSparsePoints(g, link);
589 return true;
590 }
591 return false;
592}
593
594bool ompl::geometric::SPARS::checkAddInterface(const std::vector<SparseVertex> &graphNeighborhood,
595 const std::vector<SparseVertex> &visibleNeighborhood, DenseVertex q)
596{
597 // If we have more than 1 neighbor
598 if (visibleNeighborhood.size() > 1)
599 // If our closest neighbors are also visible
600 if (graphNeighborhood[0] == visibleNeighborhood[0] && graphNeighborhood[1] == visibleNeighborhood[1])
601 // If our two closest neighbors don't share an edge
602 if (!boost::edge(visibleNeighborhood[0], visibleNeighborhood[1], s_).second)
603 {
604 // If they can be directly connected
605 if (si_->checkMotion(sparseStateProperty_[visibleNeighborhood[0]],
606 sparseStateProperty_[visibleNeighborhood[1]]))
607 {
608 // Connect them
609 connectSparsePoints(visibleNeighborhood[0], visibleNeighborhood[1]);
610 // And report that we added to the roadmap
611 resetFailures();
612 // Report success
613 return true;
614 }
615
616 // Add the new node to the graph, to bridge the interface
617 SparseVertex v = addGuard(si_->cloneState(stateProperty_[q]), INTERFACE);
618 connectSparsePoints(v, visibleNeighborhood[0]);
619 connectSparsePoints(v, visibleNeighborhood[1]);
620 // Report success
621 return true;
622 }
623 return false;
624}
625
626bool ompl::geometric::SPARS::checkAddPath(DenseVertex q, const std::vector<DenseVertex> &neigh)
627{
628 bool result = false;
629
630 // Get q's representative => v
631 SparseVertex v = representativesProperty_[q];
632
633 // Extract the representatives of neigh => n_rep
634 std::set<SparseVertex> n_rep;
635 foreach (DenseVertex qp, neigh)
636 n_rep.insert(representativesProperty_[qp]);
637
638 std::vector<SparseVertex> Xs;
639 // for each v' in n_rep
640 for (auto it = n_rep.begin(); it != n_rep.end() && !result; ++it)
641 {
642 SparseVertex vp = *it;
643 // Identify appropriate v" candidates => vpps
644 std::vector<SparseVertex> VPPs;
645 computeVPP(v, vp, VPPs);
646
647 foreach (SparseVertex vpp, VPPs)
648 {
649 double s_max = 0;
650 // Find the X nodes to test
651 computeX(v, vp, vpp, Xs);
652
653 // For each x in xs
654 foreach (SparseVertex x, Xs)
655 {
656 // Compute/Retain MAXimum distance path thorugh S
657 double dist = (si_->distance(sparseStateProperty_[x], sparseStateProperty_[v]) +
658 si_->distance(sparseStateProperty_[v], sparseStateProperty_[vp])) /
659 2.0;
660 if (dist > s_max)
661 s_max = dist;
662 }
663
664 DensePath bestDPath;
665 DenseVertex best_qpp = boost::graph_traits<DenseGraph>::null_vertex();
666 double d_min = std::numeric_limits<double>::infinity(); // Insanely big number
667 // For each vpp in vpps
668 for (std::size_t j = 0; j < VPPs.size() && !result; ++j)
669 {
670 SparseVertex vpp = VPPs[j];
671 // For each q", which are stored interface nodes on v for i(vpp,v)
672 foreach (DenseVertex qpp, interfaceListsProperty_[v][vpp])
673 {
674 // check that representatives are consistent
675 assert(representativesProperty_[qpp] == v);
676
677 // If they happen to be the one and same node
678 if (q == qpp)
679 {
680 bestDPath.push_front(stateProperty_[q]);
681 best_qpp = qpp;
682 d_min = 0;
683 }
684 else
685 {
686 // Compute/Retain MINimum distance path on D through q, q"
687 DensePath dPath;
688 computeDensePath(q, qpp, dPath);
689 if (!dPath.empty())
690 {
691 // compute path length
692 double length = 0.0;
693 DensePath::const_iterator jt = dPath.begin();
694 for (auto it = jt + 1; it != dPath.end(); ++it)
695 {
696 length += si_->distance(*jt, *it);
697 jt = it;
698 }
699
700 if (length < d_min)
701 {
702 d_min = length;
703 bestDPath.swap(dPath);
704 best_qpp = qpp;
705 }
706 }
707 }
708 }
709
710 // If the spanner property is violated for these paths
711 if (s_max > stretchFactor_ * d_min)
712 {
713 // Need to augment this path with the appropriate neighbor information
714 DenseVertex na = getInterfaceNeighbor(q, vp);
715 DenseVertex nb = getInterfaceNeighbor(best_qpp, vpp);
716
717 bestDPath.push_front(stateProperty_[na]);
718 bestDPath.push_back(stateProperty_[nb]);
719
720 // check consistency of representatives
721 assert(representativesProperty_[na] == vp && representativesProperty_[nb] == vpp);
722
723 // Add the dense path to the spanner
724 addPathToSpanner(bestDPath, vpp, vp);
725
726 // Report success
727 result = true;
728 }
729 }
730 }
731 }
732 return result;
733}
734
736{
737 double degree = 0.0;
738 foreach (DenseVertex v, boost::vertices(s_))
739 degree += (double)boost::out_degree(v, s_);
740 degree /= (double)boost::num_vertices(s_);
741 return degree;
742}
743
744void ompl::geometric::SPARS::printDebug(std::ostream &out) const
745{
746 out << "SPARS Debug Output: " << std::endl;
747 out << " Settings: " << std::endl;
748 out << " Max Failures: " << getMaxFailures() << std::endl;
749 out << " Dense Delta Fraction: " << getDenseDeltaFraction() << std::endl;
750 out << " Sparse Delta Fraction: " << getSparseDeltaFraction() << std::endl;
751 out << " Stretch Factor: " << getStretchFactor() << std::endl;
752 out << " Status: " << std::endl;
753 out << " Milestone Count: " << milestoneCount() << std::endl;
754 out << " Guard Count: " << guardCount() << std::endl;
755 out << " Iterations: " << getIterationCount() << std::endl;
756 out << " Average Valence: " << averageValence() << std::endl;
757 out << " Consecutive Failures: " << consecutiveFailures_ << std::endl;
758}
759
760void ompl::geometric::SPARS::getSparseNeighbors(base::State *inState, std::vector<SparseVertex> &graphNeighborhood)
761{
762 sparseStateProperty_[sparseQueryVertex_] = inState;
763
764 graphNeighborhood.clear();
765 snn_->nearestR(sparseQueryVertex_, sparseDelta_, graphNeighborhood);
766
767 sparseStateProperty_[sparseQueryVertex_] = nullptr;
768}
769
771 const std::vector<SparseVertex> &graphNeighborhood,
772 std::vector<SparseVertex> &visibleNeighborhood) const
773{
774 visibleNeighborhood.clear();
775 // Now that we got the neighbors from the NN, we must remove any we can't see
776 for (unsigned long i : graphNeighborhood)
777 if (si_->checkMotion(inState, sparseStateProperty_[i]))
778 visibleNeighborhood.push_back(i);
779}
780
782{
783 foreach (DenseVertex vp, boost::adjacent_vertices(q, g_))
784 if (representativesProperty_[vp] == rep)
785 if (distanceFunction(q, vp) <= denseDelta_)
786 return vp;
787 throw Exception(name_, "Vertex has no interface neighbor with given representative");
788}
789
791{
792 // First, check to see that the path has length
793 if (dense_path.size() <= 1)
794 {
795 // The path is 0 length, so simply link the representatives
796 connectSparsePoints(vp, vpp);
797 resetFailures();
798 }
799 else
800 {
801 // We will need to construct a PathGeometric to do this.
802 geomPath_.getStates().resize(dense_path.size());
803 std::copy(dense_path.begin(), dense_path.end(), geomPath_.getStates().begin());
804
805 // Attempt to simplify the path
806 psimp_->reduceVertices(geomPath_, geomPath_.getStateCount() * 2);
807
808 // we are sure there are at least 2 points left on geomPath_
809
810 std::vector<SparseVertex> added_nodes;
811 added_nodes.reserve(geomPath_.getStateCount());
812 for (std::size_t i = 0; i < geomPath_.getStateCount(); ++i)
813 {
814 // Add each guard
815 SparseVertex ng = addGuard(si_->cloneState(geomPath_.getState(i)), QUALITY);
816 added_nodes.push_back(ng);
817 }
818 // Link them up
819 for (std::size_t i = 1; i < added_nodes.size(); ++i)
820 {
821 connectSparsePoints(added_nodes[i - 1], added_nodes[i]);
822 }
823 // Don't forget to link them to their representatives
824 connectSparsePoints(added_nodes[0], vp);
825 connectSparsePoints(added_nodes[added_nodes.size() - 1], vpp);
826 }
827 geomPath_.getStates().clear();
828 return true;
829}
830
832{
833 // Get all of the dense samples which may be affected by adding this node
834 std::vector<DenseVertex> dense_points;
835
836 stateProperty_[queryVertex_] = sparseStateProperty_[v];
837
838 nn_->nearestR(queryVertex_, sparseDelta_ + denseDelta_, dense_points);
839
840 stateProperty_[queryVertex_] = nullptr;
841
842 // For each of those points
843 for (unsigned long dense_point : dense_points)
844 {
845 // Remove that point from the old representative's list(s)
846 removeFromRepresentatives(dense_point, representativesProperty_[dense_point]);
847 // Update that point's representative
848 calculateRepresentative(dense_point);
849 }
850
851 std::set<SparseVertex> interfaceRepresentatives;
852 // For each of the points
853 for (unsigned long dense_point : dense_points)
854 {
855 // Get it's representative
856 SparseVertex rep = representativesProperty_[dense_point];
857 // Extract the representatives of any interface-sharing neighbors
858 getInterfaceNeighborRepresentatives(dense_point, interfaceRepresentatives);
859 // For sanity's sake, make sure we clear ourselves out of what this new rep might think of us
860 removeFromRepresentatives(dense_point, rep);
861 // Add this vertex to it's representative's list for the other representatives
862 addToRepresentatives(dense_point, rep, interfaceRepresentatives);
863 }
864}
865
867{
868 // Get the nearest neighbors within sparseDelta_
869 std::vector<SparseVertex> graphNeighborhood;
870 getSparseNeighbors(stateProperty_[q], graphNeighborhood);
871
872 // For each neighbor
873 for (unsigned long i : graphNeighborhood)
874 if (si_->checkMotion(stateProperty_[q], sparseStateProperty_[i]))
875 {
876 // update the representative
877 representativesProperty_[q] = i;
878 // abort
879 break;
880 }
881}
882
883void ompl::geometric::SPARS::addToRepresentatives(DenseVertex q, SparseVertex rep, const std::set<SparseVertex> &oreps)
884{
885 // If this node supports no interfaces
886 if (oreps.empty())
887 {
888 // Add it to the pool of non-interface nodes
889 bool new_insert = nonInterfaceListsProperty_[rep].insert(q).second;
890
891 // we expect this was not previously tracked
892 if (!new_insert)
893 assert(false);
894 }
895 else
896 {
897 // otherwise, for every neighbor representative
898 foreach (SparseVertex v, oreps)
899 {
900 assert(rep == representativesProperty_[q]);
901 bool new_insert = interfaceListsProperty_[rep][v].insert(q).second;
902 if (!new_insert)
903 assert(false);
904 }
905 }
906}
907
909{
910 // Remove the node from the non-interface points (if there)
911 nonInterfaceListsProperty_[rep].erase(q);
912
913 // From each of the interfaces
914 foreach (SparseVertex vpp, interfaceListsProperty_[rep] | boost::adaptors::map_keys)
915 {
916 // Remove this node from that list
917 interfaceListsProperty_[rep][vpp].erase(q);
918 }
919}
920
921void ompl::geometric::SPARS::computeVPP(SparseVertex v, SparseVertex vp, std::vector<SparseVertex> &VPPs)
922{
923 foreach (SparseVertex cvpp, boost::adjacent_vertices(v, s_))
924 if (cvpp != vp)
925 if (!boost::edge(cvpp, vp, s_).second)
926 VPPs.push_back(cvpp);
927}
928
929void ompl::geometric::SPARS::computeX(SparseVertex v, SparseVertex vp, SparseVertex vpp, std::vector<SparseVertex> &Xs)
930{
931 Xs.clear();
932 foreach (SparseVertex cx, boost::adjacent_vertices(vpp, s_))
933 if (boost::edge(cx, v, s_).second && !boost::edge(cx, vp, s_).second)
934 if (!interfaceListsProperty_[vpp][cx].empty())
935 Xs.push_back(cx);
936 Xs.push_back(vpp);
937}
938
940 std::set<SparseVertex> &interfaceRepresentatives)
941{
942 interfaceRepresentatives.clear();
943
944 // Get our representative
945 SparseVertex rep = representativesProperty_[q];
946 // For each neighbor we are connected to
947 foreach (DenseVertex n, boost::adjacent_vertices(q, g_))
948 {
949 // Get his representative
950 SparseVertex orep = representativesProperty_[n];
951 // If that representative is not our own
952 if (orep != rep)
953 // If he is within denseDelta_
954 if (distanceFunction(q, n) < denseDelta_)
955 // Include his rep in the set
956 interfaceRepresentatives.insert(orep);
957 }
958}
959
960void ompl::geometric::SPARS::getInterfaceNeighborhood(DenseVertex q, std::vector<DenseVertex> &interfaceNeighborhood)
961{
962 interfaceNeighborhood.clear();
963
964 // Get our representative
965 SparseVertex rep = representativesProperty_[q];
966
967 // For each neighbor we are connected to
968 foreach (DenseVertex n, boost::adjacent_vertices(q, g_))
969 // If neighbor representative is not our own
970 if (representativesProperty_[n] != rep)
971 // If he is within denseDelta_
972 if (distanceFunction(q, n) < denseDelta_)
973 // Append him to the list
974 interfaceNeighborhood.push_back(n);
975}
976
978{
979 std::lock_guard<std::mutex> _(graphMutex_);
980
981 boost::vector_property_map<SparseVertex> prev(boost::num_vertices(s_));
982
983 try
984 {
985 // Consider using a persistent distance_map if it's slow
986 boost::astar_search(s_, start,
987 [this, goal](SparseVertex v)
988 {
989 return costHeuristic(v, goal);
990 },
991 boost::predecessor_map(prev)
992 .distance_compare([this](base::Cost c1, base::Cost c2)
993 {
994 return opt_->isCostBetterThan(c1, c2);
995 })
996 .distance_combine([this](base::Cost c1, base::Cost c2)
997 {
998 return opt_->combineCosts(c1, c2);
999 })
1000 .distance_inf(opt_->infiniteCost())
1001 .distance_zero(opt_->identityCost())
1002 .visitor(AStarGoalVisitor<SparseVertex>(goal)));
1003 }
1004 catch (AStarFoundGoal &)
1005 {
1006 }
1007
1008 if (prev[goal] == goal)
1009 throw Exception(name_, "Could not find solution path");
1010 else
1011 {
1012 auto p(std::make_shared<PathGeometric>(si_));
1013
1014 for (SparseVertex pos = goal; prev[pos] != pos; pos = prev[pos])
1015 p->append(sparseStateProperty_[pos]);
1016 p->append(sparseStateProperty_[start]);
1017 p->reverse();
1018
1019 return p;
1020 }
1021}
1022
1024{
1025 path.clear();
1026
1027 boost::vector_property_map<DenseVertex> prev(boost::num_vertices(g_));
1028
1029 try
1030 {
1031 boost::astar_search(g_, start,
1032 [this, goal](const DenseVertex a)
1033 {
1034 return distanceFunction(a, goal);
1035 },
1036 boost::predecessor_map(prev).visitor(AStarGoalVisitor<DenseVertex>(goal)));
1037 }
1038 catch (AStarFoundGoal &)
1039 {
1040 }
1041
1042 if (prev[goal] == goal)
1043 OMPL_WARN("%s: No dense path was found?", getName().c_str());
1044 else
1045 {
1046 for (DenseVertex pos = goal; prev[pos] != pos; pos = prev[pos])
1047 path.push_front(stateProperty_[pos]);
1048 path.push_front(stateProperty_[start]);
1049 }
1050}
1051
1053{
1054 Planner::getPlannerData(data);
1055
1056 // Explicitly add start and goal states:
1057 for (unsigned long i : startM_)
1058 data.addStartVertex(base::PlannerDataVertex(sparseStateProperty_[i], (int)START));
1059
1060 for (unsigned long i : goalM_)
1061 data.addGoalVertex(base::PlannerDataVertex(sparseStateProperty_[i], (int)GOAL));
1062
1063 // Adding edges and all other vertices simultaneously
1064 foreach (const SparseEdge e, boost::edges(s_))
1065 {
1066 const SparseVertex v1 = boost::source(e, s_);
1067 const SparseVertex v2 = boost::target(e, s_);
1068 data.addEdge(base::PlannerDataVertex(sparseStateProperty_[v1], (int)sparseColorProperty_[v1]),
1069 base::PlannerDataVertex(sparseStateProperty_[v2], (int)sparseColorProperty_[v2]));
1070
1071 // Add the reverse edge, since we're constructing an undirected roadmap
1072 data.addEdge(base::PlannerDataVertex(sparseStateProperty_[v2], (int)sparseColorProperty_[v1]),
1073 base::PlannerDataVertex(sparseStateProperty_[v1], (int)sparseColorProperty_[v2]));
1074 }
1075
1076 // Make sure to add edge-less nodes as well
1077 foreach (const SparseVertex n, boost::vertices(s_))
1078 if (boost::out_degree(n, s_) == 0)
1079 data.addVertex(base::PlannerDataVertex(sparseStateProperty_[n], (int)sparseColorProperty_[n]));
1080}
1081
1083{
1084 return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
1085}
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 a goal region that can be sampled.
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
An optimization objective which corresponds to optimizing path length.
A shared pointer wrapper for ompl::base::Path.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition PlannerData.h:59
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...
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....
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void addPlannerProgressProperty(const std::string &progressPropertyName, const PlannerProgressProperty &prop)
Add a planner progress property called progressPropertyName with a property querying function prop to...
Definition Planner.h:410
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition Planner.h:429
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:417
Definition of an abstract state.
Definition State.h:50
Make the minimal number of connections required to ensure asymptotic optimality.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition SPARS.cpp:98
bool checkAddPath(DenseVertex q, const std::vector< DenseVertex > &neigh)
Checks for adding an entire dense path to the Sparse Roadmap.
Definition SPARS.cpp:626
bool reachedFailureLimit() const
Returns true if we have reached the iteration failures limit, maxFailures_
Definition SPARS.cpp:296
void getInterfaceNeighborRepresentatives(DenseVertex q, std::set< SparseVertex > &interfaceRepresentatives)
Gets the representatives of all interfaces that q supports.
Definition SPARS.cpp:939
void getSparseNeighbors(base::State *inState, std::vector< SparseVertex > &graphNeighborhood)
Get all nodes in the sparse graph which are within sparseDelta_ of the given state.
Definition SPARS.cpp:760
void computeX(DenseVertex v, DenseVertex vp, DenseVertex vpp, std::vector< SparseVertex > &Xs)
Computes all nodes which qualify as a candidate x for v, v', and v".
Definition SPARS.cpp:929
boost::graph_traits< DenseGraph >::vertex_descriptor DenseVertex
A vertex in DenseGraph.
Definition SPARS.h:184
~SPARS() override
Destructor.
Definition SPARS.cpp:93
DenseVertex addMilestone(base::State *state)
Construct a milestone for a given state (state) and store it in the nearest neighbors data structure.
Definition SPARS.cpp:474
void setStretchFactor(double t)
Set the roadmap spanner stretch factor. This value represents a multiplicative upper bound on path qu...
Definition SPARS.h:292
void checkQueryStateInitialization()
Check that the query vertex is initialized (used for internal nearest neighbor searches)
Definition SPARS.cpp:301
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 SPARS.cpp:1052
void connectDensePoints(DenseVertex v, DenseVertex vp)
Connects points in the dense graph.
Definition SPARS.cpp:540
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition SPARS.cpp:171
base::Cost costHeuristic(SparseVertex u, SparseVertex v) const
Given two vertices, returns a heuristic on the cost of the path connecting them. This method wraps Op...
Definition SPARS.cpp:1082
bool reachedTerminationCriterion() const
Returns true if we have reached the iteration failures limit, maxFailures_ or if a solution was added...
Definition SPARS.cpp:291
base::PathPtr constructSolution(SparseVertex start, SparseVertex goal) const
Given two milestones from the same connected component, construct a path connecting them and set it a...
Definition SPARS.cpp:977
void setDenseDeltaFraction(double d)
Set the delta fraction for interface detection. If two nodes in the dense graph are more than a delta...
Definition SPARS.h:269
void updateRepresentatives(SparseVertex v)
Automatically updates the representatives of all dense samplse within sparseDelta_ of v.
Definition SPARS.cpp:831
double getDenseDeltaFraction() const
Retrieve the dense graph interface support delta fraction.
Definition SPARS.h:304
bool checkAddInterface(const std::vector< DenseVertex > &graphNeighborhood, const std::vector< DenseVertex > &visibleNeighborhood, DenseVertex q)
Checks the latest dense sample for bridging an edge-less interface.
Definition SPARS.cpp:594
SparseVertex addGuard(base::State *state, GuardType type)
Construct a node with the given state (state) for the spanner and store it in the nn structure.
Definition SPARS.cpp:514
std::deque< base::State * > DensePath
Internal representation of a dense path.
Definition SPARS.h:123
void computeDensePath(DenseVertex start, DenseVertex goal, DensePath &path) const
Constructs the dense path between the start and goal vertices (if connected)
Definition SPARS.cpp:1023
boost::graph_traits< SpannerGraph >::edge_descriptor SparseEdge
An edge in the sparse roadmap that is constructed.
Definition SPARS.h:154
boost::graph_traits< SpannerGraph >::vertex_descriptor SparseVertex
A vertex in the sparse roadmap that is constructed.
Definition SPARS.h:151
double averageValence() const
Returns the average valence of the spanner graph.
Definition SPARS.cpp:735
bool checkAddConnectivity(const base::State *lastState, const std::vector< SparseVertex > &neigh)
Checks the latest dense sample for connectivity, and adds appropriately.
Definition SPARS.cpp:561
DenseVertex getInterfaceNeighbor(DenseVertex q, SparseVertex rep)
Get the first neighbor of q who has representative rep and is within denseDelta_.
Definition SPARS.cpp:781
void clearQuery() override
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition SPARS.cpp:160
unsigned getMaxFailures() const
Retrieve the maximum consecutive failure limit.
Definition SPARS.h:298
void calculateRepresentative(DenseVertex q)
Calculates the representative for a dense sample.
Definition SPARS.cpp:866
void setSparseDeltaFraction(double d)
Set the delta fraction for connection distance on the sparse spanner. This value represents the visib...
Definition SPARS.h:280
void printDebug(std::ostream &out=std::cout) const
Print debug information about planner.
Definition SPARS.cpp:744
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition permits, construct the spanner graph.
Definition SPARS.cpp:415
PathSimplifierPtr psimp_
A path simplifier used to simplify dense paths added to S.
Definition SPARS.h:531
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 SPARS.cpp:318
bool haveSolution(const std::vector< DenseVertex > &starts, const std::vector< DenseVertex > &goals, base::PathPtr &solution)
Check if there exists a solution, i.e., there exists a pair of milestones such that the first is in s...
Definition SPARS.cpp:250
GuardType
Enumeration which specifies the reason a guard is added to the spanner.
Definition SPARS.h:82
void connectSparsePoints(SparseVertex v, SparseVertex vp)
Convenience function for creating an edge in the Spanner Roadmap.
Definition SPARS.cpp:531
void freeMemory()
Free all the memory allocated by the planner.
Definition SPARS.cpp:186
bool addPathToSpanner(const DensePath &dense_path, SparseVertex vp, SparseVertex vpp)
Method for actually adding a dense path to the Roadmap Spanner, S.
Definition SPARS.cpp:790
void addToRepresentatives(DenseVertex q, SparseVertex rep, const std::set< SparseVertex > &oreps)
Adds a dense sample to the appropriate lists of its representative.
Definition SPARS.cpp:883
bool checkAddCoverage(const base::State *lastState, const std::vector< SparseVertex > &neigh)
Checks the latest dense sample for the coverage property, and adds appropriately.
Definition SPARS.cpp:548
void resetFailures()
A reset function for resetting the failures count.
Definition SPARS.cpp:155
void setMaxFailures(unsigned int m)
Set the maximum consecutive failures to augment the spanner before termination. In general,...
Definition SPARS.h:261
double getStretchFactor() const
Retrieve the spanner's set stretch factor.
Definition SPARS.h:316
void computeVPP(DenseVertex v, DenseVertex vp, std::vector< SparseVertex > &VPPs)
Computes all nodes which qualify as a candidate v" for v and vp.
Definition SPARS.cpp:921
DenseVertex addSample(base::State *workState, const base::PlannerTerminationCondition &ptc)
Attempt to add a single sample to the roadmap.
Definition SPARS.cpp:204
double getSparseDeltaFraction() const
Retrieve the sparse graph visibility range delta fraction.
Definition SPARS.h:310
void getInterfaceNeighborhood(DenseVertex q, std::vector< DenseVertex > &interfaceNeighborhood)
Gets the neighbors of q who help it support an interface.
Definition SPARS.cpp:960
void filterVisibleNeighbors(base::State *inState, const std::vector< SparseVertex > &graphNeighborhood, std::vector< SparseVertex > &visibleNeighborhood) const
Get the visible neighbors.
Definition SPARS.cpp:770
void removeFromRepresentatives(DenseVertex q, SparseVertex rep)
Removes the node from its representative's lists.
Definition SPARS.cpp:908
bool sameComponent(SparseVertex m1, SparseVertex m2)
Check that two vertices are in the same connected component.
Definition SPARS.cpp:313
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
Definition SPARS.cpp:226
SPARS(const base::SpaceInformationPtr &si)
Constructor.
Definition SPARS.cpp:54
#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_WARN(fmt,...)
Log a formatted warning string.
Definition Console.h:66
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition GoalTypes.h:56
static const unsigned int FIND_VALID_STATE_ATTEMPTS_WITHOUT_TERMINATION_CHECK
Maximum number of sampling attempts to find a valid state, without checking whether the allowed time ...
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition Planner.h:199
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition Planner.h:206
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition Planner.h:196
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition Planner.h:202
A class to store the exit status of Planner::solve()
@ INVALID_START
Invalid start state or no start state specified.
@ EXACT_SOLUTION
The planner found an exact solution.
@ INVALID_GOAL
Invalid goal state.
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
@ TIMEOUT
The planner failed to find a solution.