37 #ifndef OMPL_GEOMETRIC_PLANNERS_SPARSE_ROADMAP_SPANNER_
38 #define OMPL_GEOMETRIC_PLANNERS_SPARSE_ROADMAP_SPANNER_
40 #include "ompl/geometric/planners/PlannerIncludes.h"
41 #include "ompl/datastructures/NearestNeighbors.h"
42 #include "ompl/geometric/PathSimplifier.h"
43 #include "ompl/util/Time.h"
45 #include <boost/range/adaptor/map.hpp>
46 #include <boost/unordered_map.hpp>
47 #include <boost/graph/graph_traits.hpp>
48 #include <boost/graph/adjacency_list.hpp>
49 #include <boost/pending/disjoint_sets.hpp>
50 #include <boost/function.hpp>
51 #include <boost/thread.hpp>
95 typedef boost::vertex_property_tag kind;
99 typedef boost::vertex_property_tag kind;
103 typedef boost::vertex_property_tag kind;
107 typedef boost::vertex_property_tag kind;
111 typedef boost::vertex_property_tag kind;
118 typedef boost::unordered_map<VertexIndexType, std::set<VertexIndexType>, boost::hash<VertexIndexType> >
InterfaceHash;
145 typedef boost::adjacency_list <
146 boost::vecS, boost::vecS, boost::undirectedS,
151 boost::property < vertex_list_t, std::set<VertexIndexType>,
152 boost::property < vertex_interface_list_t, InterfaceHashStruct > > > > > >,
153 boost::property < boost::edge_weight_t, double >
157 typedef boost::graph_traits<SpannerGraph>::vertex_descriptor
SparseVertex;
160 typedef boost::graph_traits<SpannerGraph>::edge_descriptor
SparseEdge;
180 typedef boost::adjacency_list <
181 boost::vecS, boost::vecS, boost::undirectedS,
185 boost::property < vertex_representative_t, SparseVertex > > > >,
186 boost::property < boost::edge_weight_t, double >
190 typedef boost::graph_traits<DenseGraph>::vertex_descriptor
DenseVertex;
193 typedef boost::graph_traits<DenseGraph>::edge_descriptor
DenseEdge;
234 virtual void clear(
void);
241 template<
template<
typename T>
class NN>
244 nn_.reset(
new NN<DenseVertex>());
254 template<
template<
typename T>
class NN>
257 snn_.reset(
new NN<SparseVertex>());
324 virtual void setup(
void);
343 return boost::num_vertices(
g_);
349 return boost::num_vertices(
s_);
391 bool checkAddInterface(
const std::vector<DenseVertex>& graphNeighborhood,
const std::vector<DenseVertex>& visibleNeighborhood,
DenseVertex q);
526 boost::disjoint_sets<
527 boost::property_map<SpannerGraph, boost::vertex_rank_t>::type,
528 boost::property_map<SpannerGraph, boost::vertex_predecessor_t>::type >
double stretchFactor_
The stretch factor in terms of graph spanners for SPARS to check against.
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.
base::StateSamplerPtr simpleSampler_
Sampler user for generating random in the state space.
unsigned long int VertexIndexType
The type used internally for representing vertex IDs.
SPARS(const base::SpaceInformationPtr &si)
Constructor.
base::ValidStateSamplerPtr sampler_
Sampler user for generating valid samples in the state space.
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, boost::property< vertex_state_t, base::State *, boost::property< boost::vertex_predecessor_t, VertexIndexType, boost::property< boost::vertex_rank_t, VertexIndexType, boost::property< vertex_representative_t, SparseVertex > > > >, boost::property< boost::edge_weight_t, double > > DenseGraph
The underlying roadmap graph.
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
PathSimplifierPtr psimp_
A path simplifier used to simplify dense paths added to S.
RNG rng_
Random number generator.
A boost shared pointer wrapper for ompl::base::ValidStateSampler.
long unsigned int iterations_
A counter for the number of iterations of the algorithm.
void freeMemory(void)
Free all the memory allocated by the planner.
boost::property_map< SpannerGraph, vertex_interface_list_t >::type interfaceListsProperty_
Access to the interface-supporting vertice hashes of the sparse nodes.
DenseVertex getInterfaceNeighbor(DenseVertex q, SparseVertex rep)
Get the first neighbor of q who has representative rep and is within denseDelta_. ...
A boost shared pointer wrapper for ompl::base::StateSampler.
double denseDeltaFraction_
SPARS parameter for dense graph connection distance as a fraction of max. extent. ...
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...
void setDenseDeltaFraction(double d)
Set the delta fraction for interface detection. If two nodes in the dense graph are more than a delta...
double sparseDelta_
SPARS parameter for Sparse Roadmap connection distance.
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...
boost::function< std::vector< DenseVertex > &(const DenseVertex)> connectionStrategy_
Function that returns the milestones to attempt connections with.
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...
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.
bool isSetup(void) const
Check if setup() was called for this planner.
SparseNeighbors snn_
Nearest Neighbors structure for the sparse roadmap.
void clearQuery(void)
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
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...
const DenseGraph & getDenseGraph(void) const
Retrieve the underlying dense graph structure. This is built as a PRM* and asymptotically approximate...
void computeVPP(DenseVertex v, DenseVertex vp, std::vector< SparseVertex > &VPPs)
Computes all nodes which qualify as a candidate v" for v and vp.
unsigned int maxFailures_
The maximum number of failures before terminating the algorithm.
boost::property_map< SpannerGraph, vertex_list_t >::type nonInterfaceListsProperty_
Access to all non-interface supporting vertices of the sparse nodes.
bool checkAddCoverage(const base::State *lastState, const std::vector< SparseVertex > &neigh)
Checks the latest dense sample for the coverage property, and adds appropriately. ...
const SpannerGraph & getRoadmap(void) const
Retrieve the sparse roadmap structure. This is the structure which answers given queries, and has the desired property of asymptotic near-optimality.
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.
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...
void addToRepresentatives(DenseVertex q, SparseVertex rep, const std::set< SparseVertex > &oreps)
Adds a dense sample to the appropriate lists of its representative.
boost::mutex graphMutex_
Mutex to guard access to the graphs.
DenseVertex addMilestone(base::State *state)
Construct a milestone for a given state (state) and store it in the nearest neighbors data structure...
std::vector< SparseVertex > goalM_
Array of goal guards.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
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.
boost::disjoint_sets< boost::property_map< SpannerGraph, boost::vertex_rank_t >::type, boost::property_map< SpannerGraph, boost::vertex_predecessor_t >::type > sparseDJSets_
Data structure that maintains the connected components of S.
boost::property_map< DenseGraph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each DenseVertex.
boost::graph_traits< DenseGraph >::edge_descriptor DenseEdge
An edge in DenseGraph.
boost::property_map< SpannerGraph, vertex_state_t >::type sparseStateProperty_
Access to the internal base::State for each SparseVertex of S.
Base class for a planner.
boost::shared_ptr< NearestNeighbors< DenseVertex > > DenseNeighbors
Nearest neighbor structure which works over the DenseGraph.
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.
double sparseDeltaFraction_
SPARS parameter for Sparse Roadmap connection distance as a fraction of max. extent.
void computeDensePath(const DenseVertex start, const DenseVertex goal, DensePath &path) const
Constructs the dense path between the start and goal vertices (if connected)
boost::unordered_map< VertexIndexType, std::set< VertexIndexType >, boost::hash< VertexIndexType > > InterfaceHash
Hash for storing interface information.
void filterVisibleNeighbors(base::State *inState, const std::vector< SparseVertex > &graphNeighborhood, std::vector< SparseVertex > &visibleNeighborhood) const
Get the visible neighbors.
SPArse Roadmap Spanner technique.
long unsigned int getIterations(void) const
Get the number of iterations the algorithm performed.
boost::property_map< SpannerGraph, vertex_color_t >::type sparseColorProperty_
Access to draw colors for the SparseVertexs of S, to indicate addition type.
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...
A class to store the exit status of Planner::solve()
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...
Definition of an abstract state.
boost::shared_ptr< NearestNeighbors< SparseVertex > > SparseNeighbors
Nearest neighbor structure which works over the SpannerGraph.
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, boost::property< vertex_state_t, base::State *, boost::property< boost::vertex_predecessor_t, VertexIndexType, boost::property< boost::vertex_rank_t, VertexIndexType, boost::property< vertex_color_t, GuardType, boost::property< vertex_list_t, std::set< VertexIndexType >, boost::property< vertex_interface_list_t, InterfaceHashStruct > > > > > >, boost::property< boost::edge_weight_t, double > > SpannerGraph
The constructed roadmap spanner.
DenseVertex sparseQueryVertex_
DenseVertex for performing nearest neighbor queries on the SPARSE roadmap.
A boost shared pointer wrapper for ompl::geometric::PathSimplifier.
unsigned int consecutiveFailures_
A counter for the number of consecutive failed iterations of the algorithm.
virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
Set the problem definition for the planner. The problem needs to be set before calling solve()...
DenseVertex queryVertex_
Vertex for performing nearest neighbor queries on the DENSE graph.
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.
SpannerGraph s_
The sparse roadmap, S.
double denseDelta_
SPARS parameter for dense graph connection distance.
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
bool addedSolution_
A flag indicating that a solution has been added during solve()
bool checkAddConnectivity(const base::State *lastState, const std::vector< SparseVertex > &neigh)
Checks the latest dense sample for connectivity, and adds appropriately.
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 setDenseNeighbors(void)
Set a different nearest neighbors datastructure for the roadmap graph. This nearest neighbor structur...
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.
void setSparseNeighbors(void)
Set a different nearest neighbors datastructure for the spanner graph. This structure is stores only ...
std::vector< SparseVertex > startM_
Array of start guards.
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.
boost::property_map< DenseGraph, boost::edge_weight_t >::type weightProperty_
Access to the weights of each DenseEdge.
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.
PathGeometric geomPath_
Geometric Path variable used for smoothing out paths.
bool addPathToSpanner(const DensePath &p, SparseVertex vp, SparseVertex vpp)
Method for actually adding a dense path to the Roadmap Spanner, S.
boost::property_map< DenseGraph, vertex_representative_t >::type representativesProperty_
Access to the representatives of the Dense vertices.
unsigned int guardCount(void) const
Returns the number of guards added to 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.
DenseGraph g_
The dense graph, D.
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.
DenseNeighbors nn_
Nearest neighbors data structure.
A boost shared pointer wrapper for ompl::base::Path.
virtual ~SPARS(void)
Destructor.