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/tools/config/SelfConfig.h"
41 #include "ompl/tools/config/MagicConstants.h"
42 #include <boost/bind.hpp>
43 #include <boost/graph/astar_search.hpp>
44 #include <boost/graph/incremental_components.hpp>
45 #include <boost/property_map/vector_property_map.hpp>
46 #include <boost/foreach.hpp>
48 #include "GoalVisitor.hpp"
50 #define foreach BOOST_FOREACH
51 #define foreach_reverse BOOST_REVERSE_FOREACH
54 base::Planner(si,
"SPARS"),
62 weightProperty_(boost::get(boost::edge_weight, g_)),
63 sparseDJSets_(boost::get(boost::vertex_rank, s_),
64 boost::get(boost::vertex_predecessor, s_)),
65 consecutiveFailures_(0),
69 addedSolution_(false),
70 denseDeltaFraction_(.001),
71 sparseDeltaFraction_(.25),
97 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<DenseVertex>(si_->getStateSpace()));
100 snn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<SparseVertex>(si_->getStateSpace()));
102 if (!connectionStrategy_)
104 double maxExt = si_->getMaximumExtent();
105 sparseDelta_ = sparseDeltaFraction_ * maxExt;
106 denseDelta_ = denseDeltaFraction_ * maxExt;
111 Planner::setProblemDefinition(pdef);
117 consecutiveFailures_ = 0;
131 simpleSampler_.reset();
145 if( stateProperty_[v] != NULL )
147 si_->freeState(stateProperty_[v]);
148 stateProperty_[v] = NULL;
151 if( sparseStateProperty_[n] != NULL )
153 si_->freeState(sparseStateProperty_[n]);
154 sparseStateProperty_[n] = NULL;
162 DenseVertex result = boost::graph_traits<DenseGraph>::null_vertex();
166 while (!found && ptc ==
false)
168 unsigned int attempts = 0;
171 found = sampler_->sample(workState);
177 result = addMilestone(si_->cloneState(workState));
184 while (!ptc && !addedSolution_)
192 addMilestone(si_->cloneState(st));
193 goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
198 addedSolution_ = haveSolution(startM_, goalM_, solution);
201 boost::this_thread::sleep(boost::posix_time::milliseconds(1));
214 bool same_component = sameComponent(start, goal);
215 graphMutex_.unlock();
217 if (same_component && g->
isStartGoalPairValid(sparseStateProperty_[goal], sparseStateProperty_[start]))
219 solution = constructSolution(start, goal);
230 return consecutiveFailures_ >= maxFailures_ || addedSolution_;
235 return consecutiveFailures_ >= maxFailures_;
240 boost::mutex::scoped_lock _(graphMutex_);
241 if (boost::num_vertices(g_) < 1)
243 sparseQueryVertex_ = boost::add_vertex(s_);
244 queryVertex_ = boost::add_vertex(g_);
245 sparseStateProperty_[sparseQueryVertex_] = NULL;
246 stateProperty_[queryVertex_] = NULL;
252 return boost::same_component(m1, m2, sparseDJSets_);
258 checkQueryStateInitialization();
264 OMPL_ERROR(
"%s: Unknown type of goal", getName().c_str());
271 addMilestone(si_->cloneState(st));
272 startM_.push_back(addGuard(si_->cloneState(st), START ));
276 OMPL_ERROR(
"%s: There are no valid initial states!", getName().c_str());
282 OMPL_ERROR(
"%s: Insufficient states in sampleable goal region", getName().c_str());
287 while (
const base::State *st = (goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal()))
289 addMilestone(si_->cloneState(st));
290 goalM_.push_back(addGuard(si_->cloneState(st), GOAL ));
294 OMPL_ERROR(
"%s: Unable to find any valid goal states", getName().c_str());
298 unsigned int nrStartStatesDense = boost::num_vertices(g_) - 1;
299 unsigned int nrStartStatesSparse = boost::num_vertices(s_) - 1;
300 OMPL_INFORM(
"%s: Starting with %u dense states, %u sparse states", getName().c_str(), nrStartStatesDense, nrStartStatesSparse);
303 addedSolution_ =
false;
313 constructRoadmap(ptcOrStop);
319 pdef_->addSolutionPath(sol,
false);
321 OMPL_INFORM(
"%s: Created %u dense states, %u sparse states", getName().c_str(),
322 (
unsigned int)(boost::num_vertices(g_) - nrStartStatesDense),
323 (
unsigned int)(boost::num_vertices(s_) - nrStartStatesSparse));
336 constructRoadmap(ptcOrFail);
339 constructRoadmap(ptc);
344 checkQueryStateInitialization();
347 sampler_ = si_->allocValidStateSampler();
349 simpleSampler_ = si_->allocStateSampler();
354 std::vector<SparseVertex> graphNeighborhood;
357 std::vector<SparseVertex> visibleNeighborhood;
360 std::vector<DenseVertex> interfaceNeighborhood;
368 if (q == boost::graph_traits<DenseGraph>::null_vertex())
373 getSparseNeighbors(workState, graphNeighborhood);
374 filterVisibleNeighbors(workState, graphNeighborhood, visibleNeighborhood);
376 if( !checkAddCoverage(workState, graphNeighborhood))
378 if( !checkAddConnectivity(workState, graphNeighborhood))
380 if( !checkAddInterface(graphNeighborhood, visibleNeighborhood, q))
383 getInterfaceNeighborhood(q, interfaceNeighborhood);
384 if (interfaceNeighborhood.size() > 0)
387 if (!checkAddPath(q, interfaceNeighborhood))
389 ++consecutiveFailures_;
393 ++consecutiveFailures_;
397 si_->freeState(workState);
402 boost::mutex::scoped_lock _(graphMutex_);
405 stateProperty_[m] = state;
408 const std::vector<DenseVertex>& neighbors = connectionStrategy_(m);
411 if (si_->checkMotion(stateProperty_[m], stateProperty_[n]))
413 const double weight = distanceFunction(m, n);
414 const DenseGraph::edge_property_type properties(weight);
416 boost::add_edge(m, n, properties, g_);
422 calculateRepresentative(m);
424 std::vector<DenseVertex> interfaceNeighborhood;
425 std::set<SparseVertex> interfaceRepresentatives;
427 getInterfaceNeighborRepresentatives(m, interfaceRepresentatives);
428 getInterfaceNeighborhood(m, interfaceNeighborhood);
429 addToRepresentatives(m, representativesProperty_[m], interfaceRepresentatives);
432 removeFromRepresentatives( qp, representativesProperty_[qp] );
433 getInterfaceNeighborRepresentatives( qp, interfaceRepresentatives );
434 addToRepresentatives( qp, representativesProperty_[qp], interfaceRepresentatives );
442 boost::mutex::scoped_lock _(graphMutex_);
445 sparseStateProperty_[v] = state;
446 sparseColorProperty_[v] = type;
448 sparseDJSets_.make_set(v);
451 updateRepresentatives(v);
459 const double weight = sparseDistanceFunction(v, vp);
460 const SpannerGraph::edge_property_type properties(weight);
461 boost::mutex::scoped_lock _(graphMutex_);
462 boost::add_edge(v, vp, properties, s_);
463 sparseDJSets_.union_set(v, vp);
468 const double weight = distanceFunction(v, vp);
469 const DenseGraph::edge_property_type properties(weight);
470 boost::mutex::scoped_lock _(graphMutex_);
471 boost::add_edge(v, vp, properties, g_);
479 if (si_->checkMotion( lastState, sparseStateProperty_[n]))
483 addGuard(si_->cloneState(lastState), COVERAGE);
489 std::vector< SparseVertex > links;
491 for (std::size_t i = 0; i < neigh.size(); ++i )
493 for (std::size_t j = i + 1; j < neigh.size(); ++j )
495 if (!sameComponent(neigh[i], neigh[j]))
497 if( si_->checkMotion( lastState, sparseStateProperty_[neigh[i]] ) && si_->checkMotion( lastState, sparseStateProperty_[neigh[j]] ) )
499 links.push_back( neigh[i] );
500 links.push_back( neigh[j] );
503 if( links.size() != 0 )
506 SparseVertex g = addGuard( si_->cloneState(lastState), CONNECTIVITY );
508 for (std::size_t i = 0; i < links.size(); ++i )
510 if (!boost::edge(g, links[i], s_).second)
512 if (!sameComponent(links[i], g))
513 connectSparsePoints( g, links[i] );
522 if( visibleNeighborhood.size() > 1 )
524 if( graphNeighborhood[0] == visibleNeighborhood[0] && graphNeighborhood[1] == visibleNeighborhood[1] )
526 if (!boost::edge(visibleNeighborhood[0], visibleNeighborhood[1], s_).second)
529 if( si_->checkMotion( sparseStateProperty_[visibleNeighborhood[0]], sparseStateProperty_[visibleNeighborhood[1]] ) )
532 connectSparsePoints( visibleNeighborhood[0], visibleNeighborhood[1] );
541 SparseVertex v = addGuard( si_->cloneState( stateProperty_[q] ), INTERFACE );
542 connectSparsePoints( v, visibleNeighborhood[0] );
543 connectSparsePoints( v, visibleNeighborhood[1] );
559 std::set<SparseVertex> n_rep;
561 n_rep.insert(representativesProperty_[qp]);
563 std::vector<SparseVertex> Xs;
565 for (std::set<SparseVertex>::iterator it = n_rep.begin() ; it != n_rep.end() && !result ; ++it)
569 std::vector<SparseVertex> VPPs;
570 computeVPP(v, vp, VPPs);
576 computeX(v, vp, vpp, Xs);
582 double dist = (si_->distance(sparseStateProperty_[x], sparseStateProperty_[v])
583 + si_->distance(sparseStateProperty_[v], sparseStateProperty_[vp])) / 2.0;
589 DenseVertex best_qpp = boost::graph_traits<DenseGraph>::null_vertex();
590 double d_min = std::numeric_limits<double>::infinity();
592 for (std::size_t j = 0; j < VPPs.size() && !result; ++j)
596 foreach(
DenseVertex qpp, interfaceListsProperty_[v].interfaceHash[vpp] )
599 assert(representativesProperty_[qpp] == v);
604 bestDPath.push_front( stateProperty_[q] );
612 computeDensePath(q, qpp, dPath);
613 if (dPath.size() > 0)
617 DensePath::const_iterator jt = dPath.begin();
618 for (DensePath::const_iterator it = jt + 1 ; it != dPath.end() ; ++it)
620 length += si_->distance(*jt, *it);
627 bestDPath.swap(dPath);
635 if (s_max > stretchFactor_* d_min)
639 DenseVertex nb = getInterfaceNeighbor(best_qpp, vpp);
641 bestDPath.push_front( stateProperty_[na] );
642 bestDPath.push_back( stateProperty_[nb] );
645 assert(representativesProperty_[na] == vp && representativesProperty_[nb] == vpp);
648 addPathToSpanner( bestDPath, vpp, vp );
663 degree += (double)boost::out_degree(v, s_);
664 degree /= (double)boost::num_vertices(s_);
670 sparseStateProperty_[sparseQueryVertex_] = inState;
672 graphNeighborhood.clear();
673 snn_->nearestR(sparseQueryVertex_, sparseDelta_, graphNeighborhood);
675 sparseStateProperty_[sparseQueryVertex_] = NULL;
679 std::vector<SparseVertex> &visibleNeighborhood)
const
681 visibleNeighborhood.clear();
683 for (std::size_t i = 0; i < graphNeighborhood.size(); ++i)
684 if (si_->checkMotion(inState, sparseStateProperty_[graphNeighborhood[i]]))
685 visibleNeighborhood.push_back(graphNeighborhood[i]);
690 foreach (
DenseVertex vp, boost::adjacent_vertices( q, g_ ))
691 if (representativesProperty_[vp] == rep )
692 if (distanceFunction( q, vp ) <= denseDelta_)
694 throw Exception(name_,
"Vertex has no interface neighbor with given representative");
700 if (dense_path.size() <= 1)
703 connectSparsePoints( vp, vpp );
709 geomPath_.getStates().resize( dense_path.size() );
710 std::copy( dense_path.begin(), dense_path.end(), geomPath_.getStates().begin() );
713 psimp_->reduceVertices( geomPath_, geomPath_.getStateCount() * 2);
717 std::vector< SparseVertex > added_nodes;
718 added_nodes.reserve(geomPath_.getStateCount());
719 for (std::size_t i = 0; i < geomPath_.getStateCount(); ++i )
722 SparseVertex ng = addGuard( si_->cloneState(geomPath_.getState(i)), QUALITY );
723 added_nodes.push_back( ng );
726 for (std::size_t i = 1; i < added_nodes.size() ; ++i )
728 connectSparsePoints(added_nodes[i - 1], added_nodes[i]);
731 connectSparsePoints( added_nodes[0], vp );
732 connectSparsePoints( added_nodes[added_nodes.size()-1], vpp );
734 geomPath_.getStates().clear();
741 std::vector< DenseVertex > dense_points;
743 stateProperty_[ queryVertex_ ] = sparseStateProperty_[ v ];
745 nn_->nearestR( queryVertex_, sparseDelta_ + denseDelta_, dense_points );
747 stateProperty_[ queryVertex_ ] = NULL;
750 for (std::size_t i = 0 ; i < dense_points.size() ; ++i)
753 removeFromRepresentatives( dense_points[i], representativesProperty_[dense_points[i]] );
755 calculateRepresentative( dense_points[i] );
758 std::set<SparseVertex> interfaceRepresentatives;
760 for (std::size_t i = 0 ; i < dense_points.size(); ++i)
763 SparseVertex rep = representativesProperty_[dense_points[i]];
765 getInterfaceNeighborRepresentatives( dense_points[i], interfaceRepresentatives );
767 removeFromRepresentatives( dense_points[i], rep );
769 addToRepresentatives( dense_points[i], rep, interfaceRepresentatives );
776 std::vector<SparseVertex> graphNeighborhood;
777 getSparseNeighbors(stateProperty_[q], graphNeighborhood);
780 for (std::size_t i = 0; i < graphNeighborhood.size(); ++i)
781 if (si_->checkMotion(stateProperty_[q], sparseStateProperty_[graphNeighborhood[i]]))
784 representativesProperty_[q] = graphNeighborhood[i];
793 if (oreps.size() == 0)
796 bool new_insert = nonInterfaceListsProperty_[rep].insert(q).second;
807 assert(rep == representativesProperty_[q]);
808 bool new_insert = interfaceListsProperty_[rep].interfaceHash[v].insert(q).second;
818 nonInterfaceListsProperty_[rep].erase(q);
821 foreach (
SparseVertex vpp, interfaceListsProperty_[rep].interfaceHash | boost::adaptors::map_keys)
824 interfaceListsProperty_[rep].interfaceHash[vpp].erase( q );
830 foreach(
SparseVertex cvpp, boost::adjacent_vertices( v, s_ ) )
832 if( !boost::edge( cvpp, vp, s_ ).second )
833 VPPs.push_back( cvpp );
839 foreach(
SparseVertex cx, boost::adjacent_vertices( vpp, s_ ) )
840 if( boost::edge( cx, v, s_ ).second && !boost::edge( cx, vp, s_ ).second )
841 if (interfaceListsProperty_[vpp].interfaceHash[cx].size() > 0)
848 interfaceRepresentatives.clear();
853 foreach(
DenseVertex n, boost::adjacent_vertices( q, g_ ) )
860 if (distanceFunction( q, n ) < denseDelta_)
862 interfaceRepresentatives.insert(orep);
868 interfaceNeighborhood.clear();
874 foreach(
DenseVertex n, boost::adjacent_vertices( q, g_ ) )
876 if( representativesProperty_[n] != rep )
878 if( distanceFunction( q, n ) < denseDelta_ )
880 interfaceNeighborhood.push_back( n );
885 boost::mutex::scoped_lock _(graphMutex_);
887 boost::vector_property_map<SparseVertex> prev(boost::num_vertices(s_));
891 boost::astar_search(s_, start,
893 boost::predecessor_map(prev).
894 visitor(AStarGoalVisitor<SparseVertex>(goal)));
896 catch (AStarFoundGoal&)
900 if (prev[goal] == goal)
901 throw Exception(name_,
"Could not find solution path");
906 for (
SparseVertex pos = goal; prev[pos] != pos; pos = prev[pos])
907 p->
append(sparseStateProperty_[pos]);
908 p->
append(sparseStateProperty_[start]);
919 boost::vector_property_map<DenseVertex> prev(boost::num_vertices(g_));
923 boost::astar_search(g_, start,
925 boost::predecessor_map(prev).
926 visitor(AStarGoalVisitor<DenseVertex>(goal)));
928 catch (AStarFoundGoal&)
932 if (prev[goal] == goal)
933 OMPL_WARN(
"%s: No dense path was found?", getName().c_str());
936 for (
DenseVertex pos = goal; prev[pos] != pos; pos = prev[pos])
937 path.push_front( stateProperty_[pos] );
938 path.push_front( stateProperty_[start] );
944 Planner::getPlannerData(data);
947 for (std::size_t i = 0; i < startM_.size(); ++i)
950 for (std::size_t i = 0; i < goalM_.size(); ++i)
954 foreach (
const SparseEdge e, boost::edges(s_))
968 if (boost::out_degree( n, s_ ) == 0)
970 data.
properties[
"iterations INTEGER"] = boost::lexical_cast<std::string>(iterations_);
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
double getDenseDeltaFraction(void) const
Retrieve the dense graph interface support delta fraction.
PlannerTerminationCondition plannerOrTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
Combine two termination conditions into one. If either termination condition returns true...
SPARS(const base::SpaceInformationPtr &si)
Constructor.
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
PathSimplifierPtr psimp_
A path simplifier used to simplify dense paths added to S.
The planner failed to find a solution.
GoalType recognizedGoal
The type of goal specification the planner can use.
void freeMemory(void)
Free all the memory allocated by the planner.
DenseVertex getInterfaceNeighbor(DenseVertex q, SparseVertex rep)
Get the first neighbor of q who has representative rep and is within denseDelta_. ...
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 base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Abstract definition of goals.
void setDenseDeltaFraction(double d)
Set the delta fraction for interface detection. If two nodes in the dense graph are more than a delta...
unsigned getMaxFailures(void) const
Retrieve the maximum consecutive failure limit.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
double distanceFunction(const DenseVertex a, const DenseVertex b) const
Compute distance between two milestones (this is simply distance between the states of the milestones...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
bool checkAddPath(DenseVertex q, const std::vector< DenseVertex > &neigh)
Checks for adding an entire dense path to the Sparse Roadmap.
boost::graph_traits< SpannerGraph >::edge_descriptor SparseEdge
An edge in the sparse roadmap that is constructed.
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added...
void removeFromRepresentatives(DenseVertex q, SparseVertex rep)
Removes the node from its representative's lists.
void getInterfaceNeighborRepresentatives(DenseVertex q, std::set< SparseVertex > &interfaceRepresentatives)
Gets the representatives of all interfaces that q supports.
void clearQuery(void)
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition permits, construct the spanner graph.
bool reachedTerminationCriterion(void) const
Returns true if we have reached the iteration failures limit, maxFailures_ or if a solution was added...
void computeVPP(DenseVertex v, DenseVertex vp, std::vector< SparseVertex > &VPPs)
Computes all nodes which qualify as a candidate v" for v and vp.
bool checkAddCoverage(const base::State *lastState, const std::vector< SparseVertex > &neigh)
Checks the latest dense sample for the coverage property, and adds appropriately. ...
void setMaxFailures(unsigned int m)
Set the maximum consecutive failures to augment the spanner before termination. In general...
void calculateRepresentative(DenseVertex q)
Calculates the representative for a dense sample.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
void updateRepresentatives(SparseVertex v)
Automatically updates the representatives of all dense samplse within sparseDelta_ of v...
void setStretchFactor(double t)
Set the roadmap spanner stretch factor. This value represents a multiplicative upper bound on path qu...
Invalid start state or no start state specified.
void addToRepresentatives(DenseVertex q, SparseVertex rep, const std::set< SparseVertex > &oreps)
Adds a dense sample to the appropriate lists of its representative.
Abstract definition of a goal region that can be sampled.
DenseVertex addMilestone(base::State *state)
Construct a milestone for a given state (state) and store it in the nearest neighbors data structure...
void getInterfaceNeighborhood(DenseVertex q, std::vector< DenseVertex > &interfaceNeighborhood)
Gets the neighbors of q who help it support an interface.
boost::graph_traits< SpannerGraph >::vertex_descriptor SparseVertex
A vertex in the sparse roadmap that is constructed.
The goal is of a type that a planner does not recognize.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
void checkQueryStateInitialization(void)
Check that the query vertex is initialized (used for internal nearest neighbor searches) ...
void connectDensePoints(DenseVertex v, DenseVertex vp)
Connects points in the dense graph.
boost::graph_traits< DenseGraph >::vertex_descriptor DenseVertex
A vertex in DenseGraph.
void computeDensePath(const DenseVertex start, const DenseVertex goal, DensePath &path) const
Constructs the dense path between the start and goal vertices (if connected)
The planner found an exact solution.
void filterVisibleNeighbors(base::State *inState, const std::vector< SparseVertex > &graphNeighborhood, std::vector< SparseVertex > &visibleNeighborhood) const
Get the visible neighbors.
double getStretchFactor(void) const
Retrieve the spanner's set stretch factor.
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".
bool haveSolution(const std::vector< DenseVertex > &start, const std::vector< DenseVertex > &goal, base::PathPtr &solution)
Check if there exists a solution, i.e., there exists a pair of milestones such that the first is in s...
void reverse(void)
Reverse the path.
A class to store the exit status of Planner::solve()
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...
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...
This class contains routines that attempt to simplify geometric paths.
bool reachedFailureLimit(void) const
Returns true if we have reached the iteration failures limit, maxFailures_.
base::PathPtr constructSolution(const SparseVertex start, const SparseVertex goal) const
Given two milestones from the same connected component, construct a path connecting them and set it a...
virtual bool couldSample(void) const
Return true if samples could be generated by this sampler at some point in the future. By default this is equivalent to canSample(), but for GoalLazySamples, this call also reflects the fact that a sampling thread is active and although no samples are produced yet, some may become available at some point in the future.
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...
Definition of an abstract state.
virtual unsigned int maxSampleCount(void) const =0
Return the maximum number of samples that can be asked for before repeating.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
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 ...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
The exception type for ompl.
virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
Set the problem definition for the planner. The problem needs to be set before calling solve()...
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.
Make the minimal number of connections required to ensure asymptotic optimality.
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
bool checkAddConnectivity(const base::State *lastState, const std::vector< SparseVertex > &neigh)
Checks the latest dense sample for connectivity, and adds appropriately.
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
unsigned int milestoneCount(void) const
Returns the number of milestones added to D.
void connectSparsePoints(SparseVertex v, SparseVertex vp)
Convenience function for creating an edge in the Spanner Roadmap.
void resetFailures(void)
A reset function for resetting the failures count.
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 of a geometric path.
double averageValence(void) const
Returns the average valence of the spanner graph.
double getSparseDeltaFraction(void) const
Retrieve the sparse graph visibility range delta fraction.
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...
SpaceInformationPtr si_
The space information for which planning is done.
double sparseDistanceFunction(const SparseVertex a, const SparseVertex b) const
Compute distance between two nodes in the sparse roadmap spanner.
bool addPathToSpanner(const DensePath &p, SparseVertex vp, SparseVertex vpp)
Method for actually adding a dense path to the Roadmap Spanner, S.
std::deque< base::State * > DensePath
Internal representation of a dense path.
GuardType
Enumeration which specifies the reason a guard is added to the spanner.
void setSparseDeltaFraction(double d)
Set the delta fraction for connection distance on the sparse spanner. This value represents the visib...
DenseVertex addSample(base::State *workState, const base::PlannerTerminationCondition &ptc)
Attempt to add a single sample to the roadmap.
bool sameComponent(SparseVertex m1, SparseVertex m2)
Check that two vertices are in the same connected component.
A boost shared pointer wrapper for ompl::base::Path.
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
virtual ~SPARS(void)
Destructor.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.