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