37 #ifndef OMPL_GEOMETRIC_PLANNERS_PRM_PRM_
38 #define OMPL_GEOMETRIC_PLANNERS_PRM_PRM_
40 #include "ompl/geometric/planners/PlannerIncludes.h"
41 #include "ompl/datastructures/NearestNeighbors.h"
42 #include <boost/graph/graph_traits.hpp>
43 #include <boost/graph/adjacency_list.hpp>
44 #include <boost/pending/disjoint_sets.hpp>
45 #include <boost/function.hpp>
46 #include <boost/thread.hpp>
57 OMPL_CLASS_FORWARD(OptimizationObjective);
88 typedef boost::vertex_property_tag kind;
92 typedef boost::vertex_property_tag kind;
96 typedef boost::vertex_property_tag kind;
100 typedef boost::vertex_property_tag kind;
104 typedef boost::edge_property_tag kind;
122 typedef boost::adjacency_list <
123 boost::vecS, boost::vecS, boost::undirectedS,
128 boost::property < boost::vertex_predecessor_t,
unsigned long int,
129 boost::property < boost::vertex_rank_t, unsigned long int > > > > > >,
130 boost::property < boost::edge_weight_t,
base::Cost,
131 boost::property < boost::edge_index_t,
unsigned int,
132 boost::property < edge_flags_t, unsigned int > > >
135 typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
136 typedef boost::graph_traits<Graph>::edge_descriptor Edge;
138 typedef boost::shared_ptr< NearestNeighbors<Vertex> > RoadmapNeighbors;
146 typedef boost::function<std::vector<Vertex>&(
const Vertex)>
251 virtual void clear(
void);
254 template<
template<
typename T>
class NN>
257 nn_.reset(
new NN<Vertex>());
264 virtual void setup(
void);
266 const Graph& getRoadmap(
void)
const
280 return boost::num_vertices(
g_);
283 const RoadmapNeighbors& getNearestNeighbors(
void)
353 boost::property_map<
Graph,
357 boost::property_map<
Graph,
367 boost::disjoint_sets<
368 boost::property_map<Graph, boost::vertex_rank_t>::type,
369 boost::property_map<Graph, boost::vertex_predecessor_t>::type >
PRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
Constructor.
unsigned int milestoneCount(void) const
Compute distance between two milestones (this is simply distance between the states of the milestones...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
std::vector< Vertex > goalM_
Array of goal milestones.
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
base::ValidStateSamplerPtr sampler_
Sampler user for generating valid samples in the state space.
bool addedNewSolution(void) const
Returns the value of the addedSolution_ member.
boost::function< std::vector< Vertex > &(const Vertex)> ConnectionStrategy
A function returning the milestones that should be attempted to connect to.
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
A boost shared pointer wrapper for ompl::base::ValidStateSampler.
void freeMemory(void)
Free all the memory allocated by the planner.
double distanceFunction(const Vertex a, const Vertex b) const
Compute distance between two milestones (this is simply distance between the states of the milestones...
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
void setNearestNeighbors(void)
Set a different nearest neighbors datastructure.
A boost shared pointer wrapper for ompl::base::StateSampler.
virtual void expandRoadmap(double expandTime)
Attempt to connect disjoint components in the roadmap using random bouncing motions (the PRM expansio...
ConnectionStrategy connectionStrategy_
Function that returns the milestones to attempt connections with.
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...
void setConnectionFilter(const ConnectionFilter &connectionFilter)
Set the function that can reject a milestone connection.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
boost::mutex graphMutex_
Mutex to guard access to the Graph member (g_)
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.
virtual void growRoadmap(double growTime)
If the user desires, the roadmap can be improved for the given time (seconds). The solve() method wil...
bool isSetup(void) const
Check if setup() was called for this planner.
unsigned int maxEdgeID_
Maximum unique id number used so for for edges.
boost::property_map< Graph, boost::edge_weight_t >::type weightProperty_
Access to the weights of each Edge.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. Grows a roadmap using constructRoadmap(). This function can be called multiple times on the same problem, without calling clear() in between. This allows the planner to continue work for more time on an unsolved problem, for example. Start and goal states from the currently specified ProblemDefinition are cached. This means that between calls to solve(), input states are only added, not removed. When using PRM as a multi-query planner, the input states should be however cleared, without clearing the roadmap itself. This can be done using the clearQuery() function.
boost::property_map< Graph, vertex_successful_connection_attempts_t >::type successfulConnectionAttemptsProperty_
Access to the number of successful connection attempts for a vertex.
std::vector< Vertex > startM_
Array of start milestones.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
bool starStrategy_
Flag indicating whether the default connection strategy is the Star strategy.
Base class for a planner.
virtual Vertex addMilestone(base::State *state)
Construct a milestone for a given state (state), store it in the nearest neighbors data structure and...
boost::property_map< Graph, boost::edge_index_t >::type edgeIDProperty_
Access to the indices of each Edge.
virtual base::PathPtr constructSolution(const Vertex &start, const Vertex &goal)
Given two milestones from the same connected component, construct a path connecting them and set it a...
bool addedSolution_
A flag indicating that a solution has been added during solve()
base::StateSamplerPtr simpleSampler_
Sampler user for generating random in the state space.
A class to store the exit status of Planner::solve()
Definition of an abstract state.
void setConnectionStrategy(const ConnectionStrategy &connectionStrategy)
Set the connection strategy function that specifies the milestones that connection attempts will be m...
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
bool userSetConnectionStrategy_
Flag indicating whether the employed connection strategy was set by the user (or defaults are assumed...
Graph g_
Connectivity graph.
A boost shared pointer wrapper for ompl::base::OptimizationObjective.
void clearQuery(void)
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
bool haveSolution(const std::vector< Vertex > &starts, const std::vector< Vertex > &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...
void uniteComponents(Vertex m1, Vertex m2)
Make two milestones (m1 and m2) be part of the same connected component. The component with fewer ele...
Probabilistic RoadMap planner.
ConnectionFilter connectionFilter_
Function that can reject a milestone connection.
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors...
base::OptimizationObjectivePtr opt_
Objective cost function for PRM graph edges.
virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
Set the problem definition for the planner. The problem needs to be set before calling solve()...
virtual base::PathPtr constructGeometricPath(const boost::vector_property_map< Vertex > &prev, const Vertex &start, const Vertex &goal)
Given a solution represented as a vector of predecesors in the roadmap, construct a geometric path...
SpaceInformationPtr si_
The space information for which planning is done.
RNG rng_
Random number generator.
virtual void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition allows, this function will construct the roadmap (using growRoadmap()...
RoadmapNeighbors nn_
Nearest neighbors data structure.
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, boost::property< vertex_state_t, base::State *, boost::property< vertex_total_connection_attempts_t, unsigned int, boost::property< vertex_successful_connection_attempts_t, unsigned int, boost::property< vertex_flags_t, unsigned int, boost::property< boost::vertex_predecessor_t, unsigned long int, boost::property< boost::vertex_rank_t, unsigned long int > > > > > >, boost::property< boost::edge_weight_t, base::Cost, boost::property< boost::edge_index_t, unsigned int, boost::property< edge_flags_t, unsigned int > > > > Graph
The underlying roadmap graph.
boost::function< bool(const Vertex &, const Vertex &)> ConnectionFilter
A function that can reject connections.
A boost shared pointer wrapper for ompl::base::Path.
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::property_map< Graph, vertex_total_connection_attempts_t >::type totalConnectionAttemptsProperty_
Access to the number of total connection attempts for a vertex.
base::Cost costHeuristic(Vertex u, Vertex v) const
Given two vertices, returns a heuristic on the cost of the path connecting them. This method wraps Op...