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
00079 class PRM : public base::Planner
00080 {
00081 public:
00082
00083 struct vertex_state_t {
00084 typedef boost::vertex_property_tag kind;
00085 };
00086
00087 struct vertex_total_connection_attempts_t {
00088 typedef boost::vertex_property_tag kind;
00089 };
00090
00091 struct vertex_successful_connection_attempts_t {
00092 typedef boost::vertex_property_tag kind;
00093 };
00094
00110 typedef boost::adjacency_list <
00111 boost::vecS, boost::vecS, boost::undirectedS,
00112 boost::property < vertex_state_t, base::State*,
00113 boost::property < vertex_total_connection_attempts_t, unsigned int,
00114 boost::property < vertex_successful_connection_attempts_t, unsigned int,
00115 boost::property < boost::vertex_predecessor_t, unsigned long int,
00116 boost::property < boost::vertex_rank_t, unsigned long int > > > > >,
00117 boost::property < boost::edge_weight_t, double,
00118 boost::property < boost::edge_index_t, unsigned int> >
00119 > Graph;
00120
00121 typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
00122 typedef boost::graph_traits<Graph>::edge_descriptor Edge;
00123
00124 typedef boost::shared_ptr< NearestNeighbors<Vertex> > RoadmapNeighbors;
00125
00132 typedef boost::function1<std::vector<Vertex>&, const Vertex>
00133 ConnectionStrategy;
00134
00140 typedef boost::function2<bool, const Vertex&, const Vertex&> ConnectionFilter;
00141
00143 PRM(const base::SpaceInformationPtr &si, bool starStrategy = false);
00144
00145 virtual ~PRM(void)
00146 {
00147 freeMemory();
00148 }
00149
00150 virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef);
00151
00165 void setConnectionStrategy(const ConnectionStrategy& connectionStrategy)
00166 {
00167 connectionStrategy_ = connectionStrategy;
00168 userSetConnectionStrategy_ = true;
00169 }
00173 void setMaxNearestNeighbors(unsigned int k);
00174
00188 void setConnectionFilter(const ConnectionFilter& connectionFilter)
00189 {
00190 connectionFilter_ = connectionFilter;
00191 }
00192
00193 virtual void getPlannerData(base::PlannerData &data) const;
00194
00198 virtual void growRoadmap(double growTime);
00199
00203 virtual void expandRoadmap(double expandTime);
00204
00205 virtual bool solve(const base::PlannerTerminationCondition &ptc);
00206
00207 virtual void clear(void);
00208
00210 template<template<typename T> class NN>
00211 void setNearestNeighbors(void)
00212 {
00213 nn_.reset(new NN<Vertex>());
00214 if (!userSetConnectionStrategy_)
00215 connectionStrategy_.clear();
00216 if (isSetup())
00217 setup();
00218 }
00219
00220 virtual void setup(void);
00221
00222 const Graph& getRoadmap(void) const
00223 {
00224 return g_;
00225 }
00226
00228 double distanceFunction(const Vertex a, const Vertex b) const
00229 {
00230 return si_->distance(stateProperty_[a], stateProperty_[b]);
00231 }
00232
00234 unsigned int milestoneCount(void) const
00235 {
00236 return boost::num_vertices(g_);
00237 }
00238
00239 const RoadmapNeighbors& getNearestNeighbors(void)
00240 {
00241 return nn_;
00242 }
00243
00244 protected:
00245
00247 void freeMemory(void);
00248
00250 Vertex addMilestone(base::State *state);
00251
00253 void uniteComponents(Vertex m1, Vertex m2);
00254
00256 void growRoadmap(const std::vector<Vertex> &start, const std::vector<Vertex> &goal, const base::PlannerTerminationCondition &ptc, base::State *workState);
00257
00261 void expandRoadmap(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals, const base::PlannerTerminationCondition &ptc, std::vector<base::State*> &workStates);
00262
00264 bool haveSolution(const std::vector<Vertex> &start, const std::vector<Vertex> &goal, std::pair<Vertex, Vertex> *endpoints = NULL);
00265
00267 base::PathPtr constructSolution(const Vertex start, const Vertex goal);
00268
00270 bool starStrategy_;
00271
00273 base::ValidStateSamplerPtr sampler_;
00274
00276 base::StateSamplerPtr simpleSampler_;
00277
00279 RoadmapNeighbors nn_;
00280
00282 Graph g_;
00283
00285 std::vector<Vertex> startM_;
00286
00288 std::vector<Vertex> goalM_;
00289
00291 base::PathPtr approxsol_;
00292
00294 double approxlen_;
00295
00297 boost::property_map<Graph, vertex_state_t>::type stateProperty_;
00298
00300 boost::property_map<Graph,
00301 vertex_total_connection_attempts_t>::type totalConnectionAttemptsProperty_;
00302
00304 boost::property_map<Graph,
00305 vertex_successful_connection_attempts_t>::type successfulConnectionAttemptsProperty_;
00306
00308 boost::property_map<Graph, boost::edge_weight_t>::type weightProperty_;
00309
00311 boost::property_map<Graph, boost::edge_index_t>::type edgeIDProperty_;
00312
00314 boost::disjoint_sets<
00315 boost::property_map<Graph, boost::vertex_rank_t>::type,
00316 boost::property_map<Graph, boost::vertex_predecessor_t>::type >
00317 disjointSets_;
00318
00320 unsigned int maxEdgeID_;
00321
00323 ConnectionStrategy connectionStrategy_;
00324
00326 ConnectionFilter connectionFilter_;
00327
00329 bool userSetConnectionStrategy_;
00330
00332 RNG rng_;
00333 };
00334
00335 }
00336 }
00337
00338 #endif