All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
src/ompl/geometric/planners/prm/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 
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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines