All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
src/ompl/base/PlannerData.h
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2012, 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: Ryan Luna */
00036 
00037 #ifndef OMPL_BASE_PLANNER_DATA_
00038 #define OMPL_BASE_PLANNER_DATA_
00039 
00040 #include <iostream>
00041 #include <vector>
00042 #include <map>
00043 #include <set>
00044 #include "ompl/base/State.h"
00045 #include "ompl/base/SpaceInformation.h"
00046 #include "ompl/util/ClassForward.h"
00047 #include <boost/noncopyable.hpp>
00048 #include <boost/function.hpp>
00049 #include <boost/serialization/access.hpp>
00050 
00051 namespace ompl
00052 {
00053     namespace base
00054     {
00059         class PlannerDataVertex
00060         {
00061         public:
00063             PlannerDataVertex (const State* st, int tag = 0) : state_(st), tag_(tag) {}
00065             PlannerDataVertex (const PlannerDataVertex& rhs) : state_(rhs.state_), tag_(rhs.tag_) {}
00066             virtual ~PlannerDataVertex (void) {}
00067 
00069             virtual int  getTag (void) const { return tag_; }
00071             virtual void setTag (int tag) { tag_ = tag; }
00073             virtual const State* getState(void) const { return state_; }
00074 
00076             virtual PlannerDataVertex* clone (void) const
00077             {
00078                 return new PlannerDataVertex(*this);
00079             }
00080 
00082             virtual bool operator == (const PlannerDataVertex &rhs) const
00083             {
00084                 // States should be unique
00085                 return state_ == rhs.state_;
00086             }
00087 
00090             bool operator != (const PlannerDataVertex &rhs) const
00091             {
00092                 return !(*this == rhs);
00093             }
00094 
00095         protected:
00096             PlannerDataVertex(void) {}
00097 
00098             friend class boost::serialization::access;
00099             template <class Archive>
00100             void serialize(Archive & ar, const unsigned int version)
00101             {
00102                 ar & tag_;
00103                 // Serialization of the state pointer is handled by PlannerDataStorage
00104             }
00105 
00107             const State* state_;
00109             int tag_;
00110 
00111             friend class PlannerData;
00112             friend class PlannerDataStorage;
00113         };
00114 
00116         class PlannerDataEdge
00117         {
00118         public:
00119             PlannerDataEdge (void) {}
00120             virtual ~PlannerDataEdge (void) {}
00122             virtual PlannerDataEdge* clone () const { return new PlannerDataEdge(); }
00123 
00125             virtual bool operator == (const PlannerDataEdge &rhs) const
00126             {
00127                 return this == &rhs;
00128             }
00129 
00132             bool operator != (const PlannerDataEdge &rhs) const
00133             {
00134                 return !(*this == rhs);
00135             }
00136 
00137         protected:
00138 
00139             friend class boost::serialization::access;
00140             template <class Archive>
00141             void serialize(Archive & ar, const unsigned int version)
00142             {
00143             }
00144         };
00145 
00147         ClassForward(PlannerData);
00149 
00153         class PlannerData : boost::noncopyable
00154         {
00155         public:
00156             class Graph;
00158             typedef boost::function<double (const PlannerDataVertex&, const PlannerDataVertex&, const PlannerDataEdge&)> EdgeWeightFn;
00159 
00161             static const PlannerDataEdge   NO_EDGE;
00163             static const PlannerDataVertex NO_VERTEX;
00165             static const double            INVALID_WEIGHT;
00167             static const unsigned int      INVALID_INDEX;
00168 
00170             PlannerData(const SpaceInformationPtr &si);
00172             virtual ~PlannerData(void);
00173 
00176 
00181             unsigned int addVertex (const PlannerDataVertex &st);
00186             unsigned int addStartVertex (const PlannerDataVertex &v);
00191             unsigned int addGoalVertex  (const PlannerDataVertex &v);
00194             bool markStartState (const State* st);
00197             bool markGoalState (const State* st);
00200             bool tagState (const State* st, int tag);
00204             virtual bool removeVertex (const PlannerDataVertex &st);
00208             virtual bool removeVertex (unsigned int vIndex);
00211             virtual bool addEdge (unsigned int v1, unsigned int v2,
00212                                   const PlannerDataEdge &edge = PlannerDataEdge(), double weight=1.0);
00217             virtual bool addEdge (const PlannerDataVertex &v1, const PlannerDataVertex &v2,
00218                                   const PlannerDataEdge &edge = PlannerDataEdge(), double weight=1.0);
00220             virtual bool removeEdge (unsigned int v1, unsigned int v2);
00223             virtual bool removeEdge (const PlannerDataVertex &v1, const PlannerDataVertex &v2);
00225             virtual void clear (void);
00233             virtual void decoupleFromPlanner(void);
00234 
00238 
00240             unsigned int numEdges (void) const;
00242             unsigned int numVertices (void) const;
00244             unsigned int numStartVertices (void) const;
00246             unsigned int numGoalVertices (void) const;
00247 
00251 
00253             bool vertexExists (const PlannerDataVertex &v) const;
00256             const PlannerDataVertex& getVertex (unsigned int index) const;
00259             PlannerDataVertex& getVertex (unsigned int index);
00262             const PlannerDataVertex& getStartVertex (unsigned int i) const;
00265             PlannerDataVertex& getStartVertex (unsigned int i);
00268             const PlannerDataVertex& getGoalVertex (unsigned int i) const;
00271             PlannerDataVertex& getGoalVertex (unsigned int i);
00275             unsigned int getStartIndex (unsigned int i) const;
00279             unsigned int getGoalIndex  (unsigned int i) const;
00281             bool isStartVertex (unsigned int index) const;
00283             bool isGoalVertex (unsigned int index) const;
00287             unsigned int vertexIndex (const PlannerDataVertex &v) const;
00288 
00292 
00294             bool edgeExists (unsigned int v1, unsigned int v2) const;
00297             const PlannerDataEdge& getEdge (unsigned int v1, unsigned int v2) const;
00300             PlannerDataEdge& getEdge (unsigned int v1, unsigned int v2);
00304             unsigned int getEdges (unsigned int v, std::vector<unsigned int>& edgeList) const;
00307             unsigned int getEdges (unsigned int v, std::map<unsigned int, const PlannerDataEdge*> &edgeMap) const;
00310             unsigned int getIncomingEdges (unsigned int v, std::vector<unsigned int>& edgeList) const;
00314             unsigned int getIncomingEdges (unsigned int v, std::map<unsigned int, const PlannerDataEdge*> &edgeMap) const;
00317             double getEdgeWeight (unsigned int v1, unsigned int v2) const;
00320             bool setEdgeWeight (unsigned int v1, unsigned int v2, double weight);
00324             void computeEdgeWeights(const EdgeWeightFn& f = NULL);
00325 
00329 
00331             void printGraphviz (std::ostream& out = std::cout) const;
00333             void printGraphML (std::ostream& out = std::cout) const;
00334 
00338 
00342             void extractMinimumSpanningTree (unsigned int v, PlannerData &mst) const;
00346             void extractReachable(unsigned int v, PlannerData &data) const;
00347 
00354             Graph& toBoostGraph (void);
00361             const Graph& toBoostGraph (void) const;
00362 
00364 
00366             const SpaceInformationPtr& getSpaceInformation(void) const;
00367 
00368             virtual bool hasControls(void) const;
00369 
00371             std::map<std::string, std::string>   properties;
00372 
00373         protected:
00374             double defaultEdgeWeight(const PlannerDataVertex &v1, const PlannerDataVertex &v2, const PlannerDataEdge& e) const;
00375 
00377             std::map<const State*, unsigned int> stateIndexMap_;
00379             std::vector<unsigned int>            startVertexIndices_;
00381             std::vector<unsigned int>            goalVertexIndices_;
00382 
00384             SpaceInformationPtr                  si_;
00387             std::set<State*>                     decoupledStates_;
00388 
00389         private:
00390             void freeMemory(void);
00391 
00392             // Abstract pointer that points to the Boost.Graph structure.
00393             // Obscured to prevent unnecessary inclusion of BGL throughout the
00394             // rest of the code.
00395             void* graphRaw_;
00396         };
00397     }
00398 }
00399 
00400 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines