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 
114  typedef boost::adjacency_list <
115  boost::vecS, boost::vecS, boost::undirectedS,
116  boost::property < vertex_state_t, base::State*,
117  boost::property < vertex_total_connection_attempts_t, unsigned long int,
118  boost::property < vertex_successful_connection_attempts_t, unsigned long int,
119  boost::property < boost::vertex_predecessor_t, unsigned long int,
120  boost::property < boost::vertex_rank_t, unsigned long int > > > > >,
121  boost::property < boost::edge_weight_t, base::Cost >
122  > Graph;
123 
125  typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
127  typedef boost::graph_traits<Graph>::edge_descriptor Edge;
128 
130  typedef boost::shared_ptr< NearestNeighbors<Vertex> > RoadmapNeighbors;
131 
134  typedef boost::function<const std::vector<Vertex>&(const Vertex)> ConnectionStrategy;
135 
141  typedef boost::function<bool(const Vertex&, const Vertex&)> ConnectionFilter;
142 
144  PRM(const base::SpaceInformationPtr &si, bool starStrategy = false);
145 
146  virtual ~PRM();
147 
148  virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef);
149 
163  void setConnectionStrategy(const ConnectionStrategy& connectionStrategy)
164  {
165  connectionStrategy_ = connectionStrategy;
167  }
171  void setMaxNearestNeighbors(unsigned int k);
172 
186  void setConnectionFilter(const ConnectionFilter& connectionFilter)
187  {
188  connectionFilter_ = connectionFilter;
189  }
190 
191  virtual void getPlannerData(base::PlannerData &data) const;
192 
196 
200  void growRoadmap(double growTime);
201 
206 
210  void expandRoadmap(double expandTime);
211 
216 
231 
236  void clearQuery();
237 
238  virtual void clear();
239 
241  template<template<typename T> class NN>
243  {
244  nn_.reset(new NN<Vertex>());
246  connectionStrategy_.clear();
247  if (isSetup())
248  setup();
249  }
250 
251  virtual void setup();
252 
253  const Graph& getRoadmap() const
254  {
255  return g_;
256  }
257 
259  unsigned long int milestoneCount() const
260  {
261  return boost::num_vertices(g_);
262  }
263 
265  unsigned long int edgeCount() const
266  {
267  return boost::num_edges(g_);
268  }
269 
270  const RoadmapNeighbors& getNearestNeighbors()
271  {
272  return nn_;
273  }
274 
275  protected:
276 
278  void freeMemory();
279 
282  Vertex addMilestone(base::State *state);
283 
285  void uniteComponents(Vertex m1, Vertex m2);
286 
288  bool sameComponent(Vertex m1, Vertex m2);
289 
293  void growRoadmap(const base::PlannerTerminationCondition &ptc, base::State *workState);
294 
298  void expandRoadmap(const base::PlannerTerminationCondition &ptc, std::vector<base::State*> &workStates);
299 
302 
304  bool maybeConstructSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals, base::PathPtr &solution);
305 
307  bool addedNewSolution() const;
308 
310  base::PathPtr constructSolution(const Vertex &start, const Vertex &goal);
311 
314  base::Cost costHeuristic(Vertex u, Vertex v) const;
315 
317  double distanceFunction(const Vertex a, const Vertex b) const
318  {
319  return si_->distance(stateProperty_[a], stateProperty_[b]);
320  }
321 
323  // Planner progress property functions
324  std::string getIterationCount() const
325  {
326  return boost::lexical_cast<std::string>(iterations_);
327  }
328  std::string getBestCost() const
329  {
330  return boost::lexical_cast<std::string>(bestCost_);
331  }
332  std::string getMilestoneCountString() const
333  {
334  return boost::lexical_cast<std::string>(milestoneCount());
335  }
336  std::string getEdgeCountString() const
337  {
338  return boost::lexical_cast<std::string>(edgeCount());
339  }
340 
343 
346 
349 
351  RoadmapNeighbors nn_;
352 
355 
357  std::vector<Vertex> startM_;
358 
360  std::vector<Vertex> goalM_;
361 
363  boost::property_map<Graph, vertex_state_t>::type stateProperty_;
364 
366  boost::property_map<Graph,
367  vertex_total_connection_attempts_t>::type totalConnectionAttemptsProperty_;
368 
370  boost::property_map<Graph,
371  vertex_successful_connection_attempts_t>::type successfulConnectionAttemptsProperty_;
372 
374  boost::property_map<Graph, boost::edge_weight_t>::type weightProperty_;
375 
377  boost::disjoint_sets<
378  boost::property_map<Graph, boost::vertex_rank_t>::type,
379  boost::property_map<Graph, boost::vertex_predecessor_t>::type >
381 
384 
386  ConnectionFilter connectionFilter_;
387 
390 
393 
396 
398  mutable boost::mutex graphMutex_;
399 
402 
404  // Planner progress properties
406  unsigned long int iterations_;
409  };
410 
411  }
412 }
413 
414 #endif
PRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
Constructor.
Definition: PRM.cpp:73
boost::shared_ptr< NearestNeighbors< Vertex > > RoadmapNeighbors
A nearest neighbors data structure for roadmap vertices.
Definition: PRM.h:130
unsigned long int iterations_
Number of iterations the algorithm performed.
Definition: PRM.h:406
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:360
void clearQuery()
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition: PRM.cpp:170
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
Definition: PRM.cpp:319
base::ValidStateSamplerPtr sampler_
Sampler user for generating valid samples in the state space.
Definition: PRM.h:345
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
A boost shared pointer wrapper for ompl::base::ValidStateSampler.
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:317
A boost 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...
Definition: PRM.cpp:198
ConnectionStrategy connectionStrategy_
Function that returns the milestones to attempt connections with.
Definition: PRM.h:383
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:527
void setConnectionFilter(const ConnectionFilter &connectionFilter)
Set the function that can reject a milestone connection.
Definition: PRM.h:186
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:398
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:380
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:278
bool addedNewSolution_
A flag indicating that a solution has been added during solve()
Definition: PRM.h:395
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition: PRM.h:242
unsigned long int edgeCount() const
Return the number of edges currently in the graph.
Definition: PRM.h:265
boost::property_map< Graph, boost::edge_weight_t >::type weightProperty_
Access to the weights of each Edge.
Definition: PRM.h:374
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: PRM.cpp:177
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:386
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:371
boost::graph_traits< Graph >::edge_descriptor Edge
The type for an edge in the roadmap.
Definition: PRM.h:127
Main namespace. Contains everything in this library.
Definition: Cost.h:42
std::vector< Vertex > startM_
Array of start milestones.
Definition: PRM.h:357
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:54
void freeMemory()
Free all the memory allocated by the planner.
Definition: PRM.cpp:191
bool starStrategy_
Flag indicating whether the default connection strategy is the Star strategy.
Definition: PRM.h:342
Base class for a planner.
Definition: Planner.h:232
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:486
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:532
base::StateSamplerPtr simpleSampler_
Sampler user for generating random in the state space.
Definition: PRM.h:348
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
A boost shared pointer wrapper for ompl::base::SpaceInformation.
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.
Definition: PRM.h:122
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:163
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition: PRM.h:363
bool userSetConnectionStrategy_
Flag indicating whether the employed connection strategy was set by the user (or defaults are assumed...
Definition: PRM.h:389
boost::function< const std::vector< Vertex > &(const Vertex)> ConnectionStrategy
A function returning the milestones that should be attempted to connect to.
Definition: PRM.h:134
Graph g_
Connectivity graph.
Definition: PRM.h:354
A boost shared pointer wrapper for ompl::base::OptimizationObjective.
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:522
Probabilistic RoadMap planner.
Definition: PRM.h:83
ConnectionFilter connectionFilter_
Function that can reject a milestone connection.
Definition: PRM.h:386
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors...
Definition: PRM.cpp:149
base::OptimizationObjectivePtr opt_
Objective cost function for PRM graph edges.
Definition: PRM.h:401
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PRM.cpp:108
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:164
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...
Definition: PRM.cpp:341
base::Cost bestCost_
Best cost found so far by algorithm.
Definition: PRM.h:408
unsigned long int milestoneCount() const
Return the number of milestones currently in the graph.
Definition: PRM.h:259
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:397
boost::graph_traits< Graph >::vertex_descriptor Vertex
The type for a vertex in the roadmap.
Definition: PRM.h:125
RNG rng_
Random number generator.
Definition: PRM.h:392
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition allows, this function will construct the roadmap (using growRoadmap()...
Definition: PRM.cpp:458
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
RoadmapNeighbors nn_
Nearest neighbors data structure.
Definition: PRM.h:351
boost::function< bool(const Vertex &, const Vertex &)> ConnectionFilter
A function that can reject connections.
Definition: PRM.h:141
bool isSetup() const
Check if setup() was called for this planner.
Definition: Planner.cpp:107
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:567
bool addedNewSolution() const
Returns the value of the addedNewSolution_ member.
Definition: PRM.cpp:381
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:367
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:596