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 */
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 <utility>
47 #include <vector>
48 #include <map>
49 
50 namespace ompl
51 {
52 
53  namespace geometric
54  {
55 
76  class PRM : public base::Planner
77  {
78  public:
79 
80  struct vertex_state_t {
81  typedef boost::vertex_property_tag kind;
82  };
83 
85  typedef boost::vertex_property_tag kind;
86  };
87 
89  typedef boost::vertex_property_tag kind;
90  };
91 
107  typedef boost::adjacency_list <
108  boost::vecS, boost::vecS, boost::undirectedS,
109  boost::property < vertex_state_t, base::State*,
110  boost::property < vertex_total_connection_attempts_t, unsigned int,
111  boost::property < vertex_successful_connection_attempts_t, unsigned int,
112  boost::property < boost::vertex_predecessor_t, unsigned long int,
113  boost::property < boost::vertex_rank_t, unsigned long int > > > > >,
114  boost::property < boost::edge_weight_t, double,
115  boost::property < boost::edge_index_t, unsigned int> >
116  > Graph;
117 
118  typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
119  typedef boost::graph_traits<Graph>::edge_descriptor Edge;
120 
121  typedef boost::shared_ptr< NearestNeighbors<Vertex> > RoadmapNeighbors;
122 
129  typedef boost::function<std::vector<Vertex>&(const Vertex)>
131 
137  typedef boost::function<bool(const Vertex&, const Vertex&)> ConnectionFilter;
138 
140  PRM(const base::SpaceInformationPtr &si, bool starStrategy = false);
141 
142  virtual ~PRM(void);
143 
144  virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef);
145 
159  void setConnectionStrategy(const ConnectionStrategy& connectionStrategy)
160  {
161  connectionStrategy_ = connectionStrategy;
163  }
167  void setMaxNearestNeighbors(unsigned int k);
168 
182  void setConnectionFilter(const ConnectionFilter& connectionFilter)
183  {
184  connectionFilter_ = connectionFilter;
185  }
186 
187  virtual void getPlannerData(base::PlannerData &data) const;
188 
192  virtual void growRoadmap(double growTime);
193 
197  virtual void growRoadmap(const base::PlannerTerminationCondition &ptc);
198 
202  virtual void expandRoadmap(double expandTime);
203 
207  virtual void expandRoadmap(const base::PlannerTerminationCondition &ptc);
208 
222 
227  void clearQuery(void);
228 
229  virtual void clear(void);
230 
232  template<template<typename T> class NN>
234  {
235  nn_.reset(new NN<Vertex>());
237  connectionStrategy_.clear();
238  if (isSetup())
239  setup();
240  }
241 
242  virtual void setup(void);
243 
244  const Graph& getRoadmap(void) const
245  {
246  return g_;
247  }
248 
250  double distanceFunction(const Vertex a, const Vertex b) const
251  {
252  return si_->distance(stateProperty_[a], stateProperty_[b]);
253  }
254 
256  unsigned int milestoneCount(void) const
257  {
258  return boost::num_vertices(g_);
259  }
260 
261  const RoadmapNeighbors& getNearestNeighbors(void)
262  {
263  return nn_;
264  }
265 
266  protected:
267 
269  void freeMemory(void);
270 
272  virtual Vertex addMilestone(base::State *state);
273 
275  void uniteComponents(Vertex m1, Vertex m2);
276 
280  void growRoadmap(const base::PlannerTerminationCondition &ptc, base::State *workState);
281 
285  void expandRoadmap(const base::PlannerTerminationCondition &ptc, std::vector<base::State*> &workStates);
286 
289 
291  bool haveSolution(const std::vector<Vertex> &start, const std::vector<Vertex> &goal, base::PathPtr &solution);
292 
294  bool addedNewSolution (void) const;
295 
297  virtual base::PathPtr constructSolution(const Vertex start, const Vertex goal) const;
298 
301 
304 
307 
309  RoadmapNeighbors nn_;
310 
313 
315  std::vector<Vertex> startM_;
316 
318  std::vector<Vertex> goalM_;
319 
321  boost::property_map<Graph, vertex_state_t>::type stateProperty_;
322 
324  boost::property_map<Graph,
326 
328  boost::property_map<Graph,
330 
332  boost::property_map<Graph, boost::edge_weight_t>::type weightProperty_;
333 
335  boost::property_map<Graph, boost::edge_index_t>::type edgeIDProperty_;
336 
338  boost::disjoint_sets<
339  boost::property_map<Graph, boost::vertex_rank_t>::type,
340  boost::property_map<Graph, boost::vertex_predecessor_t>::type >
342 
344  unsigned int maxEdgeID_;
345 
348 
351 
354 
357 
360 
362  mutable boost::mutex graphMutex_;
363 
364  };
365 
366  }
367 }
368 
369 #endif