00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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
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
00393
00394
00395 void* graphRaw_;
00396 };
00397 }
00398 }
00399
00400 #endif