00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef OMPL_GEOMETRIC_PLANNERS_PRM_PRM_
00038 #define OMPL_GEOMETRIC_PLANNERS_PRM_PRM_
00039
00040 #include "ompl/geometric/planners/PlannerIncludes.h"
00041 #include "ompl/datastructures/NearestNeighbors.h"
00042 #include <boost/graph/graph_traits.hpp>
00043 #include <boost/graph/adjacency_list.hpp>
00044 #include <boost/pending/disjoint_sets.hpp>
00045 #include <boost/function.hpp>
00046 #include <utility>
00047 #include <vector>
00048 #include <map>
00049
00050 namespace ompl
00051 {
00052
00053 namespace geometric
00054 {
00055
00076 class PRM : public base::Planner
00077 {
00078 public:
00079
00080 struct vertex_state_t {
00081 typedef boost::vertex_property_tag kind;
00082 };
00083
00084 struct vertex_total_connection_attempts_t {
00085 typedef boost::vertex_property_tag kind;
00086 };
00087
00088 struct vertex_successful_connection_attempts_t {
00089 typedef boost::vertex_property_tag kind;
00090 };
00091
00107 typedef boost::adjacency_list <
00108 boost::vecS, boost::vecS, boost::undirectedS,
00109 boost::property < vertex_state_t, base::State*,
00110 boost::property < vertex_total_connection_attempts_t, unsigned int,
00111 boost::property < vertex_successful_connection_attempts_t, unsigned int,
00112 boost::property < boost::vertex_predecessor_t, unsigned long int,
00113 boost::property < boost::vertex_rank_t, unsigned long int > > > > >,
00114 boost::property < boost::edge_weight_t, double,
00115 boost::property < boost::edge_index_t, unsigned int> >
00116 > Graph;
00117
00118 typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
00119 typedef boost::graph_traits<Graph>::edge_descriptor Edge;
00120
00121 typedef boost::shared_ptr< NearestNeighbors<Vertex> > RoadmapNeighbors;
00122
00129 typedef boost::function<std::vector<Vertex>&(const Vertex)>
00130 ConnectionStrategy;
00131
00137 typedef boost::function<bool(const Vertex&, const Vertex&)> ConnectionFilter;
00138
00140 PRM(const base::SpaceInformationPtr &si, bool starStrategy = false);
00141
00142 virtual ~PRM(void);
00143
00144 virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef);
00145
00159 void setConnectionStrategy(const ConnectionStrategy& connectionStrategy)
00160 {
00161 connectionStrategy_ = connectionStrategy;
00162 userSetConnectionStrategy_ = true;
00163 }
00167 void setMaxNearestNeighbors(unsigned int k);
00168
00182 void setConnectionFilter(const ConnectionFilter& connectionFilter)
00183 {
00184 connectionFilter_ = connectionFilter;
00185 }
00186
00187 virtual void getPlannerData(base::PlannerData &data) const;
00188
00192 virtual void growRoadmap(double growTime);
00193
00197 virtual void growRoadmap(const base::PlannerTerminationCondition &ptc);
00198
00202 virtual void expandRoadmap(double expandTime);
00203
00207 virtual void expandRoadmap(const base::PlannerTerminationCondition &ptc);
00208
00221 virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);
00222
00227 void clearQuery(void);
00228
00229 virtual void clear(void);
00230
00232 template<template<typename T> class NN>
00233 void setNearestNeighbors(void)
00234 {
00235 nn_.reset(new NN<Vertex>());
00236 if (!userSetConnectionStrategy_)
00237 connectionStrategy_.clear();
00238 if (isSetup())
00239 setup();
00240 }
00241
00242 virtual void setup(void);
00243
00244 const Graph& getRoadmap(void) const
00245 {
00246 return g_;
00247 }
00248
00250 double distanceFunction(const Vertex a, const Vertex b) const
00251 {
00252 return si_->distance(stateProperty_[a], stateProperty_[b]);
00253 }
00254
00256 unsigned int milestoneCount(void) const
00257 {
00258 return boost::num_vertices(g_);
00259 }
00260
00261 const RoadmapNeighbors& getNearestNeighbors(void)
00262 {
00263 return nn_;
00264 }
00265
00266 protected:
00267
00269 void freeMemory(void);
00270
00272 virtual Vertex addMilestone(base::State *state);
00273
00275 void uniteComponents(Vertex m1, Vertex m2);
00276
00280 void growRoadmap(const base::PlannerTerminationCondition &ptc, base::State *workState);
00281
00285 void expandRoadmap(const base::PlannerTerminationCondition &ptc, std::vector<base::State*> &workStates);
00286
00288 void checkForSolution (const base::PlannerTerminationCondition &ptc, base::PathPtr &solution);
00289
00291 bool haveSolution(const std::vector<Vertex> &start, const std::vector<Vertex> &goal, base::PathPtr &solution);
00292
00294 bool addedNewSolution (void) const;
00295
00297 virtual base::PathPtr constructSolution(const Vertex start, const Vertex goal) const;
00298
00300 bool starStrategy_;
00301
00303 base::ValidStateSamplerPtr sampler_;
00304
00306 base::StateSamplerPtr simpleSampler_;
00307
00309 RoadmapNeighbors nn_;
00310
00312 Graph g_;
00313
00315 std::vector<Vertex> startM_;
00316
00318 std::vector<Vertex> goalM_;
00319
00321 boost::property_map<Graph, vertex_state_t>::type stateProperty_;
00322
00324 boost::property_map<Graph,
00325 vertex_total_connection_attempts_t>::type totalConnectionAttemptsProperty_;
00326
00328 boost::property_map<Graph,
00329 vertex_successful_connection_attempts_t>::type successfulConnectionAttemptsProperty_;
00330
00332 boost::property_map<Graph, boost::edge_weight_t>::type weightProperty_;
00333
00335 boost::property_map<Graph, boost::edge_index_t>::type edgeIDProperty_;
00336
00338 boost::disjoint_sets<
00339 boost::property_map<Graph, boost::vertex_rank_t>::type,
00340 boost::property_map<Graph, boost::vertex_predecessor_t>::type >
00341 disjointSets_;
00342
00344 unsigned int maxEdgeID_;
00345
00347 ConnectionStrategy connectionStrategy_;
00348
00350 ConnectionFilter connectionFilter_;
00351
00353 bool userSetConnectionStrategy_;
00354
00356 RNG rng_;
00357
00359 bool addedSolution_;
00360
00362 mutable boost::mutex graphMutex_;
00363
00364 };
00365
00366 }
00367 }
00368
00369 #endif