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