All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
PRM.h
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan, James D. Marble */
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
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends