ompl/geometric/planners/prm/SPARStwo.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_SPARS_TWO_ 00038 #define OMPL_GEOMETRIC_PLANNERS_SPARS_TWO_ 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 <map> 00057 00058 namespace ompl 00059 { 00060 00061 namespace geometric 00062 { 00063 00079 class SPARStwo : public base::Planner 00080 { 00081 public: 00082 00084 enum GuardType 00085 { 00086 START, 00087 GOAL, 00088 COVERAGE, 00089 CONNECTIVITY, 00090 INTERFACE, 00091 QUALITY, 00092 }; 00093 00095 typedef unsigned long int VertexIndexType; 00096 00098 typedef std::pair< VertexIndexType, VertexIndexType > VertexPair; 00099 00101 struct InterfaceData 00102 { 00104 base::State *pointA_; 00105 base::State *pointB_; 00106 00108 base::State *sigmaA_; 00109 base::State *sigmaB_; 00110 00112 double d_; 00113 00115 InterfaceData() : 00116 pointA_(NULL), 00117 pointB_(NULL), 00118 sigmaA_(NULL), 00119 sigmaB_(NULL), 00120 d_(std::numeric_limits<double>::infinity()) 00121 { 00122 } 00123 00125 void clear(const base::SpaceInformationPtr& si) 00126 { 00127 if (pointA_) 00128 { 00129 si->freeState(pointA_); 00130 pointA_ = NULL; 00131 } 00132 if (pointB_) 00133 { 00134 si->freeState(pointB_); 00135 pointB_ = NULL; 00136 } 00137 if (sigmaA_) 00138 { 00139 si->freeState(sigmaA_); 00140 sigmaA_ = NULL; 00141 } 00142 if (sigmaB_) 00143 { 00144 si->freeState(sigmaB_); 00145 sigmaB_ = NULL; 00146 } 00147 d_ = std::numeric_limits<double>::infinity(); 00148 } 00149 00151 void setFirst(const base::State *p, const base::State *s, const base::SpaceInformationPtr& si) 00152 { 00153 if (pointA_) 00154 si->copyState(pointA_, p); 00155 else 00156 pointA_ = si->cloneState(p); 00157 if (sigmaA_) 00158 si->copyState(sigmaA_, s); 00159 else 00160 sigmaA_ = si->cloneState(s); 00161 if (pointB_) 00162 d_ = si->distance(pointA_, pointB_); 00163 } 00164 00166 void setSecond(const base::State *p, const base::State *s, const base::SpaceInformationPtr& si) 00167 { 00168 if (pointB_) 00169 si->copyState(pointB_, p); 00170 else 00171 pointB_ = si->cloneState(p); 00172 if (sigmaB_) 00173 si->copyState(sigmaB_, s); 00174 else 00175 sigmaB_ = si->cloneState(s); 00176 if (pointA_) 00177 d_ = si->distance(pointA_, pointB_); 00178 } 00179 }; 00180 00182 typedef boost::unordered_map< VertexPair, InterfaceData, boost::hash< VertexPair > > InterfaceHash; 00183 00184 // The InterfaceHash structure is wrapped inside of this struct due to a compilation error on 00185 // GCC 4.6 with Boost 1.48. An implicit assignment operator overload does not compile with these 00186 // components, so an explicit overload is given here. 00187 // Remove this struct when the minimum Boost requirement is > v1.48. 00188 struct InterfaceHashStruct 00189 { 00190 InterfaceHashStruct& operator=(const InterfaceHashStruct &rhs) { interfaceHash = rhs.interfaceHash; return *this; } 00191 InterfaceHash interfaceHash; 00192 }; 00193 00194 struct vertex_state_t { 00195 typedef boost::vertex_property_tag kind; 00196 }; 00197 00198 struct vertex_color_t { 00199 typedef boost::vertex_property_tag kind; 00200 }; 00201 00202 struct vertex_interface_data_t { 00203 typedef boost::vertex_property_tag kind; 00204 }; 00205 00221 typedef boost::adjacency_list < 00222 boost::vecS, boost::vecS, boost::undirectedS, 00223 boost::property < vertex_state_t, base::State*, 00224 boost::property < boost::vertex_predecessor_t, VertexIndexType, 00225 boost::property < boost::vertex_rank_t, VertexIndexType, 00226 boost::property < vertex_color_t, GuardType, 00227 boost::property < vertex_interface_data_t, InterfaceHashStruct > > > > >, 00228 boost::property < boost::edge_weight_t, base::Cost > 00229 > Graph; 00230 00232 typedef boost::graph_traits<Graph>::vertex_descriptor Vertex; 00233 00235 typedef boost::graph_traits<Graph>::edge_descriptor Edge; 00236 00238 SPARStwo(const base::SpaceInformationPtr &si); 00239 00241 virtual ~SPARStwo(); 00242 00243 virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef); 00244 00246 void setStretchFactor(double t) 00247 { 00248 stretchFactor_ = t; 00249 } 00250 00252 void setSparseDeltaFraction( double D ) 00253 { 00254 sparseDeltaFraction_ = D; 00255 if (sparseDelta_ > 0.0) // setup was previously called 00256 sparseDelta_ = D * si_->getMaximumExtent(); 00257 } 00258 00260 void setDenseDeltaFraction( double d ) 00261 { 00262 denseDeltaFraction_ = d; 00263 if (denseDelta_ > 0.0) // setup was previously called 00264 denseDelta_ = d * si_->getMaximumExtent(); 00265 } 00266 00268 void setMaxFailures( unsigned int m ) 00269 { 00270 maxFailures_ = m; 00271 } 00272 00274 unsigned int getMaxFailures( ) const 00275 { 00276 return maxFailures_; 00277 } 00278 00280 double getDenseDeltaFraction( ) const 00281 { 00282 return denseDeltaFraction_; 00283 } 00284 00286 double getSparseDeltaFraction( ) const 00287 { 00288 return sparseDeltaFraction_; 00289 } 00290 00292 double getStretchFactor( ) const 00293 { 00294 return stretchFactor_; 00295 } 00296 00298 void constructRoadmap(const base::PlannerTerminationCondition &ptc); 00299 00302 void constructRoadmap(const base::PlannerTerminationCondition &ptc, bool stopOnMaxFail); 00303 00316 virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc); 00317 00322 void clearQuery(); 00323 00324 virtual void clear(); 00325 00327 template<template<typename T> class NN> 00328 void setNearestNeighbors() 00329 { 00330 nn_.reset(new NN< Vertex >()); 00331 if (isSetup()) 00332 setup(); 00333 } 00334 00335 virtual void setup(); 00336 00338 const Graph& getRoadmap() const 00339 { 00340 return g_; 00341 } 00342 00344 unsigned int milestoneCount() const 00345 { 00346 return boost::num_vertices(g_); 00347 } 00348 00349 virtual void getPlannerData(base::PlannerData &data) const; 00350 00352 void printDebug(std::ostream &out = std::cout) const; 00353 00355 // Planner progress property functions 00356 std::string getIterationCount() const 00357 { 00358 return boost::lexical_cast<std::string>(iterations_); 00359 } 00360 std::string getBestCost() const 00361 { 00362 return boost::lexical_cast<std::string>(bestCost_); 00363 } 00364 00365 protected: 00366 00368 void freeMemory(); 00369 00371 void checkQueryStateInitialization(); 00372 00374 bool checkAddCoverage(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood); 00375 00377 bool checkAddConnectivity(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood); 00378 00380 bool checkAddInterface(const base::State *qNew, std::vector<Vertex> &graphNeighborhood, std::vector<Vertex> &visibleNeighborhood); 00381 00383 bool checkAddPath( Vertex v ); 00384 00386 void resetFailures(); 00387 00389 void findGraphNeighbors(base::State *st, std::vector<Vertex> &graphNeighborhood, std::vector<Vertex> &visibleNeighborhood); 00390 00392 void approachGraph( Vertex v ); 00393 00395 Vertex findGraphRepresentative(base::State *st); 00396 00398 void findCloseRepresentatives(base::State *workArea, const base::State *qNew, Vertex qRep, 00399 std::map<Vertex, base::State*> &closeRepresentatives, const base::PlannerTerminationCondition &ptc); 00400 00402 void updatePairPoints(Vertex rep, const base::State *q, Vertex r, const base::State *s); 00403 00405 void computeVPP(Vertex v, Vertex vp, std::vector<Vertex> &VPPs); 00406 00408 void computeX(Vertex v, Vertex vp, Vertex vpp, std::vector<Vertex> &Xs); 00409 00411 VertexPair index( Vertex vp, Vertex vpp ); 00412 00414 InterfaceData& getData( Vertex v, Vertex vp, Vertex vpp ); 00415 00417 void distanceCheck(Vertex rep, const base::State *q, Vertex r, const base::State *s, Vertex rp); 00418 00420 void abandonLists(base::State *st); 00421 00423 Vertex addGuard(base::State *state, GuardType type); 00424 00426 void connectGuards( Vertex v, Vertex vp ); 00427 00429 bool haveSolution(const std::vector<Vertex> &start, const std::vector<Vertex> &goal, base::PathPtr &solution); 00430 00432 void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution); 00433 00435 bool reachedTerminationCriterion() const; 00436 00438 bool reachedFailureLimit () const; 00439 00441 base::PathPtr constructSolution(const Vertex start, const Vertex goal) const; 00442 00444 bool sameComponent(Vertex m1, Vertex m2); 00445 00447 double distanceFunction(const Vertex a, const Vertex b) const 00448 { 00449 return si_->distance(stateProperty_[a], stateProperty_[b]); 00450 } 00451 00453 base::ValidStateSamplerPtr sampler_; 00454 00456 base::StateSamplerPtr simpleSampler_; 00457 00459 boost::shared_ptr< NearestNeighbors<Vertex> > nn_; 00460 00462 Graph g_; 00463 00465 std::vector<Vertex> startM_; 00466 00468 std::vector<Vertex> goalM_; 00469 00471 Vertex queryVertex_; 00472 00474 double stretchFactor_; 00475 00477 double sparseDeltaFraction_; 00478 00480 double denseDeltaFraction_; 00481 00483 unsigned int maxFailures_; 00484 00486 unsigned int nearSamplePoints_; 00487 00489 boost::property_map<Graph, vertex_state_t>::type stateProperty_; 00490 00492 PathSimplifierPtr psimp_; 00493 00495 boost::property_map<Graph, boost::edge_weight_t>::type weightProperty_; 00496 00498 boost::property_map<Graph, vertex_color_t>::type colorProperty_; 00499 00501 boost::property_map<Graph, vertex_interface_data_t>::type interfaceDataProperty_; 00502 00504 boost::disjoint_sets< 00505 boost::property_map<Graph, boost::vertex_rank_t>::type, 00506 boost::property_map<Graph, boost::vertex_predecessor_t>::type > 00507 disjointSets_; 00509 RNG rng_; 00510 00512 bool addedSolution_; 00513 00515 unsigned int consecutiveFailures_; 00516 00518 double sparseDelta_; 00519 00521 double denseDelta_; 00522 00524 mutable boost::mutex graphMutex_; 00525 00527 base::OptimizationObjectivePtr opt_; 00528 00530 base::Cost costHeuristic(Vertex u, Vertex v) const; 00531 00533 // Planner progress properties 00535 long unsigned int iterations_; 00537 base::Cost bestCost_; 00538 }; 00539 00540 } 00541 } 00542 00543 #endif