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, Luis G. Torres */
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/Cost.h"
46 #include "ompl/base/SpaceInformation.h"
47 #include "ompl/util/ClassForward.h"
48 #include <boost/noncopyable.hpp>
49 #include <boost/function.hpp>
50 #include <boost/serialization/access.hpp>
51 
52 namespace ompl
53 {
54  namespace base
55  {
61  {
62  public:
64  PlannerDataVertex(const State *st, int tag = 0) : state_(st), tag_(tag) {}
67  virtual ~PlannerDataVertex() {}
68 
70  virtual int getTag() const { return tag_; }
72  virtual void setTag(int tag) { tag_ = tag; }
74  virtual const State* getState() const { return state_; }
75 
77  virtual PlannerDataVertex* clone() const
78  {
79  return new PlannerDataVertex(*this);
80  }
81 
83  virtual bool operator==(const PlannerDataVertex &rhs) const
84  {
85  // States should be unique
86  return state_ == rhs.state_;
87  }
88 
91  bool operator!=(const PlannerDataVertex &rhs) const
92  {
93  return !(*this == rhs);
94  }
95 
96  protected:
98 
99  friend class boost::serialization::access;
100  template <class Archive>
101  void serialize(Archive & ar, const unsigned int /*version*/)
102  {
103  ar & tag_;
104  // Serialization of the state pointer is handled by PlannerDataStorage
105  }
106 
108  const State *state_;
110  int tag_;
111 
112  friend class PlannerData;
113  friend class PlannerDataStorage;
114  };
115 
118  {
119  public:
120  PlannerDataEdge() {}
121  virtual ~PlannerDataEdge() {}
123  virtual PlannerDataEdge* clone() const { return new PlannerDataEdge(); }
124 
126  virtual bool operator==(const PlannerDataEdge &rhs) const
127  {
128  return this == &rhs;
129  }
130 
133  bool operator!=(const PlannerDataEdge &rhs) const
134  {
135  return !(*this == rhs);
136  }
137 
138  protected:
139 
140  friend class boost::serialization::access;
141  template <class Archive>
142  void serialize(Archive & /*ar*/, const unsigned int /*version*/)
143  {
144  }
145  };
146 
148  OMPL_CLASS_FORWARD(StateStorage);
149  OMPL_CLASS_FORWARD(PlannerData);
150 
151  // Forward declaration for PlannerData::computeEdgeWeights
152  class OptimizationObjective;
154 
159  class PlannerData : boost::noncopyable
165  {
166  public:
167  class Graph;
168 
170  static const PlannerDataEdge NO_EDGE;
174  static const unsigned int INVALID_INDEX;
175 
177  PlannerData(const SpaceInformationPtr &si);
179  virtual ~PlannerData();
180 
183 
188  unsigned int addVertex(const PlannerDataVertex &st);
193  unsigned int addStartVertex(const PlannerDataVertex &v);
198  unsigned int addGoalVertex(const PlannerDataVertex &v);
201  bool markStartState(const State *st);
204  bool markGoalState(const State *st);
207  bool tagState(const State *st, int tag);
211  virtual bool removeVertex(const PlannerDataVertex &st);
215  virtual bool removeVertex(unsigned int vIndex);
218  virtual bool addEdge(unsigned int v1, unsigned int v2,
219  const PlannerDataEdge &edge = PlannerDataEdge(),
220  Cost weight = Cost(1.0));
225  virtual bool addEdge(const PlannerDataVertex &v1, const PlannerDataVertex &v2,
226  const PlannerDataEdge &edge = PlannerDataEdge(),
227  Cost weight = Cost(1.0));
229  virtual bool removeEdge(unsigned int v1, unsigned int v2);
232  virtual bool removeEdge(const PlannerDataVertex &v1, const PlannerDataVertex &v2);
234  virtual void clear();
242  virtual void decoupleFromPlanner();
243 
247 
249  unsigned int numEdges() const;
251  unsigned int numVertices() const;
253  unsigned int numStartVertices() const;
255  unsigned int numGoalVertices() const;
256 
260 
262  bool vertexExists(const PlannerDataVertex &v) const;
265  const PlannerDataVertex& getVertex(unsigned int index) const;
268  PlannerDataVertex& getVertex(unsigned int index);
271  const PlannerDataVertex& getStartVertex(unsigned int i) const;
274  PlannerDataVertex& getStartVertex(unsigned int i);
277  const PlannerDataVertex& getGoalVertex(unsigned int i) const;
280  PlannerDataVertex& getGoalVertex(unsigned int i);
284  unsigned int getStartIndex(unsigned int i) const;
288  unsigned int getGoalIndex(unsigned int i) const;
290  bool isStartVertex(unsigned int index) const;
292  bool isGoalVertex(unsigned int index) const;
296  unsigned int vertexIndex(const PlannerDataVertex &v) const;
297 
301 
303  bool edgeExists(unsigned int v1, unsigned int v2) const;
306  const PlannerDataEdge& getEdge(unsigned int v1, unsigned int v2) const;
309  PlannerDataEdge& getEdge(unsigned int v1, unsigned int v2);
313  unsigned int getEdges(unsigned int v, std::vector<unsigned int>& edgeList) const;
316  unsigned int getEdges(unsigned int v, std::map<unsigned int, const PlannerDataEdge*> &edgeMap) const;
319  unsigned int getIncomingEdges(unsigned int v, std::vector<unsigned int>& edgeList) const;
323  unsigned int getIncomingEdges(unsigned int v, std::map<unsigned int, const PlannerDataEdge*> &edgeMap) const;
329  bool getEdgeWeight(unsigned int v1, unsigned int v2, Cost* weight) const;
333  bool setEdgeWeight(unsigned int v1, unsigned int v2, Cost weight);
336  void computeEdgeWeights(const OptimizationObjective &opt);
339  void computeEdgeWeights();
340 
344 
346  void printGraphviz(std::ostream& out = std::cout) const;
347 
349  void printGraphML(std::ostream& out = std::cout) const;
350 
354 
358  void extractMinimumSpanningTree(unsigned int v,
359  const OptimizationObjective &opt,
360  PlannerData &mst) const;
364  void extractReachable(unsigned int v, PlannerData &data) const;
365 
368  StateStoragePtr extractStateStorage() const;
369 
376  Graph& toBoostGraph();
383  const Graph& toBoostGraph() const;
384 
386 
388  const SpaceInformationPtr& getSpaceInformation() const;
389 
391  virtual bool hasControls() const;
392 
394  std::map<std::string, std::string> properties;
395 
396  protected:
398  std::map<const State*, unsigned int> stateIndexMap_;
400  std::vector<unsigned int> startVertexIndices_;
402  std::vector<unsigned int> goalVertexIndices_;
403 
408  std::set<State*> decoupledStates_;
409 
410  private:
411  void freeMemory();
412 
413  // Abstract pointer that points to the Boost.Graph structure.
414  // Obscured to prevent unnecessary inclusion of BGL throughout the
415  // rest of the code.
416  void* graphRaw_;
417  };
418  }
419 }
420 
421 #endif
const State * state_
The state represented by this vertex.
Definition: PlannerData.h:108
Object that handles loading/storing a PlannerData object to/from a binary stream. Serialization of ve...
SpaceInformationPtr si_
The space information instance for this data.
Definition: PlannerData.h:405
Manage loading and storing for a set of states of a specified state space.
Definition: StateStorage.h:61
Wrapper class for the Boost.Graph representation of the PlannerData. This class inherits from a boost...
std::vector< unsigned int > startVertexIndices_
A mutable listing of the vertices marked as start states. Stored in sorted order. ...
Definition: PlannerData.h:400
int tag_
A generic integer tag for this state. Not used for equivalence checking.
Definition: PlannerData.h:110
PlannerDataVertex(const State *st, int tag=0)
Constructor. Takes a state pointer and an optional integer tag.
Definition: PlannerData.h:64
std::vector< unsigned int > goalVertexIndices_
A mutable listing of the vertices marked as goal states. Stored in sorted order.
Definition: PlannerData.h:402
std::set< State * > decoupledStates_
A list of states that are allocated during the decoupleFromPlanner method. These states are freed by ...
Definition: PlannerData.h:408
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
Main namespace. Contains everything in this library.
Definition: Cost.h:42
bool operator!=(const PlannerDataVertex &rhs) const
Returns true if this vertex is not equal to the argument. This is the complement of the == operator...
Definition: PlannerData.h:91
PlannerDataVertex(const PlannerDataVertex &rhs)
Copy constructor.
Definition: PlannerData.h:66
A boost shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
virtual PlannerDataEdge * clone() const
Return a clone of this object, allocated from the heap.
Definition: PlannerData.h:123
static const PlannerDataVertex NO_VERTEX
Representation for a non-existant vertex.
Definition: PlannerData.h:172
Abstract definition of optimization objectives.
static const unsigned int INVALID_INDEX
Representation of an invalid vertex index.
Definition: PlannerData.h:174
virtual int getTag() const
Returns the integer tag associated with this vertex.
Definition: PlannerData.h:70
virtual void setTag(int tag)
Set the integer tag associated with this vertex.
Definition: PlannerData.h:72
Base class for a PlannerData edge.
Definition: PlannerData.h:117
std::map< const State *, unsigned int > stateIndexMap_
A mapping of states to vertex indexes. For fast lookup of vertex index.
Definition: PlannerData.h:398
virtual PlannerDataVertex * clone() const
Return a clone of this object, allocated from the heap.
Definition: PlannerData.h:77
virtual bool operator==(const PlannerDataEdge &rhs) const
Returns true if the edges point to the same memory.
Definition: PlannerData.h:126
virtual const State * getState() const
Retrieve the state associated with this vertex.
Definition: PlannerData.h:74
virtual bool operator==(const PlannerDataVertex &rhs) const
Equivalence operator. Return true if the state pointers are equal.
Definition: PlannerData.h:83
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
static const PlannerDataEdge NO_EDGE
Representation for a non-existant edge.
Definition: PlannerData.h:167
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
Definition: PlannerData.h:394
bool operator!=(const PlannerDataEdge &rhs) const
Returns true if the edges do not point to the same memory. This is the complement of the == operator...
Definition: PlannerData.h:133