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  OMPL_CLASS_FORWARD(PlannerData);
149 
154 
155 
156 
157 
158 
159  class PlannerData : boost::noncopyable
160  {
161  public:
162  class Graph;
164  typedef boost::function<double (const PlannerDataVertex&, const PlannerDataVertex&, const PlannerDataEdge&)> EdgeWeightFn;
165 
167  static const PlannerDataEdge NO_EDGE;
171  static const double INVALID_WEIGHT;
173  static const unsigned int INVALID_INDEX;
174 
176  PlannerData(const SpaceInformationPtr &si);
178  virtual ~PlannerData(void);
179 
182 
187  unsigned int addVertex (const PlannerDataVertex &st);
192  unsigned int addStartVertex (const PlannerDataVertex &v);
197  unsigned int addGoalVertex (const PlannerDataVertex &v);
200  bool markStartState (const State* st);
203  bool markGoalState (const State* st);
206  bool tagState (const State* st, int tag);
210  virtual bool removeVertex (const PlannerDataVertex &st);
214  virtual bool removeVertex (unsigned int vIndex);
217  virtual bool addEdge (unsigned int v1, unsigned int v2,
218  const PlannerDataEdge &edge = PlannerDataEdge(), double weight=1.0);
223  virtual bool addEdge (const PlannerDataVertex &v1, const PlannerDataVertex &v2,
224  const PlannerDataEdge &edge = PlannerDataEdge(), double weight=1.0);
226  virtual bool removeEdge (unsigned int v1, unsigned int v2);
229  virtual bool removeEdge (const PlannerDataVertex &v1, const PlannerDataVertex &v2);
231  virtual void clear (void);
239  virtual void decoupleFromPlanner(void);
240 
244 
246  unsigned int numEdges (void) const;
248  unsigned int numVertices (void) const;
250  unsigned int numStartVertices (void) const;
252  unsigned int numGoalVertices (void) const;
253 
257 
259  bool vertexExists (const PlannerDataVertex &v) const;
262  const PlannerDataVertex& getVertex (unsigned int index) const;
265  PlannerDataVertex& getVertex (unsigned int index);
268  const PlannerDataVertex& getStartVertex (unsigned int i) const;
271  PlannerDataVertex& getStartVertex (unsigned int i);
274  const PlannerDataVertex& getGoalVertex (unsigned int i) const;
277  PlannerDataVertex& getGoalVertex (unsigned int i);
281  unsigned int getStartIndex (unsigned int i) const;
285  unsigned int getGoalIndex (unsigned int i) const;
287  bool isStartVertex (unsigned int index) const;
289  bool isGoalVertex (unsigned int index) const;
293  unsigned int vertexIndex (const PlannerDataVertex &v) const;
294 
298 
300  bool edgeExists (unsigned int v1, unsigned int v2) const;
303  const PlannerDataEdge& getEdge (unsigned int v1, unsigned int v2) const;
306  PlannerDataEdge& getEdge (unsigned int v1, unsigned int v2);
310  unsigned int getEdges (unsigned int v, std::vector<unsigned int>& edgeList) const;
313  unsigned int getEdges (unsigned int v, std::map<unsigned int, const PlannerDataEdge*> &edgeMap) const;
316  unsigned int getIncomingEdges (unsigned int v, std::vector<unsigned int>& edgeList) const;
320  unsigned int getIncomingEdges (unsigned int v, std::map<unsigned int, const PlannerDataEdge*> &edgeMap) const;
323  double getEdgeWeight (unsigned int v1, unsigned int v2) const;
326  bool setEdgeWeight (unsigned int v1, unsigned int v2, double weight);
330  void computeEdgeWeights(const EdgeWeightFn& f = NULL);
331 
335 
337  void printGraphviz (std::ostream& out = std::cout) const;
339  void printGraphML (std::ostream& out = std::cout) const;
340 
344 
348  void extractMinimumSpanningTree (unsigned int v, PlannerData &mst) const;
352  void extractReachable(unsigned int v, PlannerData &data) const;
353 
360  Graph& toBoostGraph (void);
367  const Graph& toBoostGraph (void) const;
368 
370 
372  const SpaceInformationPtr& getSpaceInformation(void) const;
373 
375  virtual bool hasControls(void) const;
376 
378  std::map<std::string, std::string> properties;
379 
380  protected:
381  double defaultEdgeWeight(const PlannerDataVertex &v1, const PlannerDataVertex &v2, const PlannerDataEdge& e) const;
382 
384  std::map<const State*, unsigned int> stateIndexMap_;
386  std::vector<unsigned int> startVertexIndices_;
388  std::vector<unsigned int> goalVertexIndices_;
389 
394  std::set<State*> decoupledStates_;
395 
396  private:
397  void freeMemory(void);
398 
399  // Abstract pointer that points to the Boost.Graph structure.
400  // Obscured to prevent unnecessary inclusion of BGL throughout the
401  // rest of the code.
402  void* graphRaw_;
403  };
404  }
405 }
406 
407 #endif