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