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 
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
void computeEdgeWeights()
Computes all edge weights using state space distance (i.e. getSpaceInformation()->distance()) ...
Wrapper class for the Boost.Graph representation of the PlannerData. This class inherits from a boost...
StateStoragePtr extractStateStorage() const
Extract a ompl::base::GraphStateStorage object from this PlannerData. Memory for states is copied (th...
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
bool vertexExists(const PlannerDataVertex &v) const
Check whether a vertex exists with the given vertex data.
virtual void decoupleFromPlanner()
Creates a deep copy of the states contained in the vertices of this PlannerData structure so that whe...
Definition: PlannerData.cpp:84
PlannerDataVertex(const State *st, int tag=0)
Constructor. Takes a state pointer and an optional integer tag.
Definition: PlannerData.h:64
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance...
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
void extractReachable(unsigned int v, PlannerData &data) const
Extracts the subset of PlannerData reachable from the vertex with index v. For tree structures...
bool markStartState(const State *st)
Mark the given state as a start vertex. If the given state does not exist in a vertex, false is returned.
Graph & toBoostGraph()
Extract a Boost.Graph object from this PlannerData.
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added...
unsigned int numGoalVertices() const
Returns the number of goal vertices.
const PlannerDataVertex & getStartVertex(unsigned int i) const
Retrieve a reference to the ith start vertex object. If i is greater than the number of start vertice...
std::vector< unsigned int > goalVertexIndices_
A mutable listing of the vertices marked as goal states. Stored in sorted order.
Definition: PlannerData.h:402
unsigned int getStartIndex(unsigned int i) const
Returns the index of the ith start state. INVALID_INDEX is returned if i is out of range...
bool getEdgeWeight(unsigned int v1, unsigned int v2, Cost *weight) const
Returns the weight of the edge between the given vertex indices. If there exists an edge between v1 a...
unsigned int getIncomingEdges(unsigned int v, std::vector< unsigned int > &edgeList) const
Returns a list of vertices with outgoing edges to the vertex with index v. The number of edges connec...
PlannerData(const SpaceInformationPtr &si)
Constructor. Accepts a SpaceInformationPtr for the space planned in.
Definition: PlannerData.cpp:62
bool setEdgeWeight(unsigned int v1, unsigned int v2, Cost weight)
Sets the weight of the edge between the given vertex indices. If an edge between v1 and v2 does not e...
unsigned int getEdges(unsigned int v, std::vector< unsigned int > &edgeList) const
Returns a list of the vertex indexes directly connected to vertex with index v (outgoing edges)...
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
virtual bool removeVertex(const PlannerDataVertex &st)
Removes the vertex associated with the given data. If the vertex does not exist, false is returned...
bool isGoalVertex(unsigned int index) const
Returns true if the given vertex index is marked as a goal vertex.
const PlannerDataVertex & getGoalVertex(unsigned int i) const
Retrieve a reference to the ith goal vertex object. If i is greater than the number of goal vertices...
bool tagState(const State *st, int tag)
Set the integer tag associated with the given state. If the given state does not exist in a vertex...
unsigned int numEdges() const
Retrieve the number of edges in this structure.
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
void printGraphML(std::ostream &out=std::cout) const
Writes a GraphML file of this structure to the given stream.
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
unsigned int vertexIndex(const PlannerDataVertex &v) const
Return the index for the vertex associated with the given data. INVALID_INDEX is returned if this ver...
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
PlannerDataVertex(const PlannerDataVertex &rhs)
Copy constructor.
Definition: PlannerData.h:66
A boost shared pointer wrapper for ompl::base::SpaceInformation.
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist...
void printGraphviz(std::ostream &out=std::cout) const
Writes a Graphviz dot file of this structure to the given stream.
virtual void clear()
Clears the entire data structure.
Definition: PlannerData.cpp:78
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition of an abstract state.
Definition: State.h:50
virtual ~PlannerData()
Destructor.
Definition: PlannerData.cpp:67
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.
bool edgeExists(unsigned int v1, unsigned int v2) const
Check whether an edge between vertex index v1 and index v2 exists.
static const unsigned int INVALID_INDEX
Representation of an invalid vertex index.
Definition: PlannerData.h:174
const PlannerDataEdge & getEdge(unsigned int v1, unsigned int v2) const
Retrieve a reference to the edge object connecting vertices with indexes v1 and v2. If this edge does not exist, NO_EDGE is returned.
bool markGoalState(const State *st)
Mark the given state as a goal vertex. If the given state does not exist in a vertex, false is returned.
virtual int getTag() const
Returns the integer tag associated with this vertex.
Definition: PlannerData.h:70
bool isStartVertex(unsigned int index) const
Returns true if the given vertex index is marked as a start vertex.
virtual void setTag(int tag)
Set the integer tag associated with this vertex.
Definition: PlannerData.h:72
unsigned int numStartVertices() const
Returns the number of start vertices.
void extractMinimumSpanningTree(unsigned int v, const OptimizationObjective &opt, PlannerData &mst) const
Extracts the minimum spanning tree of the data rooted at the vertex with index v. The minimum spannin...
Base class for a PlannerData edge.
Definition: PlannerData.h:117
const SpaceInformationPtr & getSpaceInformation() const
Return the instance of SpaceInformation used in this PlannerData.
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
unsigned int getGoalIndex(unsigned int i) const
Returns the index of the ith goal state. INVALID_INDEX is returned if i is out of range Indexes are v...
virtual bool removeEdge(unsigned int v1, unsigned int v2)
Removes the edge between vertex indexes v1 and v2. Success is returned.
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