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::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