All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
PlannerData.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2012, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ryan Luna */
36 
37 #ifndef OMPL_BASE_PLANNER_DATA_
38 #define OMPL_BASE_PLANNER_DATA_
39 
40 #include <iostream>
41 #include <vector>
42 #include <map>
43 #include <set>
44 #include "ompl/base/State.h"
45 #include "ompl/base/SpaceInformation.h"
46 #include "ompl/util/ClassForward.h"
47 #include <boost/noncopyable.hpp>
48 #include <boost/function.hpp>
49 #include <boost/serialization/access.hpp>
50 
51 namespace ompl
52 {
53  namespace base
54  {
60  {
61  public:
63  PlannerDataVertex (const State* st, int tag = 0) : state_(st), tag_(tag) {}
66  virtual ~PlannerDataVertex (void) {}
67 
69  virtual int getTag (void) const { return tag_; }
71  virtual void setTag (int tag) { tag_ = tag; }
73  virtual const State* getState(void) const { return state_; }
74 
76  virtual PlannerDataVertex* clone (void) const
77  {
78  return new PlannerDataVertex(*this);
79  }
80 
82  virtual bool operator == (const PlannerDataVertex &rhs) const
83  {
84  // States should be unique
85  return state_ == rhs.state_;
86  }
87 
90  bool operator != (const PlannerDataVertex &rhs) const
91  {
92  return !(*this == rhs);
93  }
94 
95  protected:
96  PlannerDataVertex(void) {}
97 
98  friend class boost::serialization::access;
99  template <class Archive>
100  void serialize(Archive & ar, const unsigned int version)
101  {
102  ar & tag_;
103  // Serialization of the state pointer is handled by PlannerDataStorage
104  }
105 
107  const State* state_;
109  int tag_;
110 
111  friend class PlannerData;
112  friend class PlannerDataStorage;
113  };
114 
117  {
118  public:
119  PlannerDataEdge (void) {}
120  virtual ~PlannerDataEdge (void) {}
122  virtual PlannerDataEdge* clone () const { return new PlannerDataEdge(); }
123 
125  virtual bool operator == (const PlannerDataEdge &rhs) const
126  {
127  return this == &rhs;
128  }
129 
132  bool operator != (const PlannerDataEdge &rhs) const
133  {
134  return !(*this == rhs);
135  }
136 
137  protected:
138 
139  friend class boost::serialization::access;
140  template <class Archive>
141  void serialize(Archive & ar, const unsigned int version)
142  {
143  }
144  };
145 
147  ClassForward(PlannerData);
149 
153  class PlannerData : boost::noncopyable
154  {
155  public:
156  class Graph;
158  typedef boost::function<double (const PlannerDataVertex&, const PlannerDataVertex&, const PlannerDataEdge&)> EdgeWeightFn;
159 
161  static const PlannerDataEdge NO_EDGE;
165  static const double INVALID_WEIGHT;
167  static const unsigned int INVALID_INDEX;
168 
170  PlannerData(const SpaceInformationPtr &si);
172  virtual ~PlannerData(void);
173 
176 
181  unsigned int addVertex (const PlannerDataVertex &st);
186  unsigned int addStartVertex (const PlannerDataVertex &v);
191  unsigned int addGoalVertex (const PlannerDataVertex &v);
194  bool markStartState (const State* st);
197  bool markGoalState (const State* st);
200  bool tagState (const State* st, int tag);
204  virtual bool removeVertex (const PlannerDataVertex &st);
208  virtual bool removeVertex (unsigned int vIndex);
211  virtual bool addEdge (unsigned int v1, unsigned int v2,
212  const PlannerDataEdge &edge = PlannerDataEdge(), double weight=1.0);
217  virtual bool addEdge (const PlannerDataVertex &v1, const PlannerDataVertex &v2,
218  const PlannerDataEdge &edge = PlannerDataEdge(), double weight=1.0);
220  virtual bool removeEdge (unsigned int v1, unsigned int v2);
223  virtual bool removeEdge (const PlannerDataVertex &v1, const PlannerDataVertex &v2);
225  virtual void clear (void);
233  virtual void decoupleFromPlanner(void);
234 
238 
240  unsigned int numEdges (void) const;
242  unsigned int numVertices (void) const;
244  unsigned int numStartVertices (void) const;
246  unsigned int numGoalVertices (void) const;
247 
251 
253  bool vertexExists (const PlannerDataVertex &v) const;
256  const PlannerDataVertex& getVertex (unsigned int index) const;
259  PlannerDataVertex& getVertex (unsigned int index);
262  const PlannerDataVertex& getStartVertex (unsigned int i) const;
265  PlannerDataVertex& getStartVertex (unsigned int i);
268  const PlannerDataVertex& getGoalVertex (unsigned int i) const;
271  PlannerDataVertex& getGoalVertex (unsigned int i);
275  unsigned int getStartIndex (unsigned int i) const;
279  unsigned int getGoalIndex (unsigned int i) const;
281  bool isStartVertex (unsigned int index) const;
283  bool isGoalVertex (unsigned int index) const;
287  unsigned int vertexIndex (const PlannerDataVertex &v) const;
288 
292 
294  bool edgeExists (unsigned int v1, unsigned int v2) const;
297  const PlannerDataEdge& getEdge (unsigned int v1, unsigned int v2) const;
300  PlannerDataEdge& getEdge (unsigned int v1, unsigned int v2);
304  unsigned int getEdges (unsigned int v, std::vector<unsigned int>& edgeList) const;
307  unsigned int getEdges (unsigned int v, std::map<unsigned int, const PlannerDataEdge*> &edgeMap) const;
310  unsigned int getIncomingEdges (unsigned int v, std::vector<unsigned int>& edgeList) const;
314  unsigned int getIncomingEdges (unsigned int v, std::map<unsigned int, const PlannerDataEdge*> &edgeMap) const;
317  double getEdgeWeight (unsigned int v1, unsigned int v2) const;
320  bool setEdgeWeight (unsigned int v1, unsigned int v2, double weight);
324  void computeEdgeWeights(const EdgeWeightFn& f = NULL);
325 
329 
331  void printGraphviz (std::ostream& out = std::cout) const;
333  void printGraphML (std::ostream& out = std::cout) const;
334 
338 
342  void extractMinimumSpanningTree (unsigned int v, PlannerData &mst) const;
346  void extractReachable(unsigned int v, PlannerData &data) const;
347 
354  Graph& toBoostGraph (void);
361  const Graph& toBoostGraph (void) const;
362 
364 
366  const SpaceInformationPtr& getSpaceInformation(void) const;
367 
368  virtual bool hasControls(void) const;
369 
371  std::map<std::string, std::string> properties;
372 
373  protected:
374  double defaultEdgeWeight(const PlannerDataVertex &v1, const PlannerDataVertex &v2, const PlannerDataEdge& e) const;
375 
377  std::map<const State*, unsigned int> stateIndexMap_;
379  std::vector<unsigned int> startVertexIndices_;
381  std::vector<unsigned int> goalVertexIndices_;
382 
387  std::set<State*> decoupledStates_;
388 
389  private:
390  void freeMemory(void);
391 
392  // Abstract pointer that points to the Boost.Graph structure.
393  // Obscured to prevent unnecessary inclusion of BGL throughout the
394  // rest of the code.
395  void* graphRaw_;
396  };
397  }
398 }
399 
400 #endif