50 namespace ompl
118 vertex_total_connection_attempts_t, unsigned long int,
119 boost::property<vertex_successful_connection_attempts_t, unsigned long int,
147 ~PRM() override;
166 connectionStrategy_ = connectionStrategy;
167 userSetConnectionStrategy_ = true;
172 void setMaxNearestNeighbors(unsigned int k);
189 connectionFilter_ = connectionFilter;
202 void growRoadmap(double growTime);
212 void expandRoadmap(double expandTime);
238 void clearQuery();
240 void clear() override;
247 OMPL_WARN("Calling setNearestNeighbors will clear all states.");
249 nn_ = std::make_shared<NN<Vertex>>();
250 if (!userSetConnectionStrategy_)
252 if (isSetup())
256 void setup() override;
258 const Graph &getRoadmap() const
260 return g_;
266 return boost::num_vertices(g_);
272 return boost::num_edges(g_);
275 const RoadmapNeighbors &getNearestNeighbors()
277 return nn_;
282 void freeMemory();
291 void uniteComponents(Vertex m1, Vertex m2);
295 bool sameComponent(Vertex m1, Vertex m2);
305 void expandRoadmap(const base::PlannerTerminationCondition &ptc, std::vector<base::State *> &workStates);
313 bool maybeConstructSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals,
314 base::PathPtr &solution);
318 ompl::base::Cost constructApproximateSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals, base::PathPtr &solution);
321 bool addedNewSolution() const;
342 return std::to_string(iterations_);
350 return std::to_string(milestoneCount());
354 return std::to_string(edgeCount());
382 boost::property_map<Graph, vertex_total_connection_attempts_t>::type totalConnectionAttemptsProperty_;
PRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
unsigned long int milestoneCount() const
Return the number of milestones currently in the graph.
double distanceFunction(const Vertex a, const Vertex b) const
Compute distance between two milestones (this is simply distance between the states of the milestones...
unsigned long int iterations_
Number of iterations the algorithm performed.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
std::vector< Vertex > goalM_
Array of goal milestones.
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
Sampler user for generating valid samples in the state space.
A shared pointer wrapper for ompl::base::ProblemDefinition.
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.
A shared pointer wrapper for ompl::base::ValidStateSampler.
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...
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
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.
Mutex to guard access to the Graph member (g_)
A shared pointer wrapper for ompl::base::StateSampler.
void expandRoadmap(double expandTime)
Attempt to connect disjoint components in the roadmap using random bouncing motions (the PRM expansio...
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...
ompl::base::Cost constructApproximateSolution(const std::vector< Vertex > &starts, const std::vector< Vertex > &goals, base::PathPtr &solution)
(Assuming that there is always an approximate solution), finds an approximate solution.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
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...
void growRoadmap(double growTime)
If the user desires, the roadmap can be improved for the given time (seconds). The solve() method wil...
A flag indicating that a solution has been added during solve()
Set a different nearest neighbors datastructure.
boost::property_map< Graph, boost::edge_weight_t >::type weightProperty_
Access to the weights of each Edge.
std::shared_ptr< NearestNeighbors< Vertex > > RoadmapNeighbors
A nearest neighbors data structure for roadmap vertices.
std::function< bool(const Vertex &, const Vertex &)> ConnectionFilter
A function that can reject connections.
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, boost::property< vertex_state_t, base::State *, boost::property< vertex_total_connection_attempts_t, unsigned long int, boost::property< vertex_successful_connection_attempts_t, unsigned long 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 > > Graph
The underlying roadmap graph.
bool addedNewSolution() const
Returns the value of the addedNewSolution_ member.
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
boost::property_map< Graph, vertex_successful_connection_attempts_t >::type successfulConnectionAttemptsProperty_
Access to the number of successful connection attempts for a vertex.
boost::graph_traits< Graph >::edge_descriptor Edge
The type for an edge in the roadmap.
std::vector< Vertex > startM_
Array of start milestones.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Free all the memory allocated by the planner.
Flag indicating whether the default connection strategy is the Star strategy.
Vertex addMilestone(base::State *state)
Construct a milestone for a given state (state), store it in the nearest neighbors data structure and...
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...
std::function< const std::vector< Vertex > &(const Vertex)> ConnectionStrategy
A function returning the milestones that should be attempted to connect to.
Sampler user for generating random in the state space.
A class to store the exit status of Planner::solve()
A shared pointer wrapper for ompl::base::SpaceInformation.
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.
Flag indicating whether the employed connection strategy was set by the user (or defaults are assumed...
void setProblemDefinition(const base::ProblemDefinitionPtr &pdef) override
Set the problem definition for the planner. The problem needs to be set before calling solve()...
A shared pointer wrapper for ompl::base::OptimizationObjective.
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
void uniteComponents(Vertex m1, Vertex m2)
Make two milestones (m1 and m2) be part of the same connected component. The component with fewer ele...
bool isSetup() const
Check if setup() was called for this planner.
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...
Objective cost function for PRM graph edges.
bool maybeConstructSolution(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...
Best cost found so far by algorithm.
The space information for which planning is done.
boost::graph_traits< Graph >::vertex_descriptor Vertex
The type for a vertex in the roadmap.
unsigned long int edgeCount() const
Return the number of edges currently in the graph.
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition allows, this function will construct the roadmap (using growRoadmap()...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Nearest neighbors data structure.
A shared pointer wrapper for ompl::base::Path.
boost::property_map< Graph, vertex_total_connection_attempts_t >::type totalConnectionAttemptsProperty_
Access to the number of total connection attempts for a vertex.