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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines