ompl/geometric/planners/prm/SPARS.h
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2013, Rutgers the State University of New Jersey, New Brunswick 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 Rutgers 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: Andrew Dobson */ 00036 00037 #ifndef OMPL_GEOMETRIC_PLANNERS_SPARSE_ROADMAP_SPANNER_ 00038 #define OMPL_GEOMETRIC_PLANNERS_SPARSE_ROADMAP_SPANNER_ 00039 00040 #include "ompl/geometric/planners/PlannerIncludes.h" 00041 #include "ompl/datastructures/NearestNeighbors.h" 00042 #include "ompl/geometric/PathSimplifier.h" 00043 #include "ompl/util/Time.h" 00044 00045 #include <boost/range/adaptor/map.hpp> 00046 #include <boost/unordered_map.hpp> 00047 #include <boost/graph/graph_traits.hpp> 00048 #include <boost/graph/adjacency_list.hpp> 00049 #include <boost/pending/disjoint_sets.hpp> 00050 #include <boost/function.hpp> 00051 #include <boost/thread.hpp> 00052 #include <iostream> 00053 #include <fstream> 00054 #include <utility> 00055 #include <vector> 00056 #include <deque> 00057 #include <map> 00058 #include <set> 00059 00060 namespace ompl 00061 { 00062 00063 namespace geometric 00064 { 00065 00080 class SPARS : public base::Planner 00081 { 00082 public: 00084 enum GuardType 00085 { 00086 START, 00087 GOAL, 00088 COVERAGE, 00089 CONNECTIVITY, 00090 INTERFACE, 00091 QUALITY, 00092 }; 00093 00094 struct vertex_state_t { 00095 typedef boost::vertex_property_tag kind; 00096 }; 00097 00098 struct vertex_representative_t { 00099 typedef boost::vertex_property_tag kind; 00100 }; 00101 00102 struct vertex_color_t { 00103 typedef boost::vertex_property_tag kind; 00104 }; 00105 00106 struct vertex_list_t { 00107 typedef boost::vertex_property_tag kind; 00108 }; 00109 00110 struct vertex_interface_list_t { 00111 typedef boost::vertex_property_tag kind; 00112 }; 00113 00115 typedef unsigned long int VertexIndexType; 00116 00118 typedef boost::unordered_map<VertexIndexType, std::set<VertexIndexType>, boost::hash<VertexIndexType> > InterfaceHash; 00119 00121 typedef std::deque<base::State*> DensePath; 00122 00123 // The InterfaceHash structure is wrapped inside of this struct due to a compilation error on 00124 // GCC 4.6 with Boost 1.48. An implicit assignment operator overload does not compile with these 00125 // components, so an explicit overload is given here. 00126 // Remove this struct when the minimum Boost requirement is > v1.48. 00127 struct InterfaceHashStruct 00128 { 00129 InterfaceHashStruct& operator=(const InterfaceHashStruct &rhs) { interfaceHash = rhs.interfaceHash; return *this; } 00130 InterfaceHash interfaceHash; 00131 }; 00132 00145 typedef boost::adjacency_list < 00146 boost::vecS, boost::vecS, boost::undirectedS, 00147 boost::property < vertex_state_t, base::State*, 00148 boost::property < boost::vertex_predecessor_t, VertexIndexType, 00149 boost::property < boost::vertex_rank_t, VertexIndexType, 00150 boost::property < vertex_color_t, GuardType, 00151 boost::property < vertex_list_t, std::set<VertexIndexType>, 00152 boost::property < vertex_interface_list_t, InterfaceHashStruct > > > > > >, 00153 boost::property < boost::edge_weight_t, base::Cost > 00154 > SpannerGraph; 00155 00157 typedef boost::graph_traits<SpannerGraph>::vertex_descriptor SparseVertex; 00158 00160 typedef boost::graph_traits<SpannerGraph>::edge_descriptor SparseEdge; 00161 00163 typedef boost::shared_ptr< NearestNeighbors<SparseVertex> > SparseNeighbors; 00164 00180 typedef boost::adjacency_list < 00181 boost::vecS, boost::vecS, boost::undirectedS, 00182 boost::property < vertex_state_t, base::State*, 00183 boost::property < boost::vertex_predecessor_t, VertexIndexType, 00184 boost::property < boost::vertex_rank_t, VertexIndexType, 00185 boost::property < vertex_representative_t, SparseVertex > > > >, 00186 boost::property < boost::edge_weight_t, double > 00187 > DenseGraph; 00188 00190 typedef boost::graph_traits<DenseGraph>::vertex_descriptor DenseVertex; 00191 00193 typedef boost::graph_traits<DenseGraph>::edge_descriptor DenseEdge; 00194 00196 typedef boost::shared_ptr< NearestNeighbors<DenseVertex> > DenseNeighbors; 00197 00199 SPARS(const base::SpaceInformationPtr &si); 00201 virtual ~SPARS(); 00202 00203 virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef); 00204 00205 virtual void getPlannerData(base::PlannerData &data) const; 00206 00208 void constructRoadmap(const base::PlannerTerminationCondition &ptc); 00209 00212 void constructRoadmap(const base::PlannerTerminationCondition &ptc, bool stopOnMaxFail); 00213 00226 virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc); 00227 00232 void clearQuery(); 00233 00234 virtual void clear(); 00235 00241 template<template<typename T> class NN> 00242 void setDenseNeighbors() 00243 { 00244 nn_.reset(new NN<DenseVertex>()); 00245 connectionStrategy_.clear(); 00246 if (isSetup()) 00247 setup(); 00248 } 00249 00254 template<template<typename T> class NN> 00255 void setSparseNeighbors() 00256 { 00257 snn_.reset(new NN<SparseVertex>()); 00258 if (isSetup()) 00259 setup(); 00260 } 00261 00266 void setMaxFailures(unsigned int m) 00267 { 00268 maxFailures_ = m; 00269 } 00270 00274 void setDenseDeltaFraction(double d) 00275 { 00276 denseDeltaFraction_ = d; 00277 if (denseDelta_ > 0.0) // setup was previously called 00278 denseDelta_ = d * si_->getMaximumExtent(); 00279 } 00280 00285 void setSparseDeltaFraction(double d) 00286 { 00287 sparseDeltaFraction_ = d; 00288 if (sparseDelta_ > 0.0) // setup was previously called 00289 sparseDelta_ = d * si_->getMaximumExtent(); 00290 } 00291 00295 void setStretchFactor(double t) 00296 { 00297 stretchFactor_ = t; 00298 } 00299 00301 unsigned getMaxFailures() const 00302 { 00303 return maxFailures_; 00304 } 00305 00307 double getDenseDeltaFraction() const 00308 { 00309 return denseDeltaFraction_; 00310 } 00311 00313 double getSparseDeltaFraction() const 00314 { 00315 return sparseDeltaFraction_; 00316 } 00317 00319 double getStretchFactor() const 00320 { 00321 return stretchFactor_; 00322 } 00323 00324 virtual void setup(); 00325 00327 const DenseGraph& getDenseGraph() const 00328 { 00329 return g_; 00330 } 00331 00334 const SpannerGraph& getRoadmap() const 00335 { 00336 return s_; 00337 } 00338 00340 unsigned int milestoneCount() const 00341 { 00342 return boost::num_vertices(g_); 00343 } 00344 00346 unsigned int guardCount() const 00347 { 00348 return boost::num_vertices(s_); 00349 } 00350 00352 double averageValence() const; 00353 00355 void printDebug(std::ostream &out = std::cout) const; 00356 00358 bool reachedFailureLimit() const; 00359 00361 // Planner progress property functions 00362 std::string getIterationCount() const 00363 { 00364 return boost::lexical_cast<std::string>(iterations_); 00365 } 00366 std::string getBestCost() const 00367 { 00368 return boost::lexical_cast<std::string>(bestCost_); 00369 } 00370 00371 protected: 00372 00374 DenseVertex addSample(base::State *workState, const base::PlannerTerminationCondition &ptc); 00375 00377 void checkQueryStateInitialization(); 00378 00380 bool sameComponent(SparseVertex m1, SparseVertex m2); 00381 00383 DenseVertex addMilestone(base::State *state); 00384 00386 SparseVertex addGuard(base::State *state, GuardType type); 00387 00389 void connectSparsePoints(SparseVertex v, SparseVertex vp); 00390 00392 void connectDensePoints(DenseVertex v, DenseVertex vp); 00393 00395 bool checkAddCoverage(const base::State *lastState, const std::vector<SparseVertex> &neigh); 00396 00398 bool checkAddConnectivity(const base::State *lastState, const std::vector<SparseVertex> &neigh); 00399 00401 bool checkAddInterface(const std::vector<DenseVertex>& graphNeighborhood, const std::vector<DenseVertex>& visibleNeighborhood, DenseVertex q); 00402 00404 bool checkAddPath( DenseVertex q, const std::vector<DenseVertex>& neigh ); 00405 00407 DenseVertex getInterfaceNeighbor(DenseVertex q, SparseVertex rep); 00408 00410 bool addPathToSpanner(const DensePath &p, SparseVertex vp, SparseVertex vpp); 00411 00413 void updateRepresentatives(SparseVertex v); 00414 00416 void calculateRepresentative( DenseVertex q ); 00417 00419 void addToRepresentatives( DenseVertex q, SparseVertex rep, const std::set<SparseVertex>& oreps ); 00420 00422 void removeFromRepresentatives( DenseVertex q, SparseVertex rep ); 00423 00425 void computeVPP(DenseVertex v, DenseVertex vp, std::vector<SparseVertex> &VPPs); 00426 00428 void computeX(DenseVertex v, DenseVertex vp, DenseVertex vpp, std::vector<SparseVertex> &Xs); 00429 00431 void resetFailures(); 00432 00434 void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution); 00435 00437 bool haveSolution(const std::vector<DenseVertex> &start, const std::vector<DenseVertex> &goal, base::PathPtr &solution); 00438 00440 bool reachedTerminationCriterion() const; 00441 00443 base::PathPtr constructSolution(const SparseVertex start, const SparseVertex goal) const; 00444 00446 void computeDensePath(const DenseVertex start, const DenseVertex goal, DensePath &path) const; 00447 00449 void freeMemory(); 00450 00452 void getSparseNeighbors(base::State *inState, std::vector< SparseVertex > &graphNeighborhood); 00453 00455 void filterVisibleNeighbors(base::State *inState, const std::vector<SparseVertex> &graphNeighborhood, std::vector<SparseVertex> &visibleNeighborhood) const; 00456 00458 void getInterfaceNeighborRepresentatives(DenseVertex q, std::set<SparseVertex> &interfaceRepresentatives); 00459 00461 void getInterfaceNeighborhood(DenseVertex q, std::vector<DenseVertex> &interfaceNeighborhood); 00462 00464 double distanceFunction(const DenseVertex a, const DenseVertex b) const 00465 { 00466 return si_->distance(stateProperty_[a], stateProperty_[b]); 00467 } 00468 00470 double sparseDistanceFunction( const SparseVertex a, const SparseVertex b ) const 00471 { 00472 return si_->distance( sparseStateProperty_[a], sparseStateProperty_[b] ); 00473 } 00474 00476 base::ValidStateSamplerPtr sampler_; 00477 00479 base::StateSamplerPtr simpleSampler_; 00480 00482 DenseNeighbors nn_; 00483 00485 SparseNeighbors snn_; 00486 00488 DenseGraph g_; 00489 00491 SpannerGraph s_; 00492 00494 std::vector<SparseVertex> startM_; 00495 00497 std::vector<SparseVertex> goalM_; 00498 00500 DenseVertex sparseQueryVertex_; 00501 00503 DenseVertex queryVertex_; 00504 00506 PathGeometric geomPath_; 00507 00509 boost::property_map<DenseGraph, vertex_state_t>::type stateProperty_; 00510 00512 boost::property_map<SpannerGraph, vertex_state_t>::type sparseStateProperty_; 00513 00515 boost::property_map<SpannerGraph, vertex_color_t>::type sparseColorProperty_; 00516 00518 boost::property_map<DenseGraph, vertex_representative_t>::type representativesProperty_; 00519 00521 boost::property_map<SpannerGraph, vertex_list_t>::type nonInterfaceListsProperty_; 00522 00524 boost::property_map<SpannerGraph, vertex_interface_list_t>::type interfaceListsProperty_; 00525 00527 PathSimplifierPtr psimp_; 00528 00530 boost::property_map<DenseGraph, boost::edge_weight_t>::type weightProperty_; 00531 00533 boost::disjoint_sets< 00534 boost::property_map<SpannerGraph, boost::vertex_rank_t>::type, 00535 boost::property_map<SpannerGraph, boost::vertex_predecessor_t>::type > 00536 sparseDJSets_; 00537 00539 boost::function<const std::vector<DenseVertex>&(const DenseVertex)> connectionStrategy_; 00540 00542 unsigned int consecutiveFailures_; 00543 00545 double stretchFactor_; 00546 00548 unsigned int maxFailures_; 00549 00551 bool addedSolution_; 00552 00554 double denseDeltaFraction_; 00555 00557 double sparseDeltaFraction_; 00558 00560 double denseDelta_; 00561 00563 double sparseDelta_; 00564 00566 RNG rng_; 00567 00569 mutable boost::mutex graphMutex_; 00570 00572 base::OptimizationObjectivePtr opt_; 00573 00575 base::Cost costHeuristic(SparseVertex u, SparseVertex v) const; 00576 00578 // Planner progress properties 00580 long unsigned int iterations_; 00582 base::Cost bestCost_; 00583 }; 00584 00585 } 00586 } 00587 00588 #endif