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, Ryan Luna */
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 <boost/thread.hpp>
00047 #include <utility>
00048 #include <vector>
00049 #include <map>
00050 
00051 namespace ompl
00052 {
00053 
00054     namespace base
00055     {
00056         // Forward declare for use in implementation
00057         OMPL_CLASS_FORWARD(OptimizationObjective);
00058     }
00059 
00060     namespace geometric
00061     {
00062 
00083         class PRM : public base::Planner
00084         {
00085         public:
00086 
00087             struct vertex_state_t {
00088                 typedef boost::vertex_property_tag kind;
00089             };
00090 
00091             struct vertex_total_connection_attempts_t {
00092                 typedef boost::vertex_property_tag kind;
00093             };
00094 
00095             struct vertex_successful_connection_attempts_t {
00096                 typedef boost::vertex_property_tag kind;
00097             };
00098 
00114             typedef boost::adjacency_list <
00115                 boost::vecS, boost::vecS, boost::undirectedS,
00116                 boost::property < vertex_state_t, base::State*,
00117                 boost::property < vertex_total_connection_attempts_t, unsigned long int,
00118                 boost::property < vertex_successful_connection_attempts_t, unsigned long int,
00119                 boost::property < boost::vertex_predecessor_t, unsigned long int,
00120                 boost::property < boost::vertex_rank_t, unsigned long int > > > > >,
00121                 boost::property < boost::edge_weight_t, base::Cost >
00122             > Graph;
00123 
00125             typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
00127             typedef boost::graph_traits<Graph>::edge_descriptor   Edge;
00128 
00130             typedef boost::shared_ptr< NearestNeighbors<Vertex> > RoadmapNeighbors;
00131 
00134             typedef boost::function<const std::vector<Vertex>&(const Vertex)> ConnectionStrategy;
00135 
00141             typedef boost::function<bool(const Vertex&, const Vertex&)> ConnectionFilter;
00142 
00144             PRM(const base::SpaceInformationPtr &si, bool starStrategy = false);
00145 
00146             virtual ~PRM();
00147 
00148             virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef);
00149 
00163             void setConnectionStrategy(const ConnectionStrategy& connectionStrategy)
00164             {
00165                 connectionStrategy_ = connectionStrategy;
00166                 userSetConnectionStrategy_ = true;
00167             }
00171             void setMaxNearestNeighbors(unsigned int k);
00172 
00186             void setConnectionFilter(const ConnectionFilter& connectionFilter)
00187             {
00188                 connectionFilter_ = connectionFilter;
00189             }
00190 
00191             virtual void getPlannerData(base::PlannerData &data) const;
00192 
00195             void constructRoadmap(const base::PlannerTerminationCondition &ptc);
00196 
00200             void growRoadmap(double growTime);
00201 
00205             void growRoadmap(const base::PlannerTerminationCondition &ptc);
00206 
00210             void expandRoadmap(double expandTime);
00211 
00215             void expandRoadmap(const base::PlannerTerminationCondition &ptc);
00216 
00230             virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);
00231 
00236             void clearQuery();
00237 
00238             virtual void clear();
00239 
00241             template<template<typename T> class NN>
00242             void setNearestNeighbors()
00243             {
00244                 nn_.reset(new NN<Vertex>());
00245                 if (!userSetConnectionStrategy_)
00246                     connectionStrategy_.clear();
00247                 if (isSetup())
00248                     setup();
00249             }
00250 
00251             virtual void setup();
00252 
00253             const Graph& getRoadmap() const
00254             {
00255                 return g_;
00256             }
00257 
00259             unsigned long int milestoneCount() const
00260             {
00261                 return boost::num_vertices(g_);
00262             }
00263 
00265             unsigned long int edgeCount() const
00266             {
00267                 return boost::num_edges(g_);
00268             }
00269 
00270             const RoadmapNeighbors& getNearestNeighbors()
00271             {
00272                 return nn_;
00273             }
00274 
00275         protected:
00276 
00278             void freeMemory();
00279 
00282             Vertex addMilestone(base::State *state);
00283 
00285             void uniteComponents(Vertex m1, Vertex m2);
00286 
00288             bool sameComponent(Vertex m1, Vertex m2);
00289 
00293             void growRoadmap(const base::PlannerTerminationCondition &ptc, base::State *workState);
00294 
00298             void expandRoadmap(const base::PlannerTerminationCondition &ptc, std::vector<base::State*> &workStates);
00299 
00301             void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution);
00302 
00304             bool maybeConstructSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals, base::PathPtr &solution);
00305 
00307             bool addedNewSolution() const;
00308 
00310             base::PathPtr constructSolution(const Vertex &start, const Vertex &goal);
00311 
00314             base::Cost costHeuristic(Vertex u, Vertex v) const;
00315 
00317             double distanceFunction(const Vertex a, const Vertex b) const
00318             {
00319                 return si_->distance(stateProperty_[a], stateProperty_[b]);
00320             }
00321 
00323             // Planner progress property functions
00324             std::string getIterationCount() const
00325             {
00326                 return boost::lexical_cast<std::string>(iterations_);
00327             }
00328             std::string getBestCost() const
00329             {
00330                 return boost::lexical_cast<std::string>(bestCost_);
00331             }
00332             std::string getMilestoneCountString() const
00333             {
00334                 return boost::lexical_cast<std::string>(milestoneCount());
00335             }
00336             std::string getEdgeCountString() const
00337             {
00338                 return boost::lexical_cast<std::string>(edgeCount());
00339             }
00340 
00342             bool                                                   starStrategy_;
00343 
00345             base::ValidStateSamplerPtr                             sampler_;
00346 
00348             base::StateSamplerPtr                                  simpleSampler_;
00349 
00351             RoadmapNeighbors                                       nn_;
00352 
00354             Graph                                                  g_;
00355 
00357             std::vector<Vertex>                                    startM_;
00358 
00360             std::vector<Vertex>                                    goalM_;
00361 
00363             boost::property_map<Graph, vertex_state_t>::type       stateProperty_;
00364 
00366             boost::property_map<Graph,
00367                 vertex_total_connection_attempts_t>::type          totalConnectionAttemptsProperty_;
00368 
00370             boost::property_map<Graph,
00371                 vertex_successful_connection_attempts_t>::type     successfulConnectionAttemptsProperty_;
00372 
00374             boost::property_map<Graph, boost::edge_weight_t>::type weightProperty_;
00375 
00377             boost::disjoint_sets<
00378                 boost::property_map<Graph, boost::vertex_rank_t>::type,
00379                 boost::property_map<Graph, boost::vertex_predecessor_t>::type >
00380                                                                    disjointSets_;
00381 
00383             ConnectionStrategy                                     connectionStrategy_;
00384 
00386             ConnectionFilter                                       connectionFilter_;
00387 
00389             bool                                                   userSetConnectionStrategy_;
00390 
00392             RNG                                                    rng_;
00393 
00395             bool                                                   addedNewSolution_;
00396 
00398             mutable boost::mutex                                   graphMutex_;
00399 
00401             base::OptimizationObjectivePtr                         opt_;
00402 
00404             // Planner progress properties
00406             unsigned long int                                      iterations_;
00408             base::Cost                                             bestCost_;
00409         };
00410 
00411     }
00412 }
00413 
00414 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines