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, Luis G. Torres */
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/Cost.h"
00046 #include "ompl/base/SpaceInformation.h"
00047 #include "ompl/util/ClassForward.h"
00048 #include <boost/noncopyable.hpp>
00049 #include <boost/function.hpp>
00050 #include <boost/serialization/access.hpp>
00051 
00052 namespace ompl
00053 {
00054     namespace base
00055     {
00060         class PlannerDataVertex
00061         {
00062         public:
00064             PlannerDataVertex(const State *st, int tag = 0) : state_(st), tag_(tag) {}
00066             PlannerDataVertex(const PlannerDataVertex &rhs) : state_(rhs.state_), tag_(rhs.tag_) {}
00067             virtual ~PlannerDataVertex() {}
00068 
00070             virtual int  getTag() const { return tag_; }
00072             virtual void setTag(int tag) { tag_ = tag; }
00074             virtual const State* getState() const { return state_; }
00075 
00077             virtual PlannerDataVertex* clone() const
00078             {
00079                 return new PlannerDataVertex(*this);
00080             }
00081 
00083             virtual bool operator==(const PlannerDataVertex &rhs) const
00084             {
00085                 // States should be unique
00086                 return state_ == rhs.state_;
00087             }
00088 
00091             bool operator!=(const PlannerDataVertex &rhs) const
00092             {
00093                 return !(*this == rhs);
00094             }
00095 
00096         protected:
00097             PlannerDataVertex() {}
00098 
00099             friend class boost::serialization::access;
00100             template <class Archive>
00101             void serialize(Archive & ar, const unsigned int /*version*/)
00102             {
00103                 ar & tag_;
00104                 // Serialization of the state pointer is handled by PlannerDataStorage
00105             }
00106 
00108             const State *state_;
00110             int tag_;
00111 
00112             friend class PlannerData;
00113             friend class PlannerDataStorage;
00114         };
00115 
00117         class PlannerDataEdge
00118         {
00119         public:
00120             PlannerDataEdge() {}
00121             virtual ~PlannerDataEdge() {}
00123             virtual PlannerDataEdge* clone() const { return new PlannerDataEdge(); }
00124 
00126             virtual bool operator==(const PlannerDataEdge &rhs) const
00127             {
00128                 return this == &rhs;
00129             }
00130 
00133             bool operator!=(const PlannerDataEdge &rhs) const
00134             {
00135                 return !(*this == rhs);
00136             }
00137 
00138         protected:
00139 
00140             friend class boost::serialization::access;
00141             template <class Archive>
00142             void serialize(Archive & /*ar*/, const unsigned int /*version*/)
00143             {
00144             }
00145         };
00146 
00148         OMPL_CLASS_FORWARD(StateStorage);
00149         OMPL_CLASS_FORWARD(PlannerData);
00150 
00151         // Forward declaration for PlannerData::computeEdgeWeights
00152         class OptimizationObjective;
00154 
00159 
00160 
00161 
00162 
00163 
00164         class PlannerData : boost::noncopyable
00165         {
00166         public:
00167             class Graph;
00168 
00170             static const PlannerDataEdge   NO_EDGE;
00172             static const PlannerDataVertex NO_VERTEX;
00174             static const unsigned int      INVALID_INDEX;
00175 
00177             PlannerData(const SpaceInformationPtr &si);
00179             virtual ~PlannerData();
00180 
00183 
00188             unsigned int addVertex(const PlannerDataVertex &st);
00193             unsigned int addStartVertex(const PlannerDataVertex &v);
00198             unsigned int addGoalVertex(const PlannerDataVertex &v);
00201             bool markStartState(const State *st);
00204             bool markGoalState(const State *st);
00207             bool tagState(const State *st, int tag);
00211             virtual bool removeVertex(const PlannerDataVertex &st);
00215             virtual bool removeVertex(unsigned int vIndex);
00218             virtual bool addEdge(unsigned int v1, unsigned int v2,
00219                                  const PlannerDataEdge &edge = PlannerDataEdge(),
00220                                  Cost weight = Cost(1.0));
00225             virtual bool addEdge(const PlannerDataVertex &v1, const PlannerDataVertex &v2,
00226                                  const PlannerDataEdge &edge = PlannerDataEdge(),
00227                                  Cost weight = Cost(1.0));
00229             virtual bool removeEdge(unsigned int v1, unsigned int v2);
00232             virtual bool removeEdge(const PlannerDataVertex &v1, const PlannerDataVertex &v2);
00234             virtual void clear();
00242             virtual void decoupleFromPlanner();
00243 
00247 
00249             unsigned int numEdges() const;
00251             unsigned int numVertices() const;
00253             unsigned int numStartVertices() const;
00255             unsigned int numGoalVertices() const;
00256 
00260 
00262             bool vertexExists(const PlannerDataVertex &v) const;
00265             const PlannerDataVertex& getVertex(unsigned int index) const;
00268             PlannerDataVertex& getVertex(unsigned int index);
00271             const PlannerDataVertex& getStartVertex(unsigned int i) const;
00274             PlannerDataVertex& getStartVertex(unsigned int i);
00277             const PlannerDataVertex& getGoalVertex(unsigned int i) const;
00280             PlannerDataVertex& getGoalVertex(unsigned int i);
00284             unsigned int getStartIndex(unsigned int i) const;
00288             unsigned int getGoalIndex(unsigned int i) const;
00290             bool isStartVertex(unsigned int index) const;
00292             bool isGoalVertex(unsigned int index) const;
00296             unsigned int vertexIndex(const PlannerDataVertex &v) const;
00297 
00301 
00303             bool edgeExists(unsigned int v1, unsigned int v2) const;
00306             const PlannerDataEdge& getEdge(unsigned int v1, unsigned int v2) const;
00309             PlannerDataEdge& getEdge(unsigned int v1, unsigned int v2);
00313             unsigned int getEdges(unsigned int v, std::vector<unsigned int>& edgeList) const;
00316             unsigned int getEdges(unsigned int v, std::map<unsigned int, const PlannerDataEdge*> &edgeMap) const;
00319             unsigned int getIncomingEdges(unsigned int v, std::vector<unsigned int>& edgeList) const;
00323             unsigned int getIncomingEdges(unsigned int v, std::map<unsigned int, const PlannerDataEdge*> &edgeMap) const;
00329             bool getEdgeWeight(unsigned int v1, unsigned int v2, Cost* weight) const;
00333             bool setEdgeWeight(unsigned int v1, unsigned int v2, Cost weight);
00336             void computeEdgeWeights(const OptimizationObjective &opt);
00339             void computeEdgeWeights();
00340 
00344 
00346             void printGraphviz(std::ostream& out = std::cout) const;
00347 
00349             void printGraphML(std::ostream& out = std::cout) const;
00350 
00354 
00358             void extractMinimumSpanningTree(unsigned int v,
00359                                             const OptimizationObjective &opt,
00360                                             PlannerData &mst) const;
00364             void extractReachable(unsigned int v, PlannerData &data) const;
00365 
00368             StateStoragePtr extractStateStorage() const;
00369 
00376             Graph& toBoostGraph();
00383             const Graph& toBoostGraph() const;
00384 
00386 
00388             const SpaceInformationPtr& getSpaceInformation() const;
00389 
00391             virtual bool hasControls() const;
00392 
00394             std::map<std::string, std::string>   properties;
00395 
00396         protected:
00398             std::map<const State*, unsigned int> stateIndexMap_;
00400             std::vector<unsigned int>            startVertexIndices_;
00402             std::vector<unsigned int>            goalVertexIndices_;
00403 
00405             SpaceInformationPtr                  si_;
00408             std::set<State*>                     decoupledStates_;
00409 
00410         private:
00411             void freeMemory();
00412 
00413             // Abstract pointer that points to the Boost.Graph structure.
00414             // Obscured to prevent unnecessary inclusion of BGL throughout the
00415             // rest of the code.
00416             void* graphRaw_;
00417         };
00418     }
00419 }
00420 
00421 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines