All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
PRM.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan, James D. Marble, Ryan Luna */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_PRM_PRM_
38 #define OMPL_GEOMETRIC_PLANNERS_PRM_PRM_
39 
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>
47 #include <utility>
48 #include <vector>
49 #include <map>
50 
51 namespace ompl
52 {
53 
54  namespace base
55  {
56  // Forward declare for use in implementation
57  OMPL_CLASS_FORWARD(OptimizationObjective);
58  }
59 
60  namespace geometric
61  {
62 
83  class PRM : public base::Planner
84  {
85  public:
86 
87  struct vertex_state_t {
88  typedef boost::vertex_property_tag kind;
89  };
90 
92  typedef boost::vertex_property_tag kind;
93  };
94 
96  typedef boost::vertex_property_tag kind;
97  };
98 
99  struct vertex_flags_t {
100  typedef boost::vertex_property_tag kind;
101  };
102 
103  struct edge_flags_t {
104  typedef boost::edge_property_tag kind;
105  };
106 
122  typedef boost::adjacency_list <
123  boost::vecS, boost::vecS, boost::undirectedS,
124  boost::property < vertex_state_t, base::State*,
125  boost::property < vertex_total_connection_attempts_t, unsigned int,
126  boost::property < vertex_successful_connection_attempts_t, unsigned int,
127  boost::property < vertex_flags_t, unsigned int,
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 > > >
133  > Graph;
134 
135  typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
136  typedef boost::graph_traits<Graph>::edge_descriptor Edge;
137 
138  typedef boost::shared_ptr< NearestNeighbors<Vertex> > RoadmapNeighbors;
139 
146  typedef boost::function<std::vector<Vertex>&(const Vertex)>
148 
154  typedef boost::function<bool(const Vertex&, const Vertex&)> ConnectionFilter;
155 
157  PRM(const base::SpaceInformationPtr &si, bool starStrategy = false);
158 
159  virtual ~PRM(void);
160 
161  virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef);
162 
176  void setConnectionStrategy(const ConnectionStrategy& connectionStrategy)
177  {
178  connectionStrategy_ = connectionStrategy;
180  }
184  void setMaxNearestNeighbors(unsigned int k);
185 
199  void setConnectionFilter(const ConnectionFilter& connectionFilter)
200  {
201  connectionFilter_ = connectionFilter;
202  }
203 
204  virtual void getPlannerData(base::PlannerData &data) const;
205 
208  virtual void constructRoadmap(const base::PlannerTerminationCondition &ptc);
209 
213  virtual void growRoadmap(double growTime);
214 
218  virtual void growRoadmap(const base::PlannerTerminationCondition &ptc);
219 
223  virtual void expandRoadmap(double expandTime);
224 
228  virtual void expandRoadmap(const base::PlannerTerminationCondition &ptc);
229 
244 
249  void clearQuery(void);
250 
251  virtual void clear(void);
252 
254  template<template<typename T> class NN>
256  {
257  nn_.reset(new NN<Vertex>());
259  connectionStrategy_.clear();
260  if (isSetup())
261  setup();
262  }
263 
264  virtual void setup(void);
265 
266  const Graph& getRoadmap(void) const
267  {
268  return g_;
269  }
270 
272  double distanceFunction(const Vertex a, const Vertex b) const
273  {
274  return si_->distance(stateProperty_[a], stateProperty_[b]);
275  }
276 
278  unsigned int milestoneCount(void) const
279  {
280  return boost::num_vertices(g_);
281  }
282 
283  const RoadmapNeighbors& getNearestNeighbors(void)
284  {
285  return nn_;
286  }
287 
288  protected:
289 
291  void freeMemory(void);
292 
295  virtual Vertex addMilestone(base::State *state);
296 
298  void uniteComponents(Vertex m1, Vertex m2);
299 
301  bool sameComponent(Vertex m1, Vertex m2);
302 
306  virtual void growRoadmap(const base::PlannerTerminationCondition &ptc, base::State *workState);
307 
311  virtual void expandRoadmap(const base::PlannerTerminationCondition &ptc, std::vector<base::State*> &workStates);
312 
315 
317  bool haveSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals, base::PathPtr &solution);
318 
320  bool addedNewSolution(void) const;
321 
323  virtual base::PathPtr constructSolution(const Vertex &start, const Vertex &goal);
324 
326  virtual base::PathPtr constructGeometricPath(const boost::vector_property_map<Vertex> &prev, const Vertex &start, const Vertex &goal);
327 
330 
333 
336 
338  RoadmapNeighbors nn_;
339 
342 
344  std::vector<Vertex> startM_;
345 
347  std::vector<Vertex> goalM_;
348 
350  boost::property_map<Graph, vertex_state_t>::type stateProperty_;
351 
353  boost::property_map<Graph,
355 
357  boost::property_map<Graph,
359 
361  boost::property_map<Graph, boost::edge_weight_t>::type weightProperty_;
362 
364  boost::property_map<Graph, boost::edge_index_t>::type edgeIDProperty_;
365 
367  boost::disjoint_sets<
368  boost::property_map<Graph, boost::vertex_rank_t>::type,
369  boost::property_map<Graph, boost::vertex_predecessor_t>::type >
371 
373  unsigned int maxEdgeID_;
374 
377 
380 
383 
386 
389 
391  mutable boost::mutex graphMutex_;
392 
395 
397  base::Cost costHeuristic(Vertex u, Vertex v) const;
398  };
399 
400  }
401 }
402 
403 #endif
PRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
Constructor.
Definition: PRM.cpp:74
unsigned int milestoneCount(void) const
Compute distance between two milestones (this is simply distance between the states of the milestones...
Definition: PRM.h:278
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
std::vector< Vertex > goalM_
Array of goal milestones.
Definition: PRM.h:347
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
Definition: PRM.cpp:290
base::ValidStateSamplerPtr sampler_
Sampler user for generating valid samples in the state space.
Definition: PRM.h:332
bool addedNewSolution(void) const
Returns the value of the addedSolution_ member.
Definition: PRM.cpp:352
boost::function< std::vector< Vertex > &(const Vertex)> ConnectionStrategy
A function returning the milestones that should be attempted to connect to.
Definition: PRM.h:147
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.
Definition: PRM.cpp:162
double distanceFunction(const Vertex a, const Vertex b) const
Compute distance between two milestones (this is simply distance between the states of the milestones...
Definition: PRM.h:272
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: PRM.cpp:150
void setNearestNeighbors(void)
Set a different nearest neighbors datastructure.
Definition: PRM.h:255
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...
Definition: PRM.cpp:169
ConnectionStrategy connectionStrategy_
Function that returns the milestones to attempt connections with.
Definition: PRM.h:376
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...
Definition: PRM.cpp:499
void setConnectionFilter(const ConnectionFilter &connectionFilter)
Set the function that can reject a milestone connection.
Definition: PRM.h:199
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_)
Definition: PRM.h:391
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.
Definition: PRM.h:370
virtual void growRoadmap(double growTime)
If the user desires, the roadmap can be improved for the given time (seconds). The solve() method wil...
Definition: PRM.cpp:250
bool isSetup(void) const
Check if setup() was called for this planner.
Definition: Planner.cpp:107
unsigned int maxEdgeID_
Maximum unique id number used so for for edges.
Definition: PRM.h:373
boost::property_map< Graph, boost::edge_weight_t >::type weightProperty_
Access to the weights of each Edge.
Definition: PRM.h:361
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PRM.cpp:100
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.
Definition: PRM.cpp:357
boost::property_map< Graph, vertex_successful_connection_attempts_t >::type successfulConnectionAttemptsProperty_
Access to the number of successful connection attempts for a vertex.
Definition: PRM.h:358
std::vector< Vertex > startM_
Array of start milestones.
Definition: PRM.h:344
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:54
bool starStrategy_
Flag indicating whether the default connection strategy is the Star strategy.
Definition: PRM.h:329
Base class for a planner.
Definition: Planner.h:227
virtual Vertex addMilestone(base::State *state)
Construct a milestone for a given state (state), store it in the nearest neighbors data structure and...
Definition: PRM.cpp:457
boost::property_map< Graph, boost::edge_index_t >::type edgeIDProperty_
Access to the indices of each Edge.
Definition: PRM.h:364
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...
Definition: PRM.cpp:504
bool addedSolution_
A flag indicating that a solution has been added during solve()
Definition: PRM.h:388
base::StateSamplerPtr simpleSampler_
Sampler user for generating random in the state space.
Definition: PRM.h:335
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
A boost shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
void setConnectionStrategy(const ConnectionStrategy &connectionStrategy)
Set the connection strategy function that specifies the milestones that connection attempts will be m...
Definition: PRM.h:176
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition: PRM.h:350
bool userSetConnectionStrategy_
Flag indicating whether the employed connection strategy was set by the user (or defaults are assumed...
Definition: PRM.h:382
Graph g_
Connectivity graph.
Definition: PRM.h:341
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 ...
Definition: PRM.cpp:143
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...
Definition: PRM.cpp:312
void uniteComponents(Vertex m1, Vertex m2)
Make two milestones (m1 and m2) be part of the same connected component. The component with fewer ele...
Definition: PRM.cpp:494
Probabilistic RoadMap planner.
Definition: PRM.h:83
ConnectionFilter connectionFilter_
Function that can reject a milestone connection.
Definition: PRM.h:379
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors...
Definition: PRM.cpp:127
base::OptimizationObjectivePtr opt_
Objective cost function for PRM graph edges.
Definition: PRM.h:394
virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
Set the problem definition for the planner. The problem needs to be set before calling solve()...
Definition: PRM.cpp:137
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...
Definition: PRM.cpp:533
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:392
RNG rng_
Random number generator.
Definition: PRM.h:385
virtual void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition allows, this function will construct the roadmap (using growRoadmap()...
Definition: PRM.cpp:430
RoadmapNeighbors nn_
Nearest neighbors data structure.
Definition: PRM.h:338
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.
Definition: PRM.h:133
boost::function< bool(const Vertex &, const Vertex &)> ConnectionFilter
A function that can reject connections.
Definition: PRM.h:154
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...
Definition: PRM.cpp:544
boost::property_map< Graph, vertex_total_connection_attempts_t >::type totalConnectionAttemptsProperty_
Access to the number of total connection attempts for a vertex.
Definition: PRM.h:354
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...
Definition: PRM.cpp:573