37 #ifndef OMPL_GEOMETRIC_PLANNERS_SPARS_TWO_
38 #define OMPL_GEOMETRIC_PLANNERS_SPARS_TWO_
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>
98 typedef std::pair< VertexIndexType, VertexIndexType >
VertexPair;
120 d_(std::numeric_limits<double>::infinity())
134 si->freeState(pointB_);
144 si->freeState(sigmaB_);
147 d_ = std::numeric_limits<double>::infinity();
169 si->copyState(pointB_, p);
171 pointB_ = si->cloneState(p);
173 si->copyState(sigmaB_, s);
175 sigmaB_ = si->cloneState(s);
182 typedef boost::unordered_map< VertexPair, InterfaceData, boost::hash< VertexPair > >
InterfaceHash;
195 typedef boost::vertex_property_tag kind;
199 typedef boost::vertex_property_tag kind;
203 typedef boost::vertex_property_tag kind;
221 typedef boost::adjacency_list <
222 boost::vecS, boost::vecS, boost::undirectedS,
227 boost::property < vertex_interface_data_t, InterfaceHashStruct > > > > >,
228 boost::property < boost::edge_weight_t, double >
232 typedef boost::graph_traits<Graph>::vertex_descriptor
Vertex;
235 typedef boost::graph_traits<Graph>::edge_descriptor
Edge;
324 virtual void clear(
void);
327 template<
template<
typename T>
class NN>
330 nn_.reset(
new NN< Vertex >());
335 virtual void setup(
void);
346 return boost::num_vertices(
g_);
451 boost::shared_ptr< NearestNeighbors<Vertex> >
nn_;
496 boost::disjoint_sets<
497 boost::property_map<Graph, boost::vertex_rank_t>::type,
498 boost::property_map<Graph, boost::vertex_predecessor_t>::type >
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...
double sparseDeltaFraction_
Maximum visibility range for nodes in the graph as a fraction of maximum extent.
virtual ~SPARStwo(void)
Destructor.
Graph g_
Connectivity graph.
base::State * pointA_
States which lie inside the visibility region of a vertex and support an interface.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
base::ValidStateSamplerPtr sampler_
Sampler user for generating valid samples in the state space.
bool haveSolution(const std::vector< Vertex > &start, const std::vector< Vertex > &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 boost shared pointer wrapper for ompl::base::ProblemDefinition.
double distanceFunction(const Vertex a, const Vertex b) const
Compute distance between two milestones (this is simply distance between the states of the milestones...
void setDenseDeltaFraction(double d)
Sets interface support tolerance as a fraction of max. extent.
void findCloseRepresentatives(base::State *workArea, 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.
A boost shared pointer wrapper for ompl::base::ValidStateSampler.
void findGraphNeighbors(base::State *st, std::vector< Vertex > &graphNeighborhood, std::vector< Vertex > &visibleNeighborhood)
Finds visible nodes in the graph near st.
unsigned long int VertexIndexType
The type used internally for representing vertex IDs.
A boost shared pointer wrapper for ompl::base::StateSampler.
double denseDeltaFraction_
Maximum range for allowing two samples to support an interface as a fraction of maximum extent...
long unsigned int iterations_
A counter for the number of iterations of the algorithm.
void setStretchFactor(double t)
Sets the stretch factor.
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.
SPARStwo(const base::SpaceInformationPtr &si)
Constructor.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Vertex findGraphRepresentative(base::State *st)
Finds the representative of the input state, st.
boost::property_map< Graph, boost::edge_weight_t >::type weightProperty_
Access to the weights of each Edge.
std::vector< Vertex > startM_
Array of start milestones.
bool isSetup(void) const
Check if setup() was called for this planner.
bool reachedFailureLimit(void) const
Returns whether we have reached the iteration failures limit, maxFailures_.
long unsigned int getIterations(void) const
Get the number of iterations the algorithm performed.
double getDenseDeltaFraction() const
Retrieve the dense graph interface support delta.
std::vector< Vertex > goalM_
Array of goal milestones.
unsigned int milestoneCount(void) const
Get the number of vertices in the sparse roadmap.
void setSparseDeltaFraction(double D)
Sets vertex visibility range as a fraction of max. extent.
void setNearestNeighbors(void)
Set a different nearest neighbors datastructure.
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...
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 getStretchFactor() const
Retrieve the spanner's set stretch factor.
base::State * sigmaA_
States which lie just outside the visibility region of a vertex and support an interface.
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.
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()...
void resetFailures(void)
A reset function for resetting the failures count.
double denseDelta_
Maximum range for allowing two samples to support an interface.
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_interface_data_t, InterfaceHashStruct > > > > >, boost::property< boost::edge_weight_t, double > > Graph
The underlying roadmap graph.
bool checkAddConnectivity(const base::State *qNew, std::vector< Vertex > &visibleNeighborhood)
Checks to see if the sample needs to be added to ensure connectivity.
std::pair< VertexIndexType, VertexIndexType > VertexPair
Pair of vertices which support an interface.
void computeVPP(Vertex v, Vertex vp, std::vector< Vertex > &VPPs)
Computes all nodes which qualify as a candidate v" for v and vp.
unsigned int getMaxFailures() const
Retrieve the maximum consecutive failure limit.
bool addedSolution_
A flag indicating that a solution has been added during solve()
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).
Vertex addGuard(base::State *state, GuardType type)
Construct a guard for a given state (state) and store it in the nearest neighbors data structure...
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition permits, construct the spanner graph.
const Graph & getRoadmap(void) const
Retrieve the computed roadmap.
Base class for a planner.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
SPArse Roadmap Spanner Version 2.0
Interface information storage class, which does bookkeeping for criterion four.
base::PathPtr constructSolution(const Vertex start, const Vertex goal) const
Given two milestones from the same connected component, construct a path connecting them and set it a...
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.
double sparseDelta_
Maximum visibility range for nodes in the graph.
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...
void approachGraph(Vertex v)
Approaches the graph from a given vertex.
A class to store the exit status of Planner::solve()
void clear(const base::SpaceInformationPtr &si)
Clears the given interface data.
Definition of an abstract state.
PathSimplifierPtr psimp_
A path simplifier used to simplify dense paths added to the graph.
void setMaxFailures(unsigned int m)
Sets the maximum failures until termination.
bool reachedTerminationCriterion(void) const
Returns true if we have reached the iteration failures limit, maxFailures_ or if a solution was added...
unsigned int maxFailures_
The number of consecutive failures to add to the graph before termination.
A boost shared pointer wrapper for ompl::geometric::PathSimplifier.
void checkQueryStateInitialization(void)
Check that the query vertex is initialized (used for internal nearest neighbor searches) ...
boost::graph_traits< Graph >::edge_descriptor Edge
Edge in Graph.
InterfaceData & getData(Vertex v, Vertex vp, Vertex vpp)
Retrieves the Vertex data associated with v,vp,vpp.
boost::unordered_map< VertexPair, InterfaceData, boost::hash< VertexPair > > InterfaceHash
the hash which maps pairs of neighbor points to pairs of states
GuardType
Enumeration which specifies the reason a guard is added to the spanner.
base::StateSamplerPtr simpleSampler_
Sampler user for generating random in the state space.
void connectGuards(Vertex v, Vertex vp)
Connect two guards in the roadmap.
boost::property_map< Graph, vertex_color_t >::type colorProperty_
Access to the colors for the vertices.
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
boost::graph_traits< Graph >::vertex_descriptor Vertex
Vertex in Graph.
void freeMemory(void)
Free all the memory allocated by the planner.
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. ...
double d_
Last known distance between the two interfaces supported by points_ and sigmas.
double stretchFactor_
Stretch Factor as per graph spanner literature (multiplicative bound on path quality) ...
void clearQuery(void)
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
bool checkAddPath(Vertex v)
Checks vertex v for short paths through its region and adds when appropriate.
SpaceInformationPtr si_
The space information for which planning is done.
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
void abandonLists(base::State *st)
When a new guard is added at state st, finds all guards who must abandon their interface information ...
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
boost::mutex graphMutex_
Mutex to guard access to the Graph member (g_)
VertexPair index(Vertex vp, Vertex vpp)
Rectifies indexing order for accessing the vertex data.
Vertex queryVertex_
Vertex for performing nearest neighbor queries.
RNG rng_
Random number generator.
InterfaceData(void)
Constructor.
boost::property_map< Graph, vertex_interface_data_t >::type interfaceDataProperty_
Access to the interface pair information for the vertices.
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".
double getSparseDeltaFraction() const
Retrieve the sparse graph visibility range delta.
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).
unsigned int nearSamplePoints_
Number of sample points to use when trying to detect interfaces.
A boost shared pointer wrapper for ompl::base::Path.
boost::shared_ptr< NearestNeighbors< Vertex > > nn_
Nearest neighbors data structure.