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::function<std::vector<Vertex>&(const Vertex)>
00133 ConnectionStrategy;
00134
00140 typedef boost::function<bool(const Vertex&, const Vertex&)> ConnectionFilter;
00141
00143 PRM(const base::SpaceInformationPtr &si, bool starStrategy = false);
00144
00145 virtual ~PRM(void);
00146
00147 virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef);
00148
00162 void setConnectionStrategy(const ConnectionStrategy& connectionStrategy)
00163 {
00164 connectionStrategy_ = connectionStrategy;
00165 userSetConnectionStrategy_ = true;
00166 }
00170 void setMaxNearestNeighbors(unsigned int k);
00171
00185 void setConnectionFilter(const ConnectionFilter& connectionFilter)
00186 {
00187 connectionFilter_ = connectionFilter;
00188 }
00189
00190 virtual void getPlannerData(base::PlannerData &data) const;
00191
00195 virtual void growRoadmap(double growTime);
00196
00200 virtual void expandRoadmap(double expandTime);
00201
00202 virtual bool solve(const base::PlannerTerminationCondition &ptc);
00203
00204 virtual void clear(void);
00205
00207 template<template<typename T> class NN>
00208 void setNearestNeighbors(void)
00209 {
00210 nn_.reset(new NN<Vertex>());
00211 if (!userSetConnectionStrategy_)
00212 connectionStrategy_.clear();
00213 if (isSetup())
00214 setup();
00215 }
00216
00217 virtual void setup(void);
00218
00219 const Graph& getRoadmap(void) const
00220 {
00221 return g_;
00222 }
00223
00225 double distanceFunction(const Vertex a, const Vertex b) const
00226 {
00227 return si_->distance(stateProperty_[a], stateProperty_[b]);
00228 }
00229
00231 unsigned int milestoneCount(void) const
00232 {
00233 return boost::num_vertices(g_);
00234 }
00235
00236 const RoadmapNeighbors& getNearestNeighbors(void)
00237 {
00238 return nn_;
00239 }
00240
00241 protected:
00242
00244 void freeMemory(void);
00245
00247 virtual Vertex addMilestone(base::State *state);
00248
00250 void uniteComponents(Vertex m1, Vertex m2);
00251
00253 void growRoadmap(const std::vector<Vertex> &start, const std::vector<Vertex> &goal, const base::PlannerTerminationCondition &ptc, base::State *workState);
00254
00258 void expandRoadmap(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals, const base::PlannerTerminationCondition &ptc, std::vector<base::State*> &workStates);
00259
00261 bool haveSolution(const std::vector<Vertex> &start, const std::vector<Vertex> &goal, std::pair<Vertex, Vertex> *endpoints = NULL);
00262
00264 virtual base::PathPtr constructSolution(const Vertex start, const Vertex goal);
00265
00267 bool starStrategy_;
00268
00270 base::ValidStateSamplerPtr sampler_;
00271
00273 base::StateSamplerPtr simpleSampler_;
00274
00276 RoadmapNeighbors nn_;
00277
00279 Graph g_;
00280
00282 std::vector<Vertex> startM_;
00283
00285 std::vector<Vertex> goalM_;
00286
00288 base::PathPtr approxsol_;
00289
00291 double approxlen_;
00292
00294 boost::property_map<Graph, vertex_state_t>::type stateProperty_;
00295
00297 boost::property_map<Graph,
00298 vertex_total_connection_attempts_t>::type totalConnectionAttemptsProperty_;
00299
00301 boost::property_map<Graph,
00302 vertex_successful_connection_attempts_t>::type successfulConnectionAttemptsProperty_;
00303
00305 boost::property_map<Graph, boost::edge_weight_t>::type weightProperty_;
00306
00308 boost::property_map<Graph, boost::edge_index_t>::type edgeIDProperty_;
00309
00311 boost::disjoint_sets<
00312 boost::property_map<Graph, boost::vertex_rank_t>::type,
00313 boost::property_map<Graph, boost::vertex_predecessor_t>::type >
00314 disjointSets_;
00315
00317 unsigned int maxEdgeID_;
00318
00320 ConnectionStrategy connectionStrategy_;
00321
00323 ConnectionFilter connectionFilter_;
00324
00326 bool userSetConnectionStrategy_;
00327
00329 RNG rng_;
00330 };
00331
00332 }
00333 }
00334
00335 #endif