ompl/geometric/planners/prm/LazyPRM.h
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2013, 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 */ 00036 00037 #ifndef OMPL_GEOMETRIC_PLANNERS_PRM_LAZY_PRM_ 00038 #define OMPL_GEOMETRIC_PLANNERS_PRM_LAZY_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/function.hpp> 00045 #include <utility> 00046 #include <vector> 00047 #include <map> 00048 00049 namespace ompl 00050 { 00051 namespace base 00052 { 00053 // Forward declare for use in implementation 00054 OMPL_CLASS_FORWARD(OptimizationObjective); 00055 } 00056 00057 namespace geometric 00058 { 00059 00075 class LazyPRM : public base::Planner 00076 { 00077 public: 00078 struct vertex_state_t { 00079 typedef boost::vertex_property_tag kind; 00080 }; 00081 00082 struct vertex_flags_t { 00083 typedef boost::vertex_property_tag kind; 00084 }; 00085 00086 struct vertex_component_t { 00087 typedef boost::vertex_property_tag kind; 00088 }; 00089 00090 struct edge_flags_t { 00091 typedef boost::edge_property_tag kind; 00092 }; 00093 00095 typedef boost::adjacency_list_traits<boost::vecS, boost::listS, 00096 boost::undirectedS>::vertex_descriptor Vertex; 00097 00114 typedef boost::adjacency_list < 00115 boost::vecS, boost::listS, boost::undirectedS, 00116 boost::property < vertex_state_t, base::State*, 00117 boost::property < boost::vertex_index_t, unsigned long int, 00118 boost::property < vertex_flags_t, unsigned int, 00119 boost::property < vertex_component_t, unsigned long int, 00120 boost::property < boost::vertex_predecessor_t, Vertex, 00121 boost::property < boost::vertex_rank_t, unsigned long int > > > > > >, 00122 boost::property < boost::edge_weight_t, base::Cost, 00123 boost::property < edge_flags_t, unsigned int > > 00124 > Graph; 00125 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 LazyPRM(const base::SpaceInformationPtr &si, bool starStrategy = false); 00145 00146 virtual ~LazyPRM(); 00147 00149 void setRange(double distance); 00150 00152 double getRange() const 00153 { 00154 return maxDistance_; 00155 } 00156 00158 template<template<typename T> class NN> 00159 void setNearestNeighbors() 00160 { 00161 nn_.reset(new NN<Vertex>()); 00162 if (!userSetConnectionStrategy_) 00163 connectionStrategy_.clear(); 00164 if (isSetup()) 00165 setup(); 00166 } 00167 00168 virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef); 00169 00183 void setConnectionStrategy(const ConnectionStrategy &connectionStrategy) 00184 { 00185 connectionStrategy_ = connectionStrategy; 00186 userSetConnectionStrategy_ = true; 00187 } 00188 00192 void setMaxNearestNeighbors(unsigned int k); 00193 00207 void setConnectionFilter(const ConnectionFilter &connectionFilter) 00208 { 00209 connectionFilter_ = connectionFilter; 00210 } 00211 00213 unsigned long int milestoneCount() const 00214 { 00215 return boost::num_vertices(g_); 00216 } 00217 00219 unsigned long int edgeCount() const 00220 { 00221 return boost::num_edges(g_); 00222 } 00223 00224 virtual void getPlannerData(base::PlannerData &data) const; 00225 00226 virtual void setup(); 00227 00228 virtual void clear(); 00229 00234 void clearQuery(); 00235 00236 virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc); 00237 00238 protected: 00239 00241 static const unsigned int VALIDITY_UNKNOWN = 0; 00242 00244 static const unsigned int VALIDITY_TRUE = 1; 00245 00247 // Planner progress property functions 00248 std::string getIterationCount() const 00249 { 00250 return boost::lexical_cast<std::string>(iterations_); 00251 } 00252 std::string getBestCost() const 00253 { 00254 return boost::lexical_cast<std::string>(bestCost_); 00255 } 00256 std::string getMilestoneCountString() const 00257 { 00258 return boost::lexical_cast<std::string>(milestoneCount()); 00259 } 00260 std::string getEdgeCountString() const 00261 { 00262 return boost::lexical_cast<std::string>(edgeCount()); 00263 } 00264 00266 void freeMemory(); 00267 00270 Vertex addMilestone(base::State *state); 00271 00272 void uniteComponents(Vertex a, Vertex b); 00273 00274 void markComponent(Vertex v, unsigned long int newComponent); 00275 00278 long int solutionComponent(std::pair<std::size_t, std::size_t> *startGoalPair) const; 00279 00281 ompl::base::PathPtr constructSolution(const Vertex &start, const Vertex &goal); 00282 00284 double distanceFunction(const Vertex a, const Vertex b) const 00285 { 00286 return si_->distance(stateProperty_[a], stateProperty_[b]); 00287 } 00288 00291 base::Cost costHeuristic(Vertex u, Vertex v) const; 00292 00294 bool starStrategy_; 00295 00297 ConnectionStrategy connectionStrategy_; 00298 00300 ConnectionFilter connectionFilter_; 00301 00303 bool userSetConnectionStrategy_; 00304 00306 double maxDistance_; 00307 00309 base::StateSamplerPtr sampler_; 00310 00312 RoadmapNeighbors nn_; 00313 00315 Graph g_; 00316 00318 std::vector<Vertex> startM_; 00319 00321 std::vector<Vertex> goalM_; 00322 00324 boost::property_map<Graph, boost::vertex_index_t>::type indexProperty_; 00325 00327 boost::property_map<Graph, vertex_state_t>::type stateProperty_; 00328 00330 boost::property_map<Graph, boost::edge_weight_t>::type weightProperty_; 00331 00333 boost::property_map<Graph, vertex_component_t>::type vertexComponentProperty_; 00334 00336 boost::property_map<Graph, vertex_flags_t>::type vertexValidityProperty_; 00337 00339 boost::property_map<Graph, edge_flags_t>::type edgeValidityProperty_; 00340 00343 unsigned long int componentCount_; 00344 00346 std::map<unsigned long int, unsigned long int> componentSize_; 00347 00349 base::OptimizationObjectivePtr opt_; 00350 00351 base::Cost bestCost_; 00352 00353 unsigned long int iterations_; 00354 }; 00355 00356 } 00357 } 00358 00359 #endif