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/serialization/access.hpp>
49 
50 namespace ompl
51 {
52  namespace base
53  {
58  class PlannerDataVertex
59  {
60  public:
62  PlannerDataVertex(const State *st, int tag = 0) : state_(st), tag_(tag)
63  {
64  }
66  PlannerDataVertex(const PlannerDataVertex &rhs) = default;
67  virtual ~PlannerDataVertex() = default;
68 
70  virtual int getTag() const
71  {
72  return tag_;
73  }
75  virtual void setTag(int tag)
76  {
77  tag_ = tag;
78  }
80  virtual const State *getState() const
81  {
82  return state_;
83  }
84 
86  virtual PlannerDataVertex *clone() const
87  {
88  return new PlannerDataVertex(*this);
89  }
90 
92  virtual bool operator==(const PlannerDataVertex &rhs) const
93  {
94  // States should be unique
95  return state_ == rhs.state_;
96  }
97 
100  bool operator!=(const PlannerDataVertex &rhs) const
101  {
102  return !(*this == rhs);
103  }
104 
105  protected:
106  PlannerDataVertex() = default;
107 
108  friend class boost::serialization::access;
109  template <class Archive>
110  void serialize(Archive &ar, const unsigned int /*version*/)
111  {
112  ar &tag_;
113  // Serialization of the state pointer is handled by PlannerDataStorage
114  }
115 
117  const State *state_;
119  int tag_;
120 
121  friend class PlannerData;
122  friend class PlannerDataStorage;
123  };
124 
126  class PlannerDataEdge
127  {
128  public:
129  PlannerDataEdge() = default;
130  virtual ~PlannerDataEdge() = default;
132  virtual PlannerDataEdge *clone() const
133  {
134  return new PlannerDataEdge();
135  }
136 
138  virtual bool operator==(const PlannerDataEdge &rhs) const
139  {
140  return this == &rhs;
141  }
142 
145  bool operator!=(const PlannerDataEdge &rhs) const
146  {
147  return !(*this == rhs);
148  }
149 
150  protected:
151  friend class boost::serialization::access;
152  template <class Archive>
153  void serialize(Archive & /*ar*/, const unsigned int /*version*/)
154  {
155  }
156  };
157 
159  OMPL_CLASS_FORWARD(StateStorage);
160  OMPL_CLASS_FORWARD(PlannerData);
161 
162  // Forward declaration for PlannerData::computeEdgeWeights
163  class OptimizationObjective;
165 
169  class PlannerData
175  {
176  public:
177  class Graph;
178 
180  static const PlannerDataEdge NO_EDGE;
184  static const unsigned int INVALID_INDEX;
185 
186  // non-copyable
187  PlannerData(const PlannerData &) = delete;
188  PlannerData &operator=(const PlannerData &) = delete;
189 
193  virtual ~PlannerData();
194 
197 
202  unsigned int addVertex(const PlannerDataVertex &st);
207  unsigned int addStartVertex(const PlannerDataVertex &v);
212  unsigned int addGoalVertex(const PlannerDataVertex &v);
215  bool markStartState(const State *st);
218  bool markGoalState(const State *st);
221  bool tagState(const State *st, int tag);
225  virtual bool removeVertex(const PlannerDataVertex &st);
229  virtual bool removeVertex(unsigned int vIndex);
232  virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge = PlannerDataEdge(),
233  Cost weight = Cost(1.0));
238  virtual bool addEdge(const PlannerDataVertex &v1, const PlannerDataVertex &v2,
239  const PlannerDataEdge &edge = PlannerDataEdge(), Cost weight = Cost(1.0));
241  virtual bool removeEdge(unsigned int v1, unsigned int v2);
244  virtual bool removeEdge(const PlannerDataVertex &v1, const PlannerDataVertex &v2);
246  virtual void clear();
254  virtual void decoupleFromPlanner();
255 
259 
261  unsigned int numEdges() const;
263  unsigned int numVertices() const;
265  unsigned int numStartVertices() const;
267  unsigned int numGoalVertices() const;
268 
272 
274  bool vertexExists(const PlannerDataVertex &v) const;
277  const PlannerDataVertex &getVertex(unsigned int index) const;
280  PlannerDataVertex &getVertex(unsigned int index);
283  const PlannerDataVertex &getStartVertex(unsigned int i) const;
286  PlannerDataVertex &getStartVertex(unsigned int i);
289  const PlannerDataVertex &getGoalVertex(unsigned int i) const;
292  PlannerDataVertex &getGoalVertex(unsigned int i);
296  unsigned int getStartIndex(unsigned int i) const;
300  unsigned int getGoalIndex(unsigned int i) const;
302  bool isStartVertex(unsigned int index) const;
304  bool isGoalVertex(unsigned int index) const;
308  unsigned int vertexIndex(const PlannerDataVertex &v) const;
309 
313 
315  bool edgeExists(unsigned int v1, unsigned int v2) const;
318  const PlannerDataEdge &getEdge(unsigned int v1, unsigned int v2) const;
321  PlannerDataEdge &getEdge(unsigned int v1, unsigned int v2);
325  unsigned int getEdges(unsigned int v, std::vector<unsigned int> &edgeList) const;
328  unsigned int getEdges(unsigned int v, std::map<unsigned int, const PlannerDataEdge *> &edgeMap) const;
331  unsigned int getIncomingEdges(unsigned int v, std::vector<unsigned int> &edgeList) const;
335  unsigned int getIncomingEdges(unsigned int v,
336  std::map<unsigned int, const PlannerDataEdge *> &edgeMap) const;
342  bool getEdgeWeight(unsigned int v1, unsigned int v2, Cost *weight) const;
346  bool setEdgeWeight(unsigned int v1, unsigned int v2, Cost weight);
349  void computeEdgeWeights(const OptimizationObjective &opt);
352  void computeEdgeWeights();
353 
357 
359  void printGraphviz(std::ostream &out = std::cout) const;
360 
362  void printGraphML(std::ostream &out = std::cout) const;
363 
366  void printPLY(std::ostream &out, bool asIs = false) const;
367 
371 
375  void extractMinimumSpanningTree(unsigned int v, const OptimizationObjective &opt, PlannerData &mst) const;
379  void extractReachable(unsigned int v, PlannerData &data) const;
380 
383  StateStoragePtr extractStateStorage() const;
384 
391  Graph &toBoostGraph();
398  const Graph &toBoostGraph() const;
399 
401 
404 
407  virtual bool hasControls() const;
408 
410  std::map<std::string, std::string> properties;
411 
412  protected:
414  std::map<const State *, unsigned int> stateIndexMap_;
416  std::vector<unsigned int> startVertexIndices_;
418  std::vector<unsigned int> goalVertexIndices_;
419 
424  std::set<State *> decoupledStates_;
425 
426  private:
427  void freeMemory();
428 
429  // Abstract pointer that points to the Boost.Graph structure.
430  // Obscured to prevent unnecessary inclusion of BGL throughout the
431  // rest of the code.
432  void *graphRaw_;
433  };
434  }
435 }
436 
437 #endif
bool edgeExists(unsigned int v1, unsigned int v2) const
Check whether an edge between vertex index v1 and index v2 exists.
virtual bool operator==(const PlannerDataVertex &rhs) const
Equivalence operator. Return true if the state pointers are equal.
Definition: PlannerData.h:188
unsigned int numGoalVertices() const
Returns the number of goal vertices.
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 getStartIndex(unsigned int i) const
Returns the index of the ith start state. INVALID_INDEX is returned if i is out of range....
virtual bool removeEdge(unsigned int v1, unsigned int v2)
Removes the edge between vertex indexes v1 and v2. Success is returned.
A shared pointer wrapper for ompl::base::SpaceInformation.
virtual int getTag() const
Returns the integer tag associated with this vertex.
Definition: PlannerData.h:166
virtual PlannerDataEdge * clone() const
Return a clone of this object, allocated from the heap.
Definition: PlannerData.h:196
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)....
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:209
const SpaceInformationPtr & getSpaceInformation() const
Return the instance of SpaceInformation used in this PlannerData.
bool isStartVertex(unsigned int index) const
Returns true if the given vertex index is marked as a start vertex.
Wrapper class for the Boost.Graph representation of the PlannerData. This class inherits from a boost...
bool isGoalVertex(unsigned int index) const
Returns true if the given vertex index is marked as a goal vertex.
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:196
static const PlannerDataEdge NO_EDGE
Representation for a non-existant edge.
Definition: PlannerData.h:241
Definition of an abstract state.
Definition: State.h:113
virtual PlannerDataVertex * clone() const
Return a clone of this object, allocated from the heap.
Definition: PlannerData.h:182
bool vertexExists(const PlannerDataVertex &v) const
Check whether a vertex exists with the given vertex data.
void printPLY(std::ostream &out, bool asIs=false) const
Write a mesh of the planner graph to a stream. Insert additional vertices if asIs == true.
virtual void setTag(int tag)
Set the integer tag associated with this vertex.
Definition: PlannerData.h:171
std::vector< unsigned int > startVertexIndices_
A mutable listing of the vertices marked as start states. Stored in sorted order.
Definition: PlannerData.h:480
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
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,...
virtual void decoupleFromPlanner()
Creates a deep copy of the states contained in the vertices of this PlannerData structure so that whe...
Definition: PlannerData.cpp:80
unsigned int numEdges() const
Retrieve the number of edges in this structure.
StateStoragePtr extractStateStorage() const
Extract a ompl::base::GraphStateStorage object from this PlannerData. Memory for states is copied (th...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
static const unsigned int INVALID_INDEX
Representation of an invalid vertex index.
Definition: PlannerData.h:248
Abstract definition of optimization objectives.
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 numStartVertices() const
Returns the number of start vertices.
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...
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...
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...
Object that handles loading/storing a PlannerData object to/from a binary stream. Serialization of ve...
bool markGoalState(const State *st)
Mark the given state as a goal vertex. If the given state does not exist in a vertex,...
void printGraphviz(std::ostream &out=std::cout) const
Writes a Graphviz dot file of this structure to the given stream.
void extractReachable(unsigned int v, PlannerData &data) const
Extracts the subset of PlannerData reachable from the vertex with index v. For tree structures,...
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist,...
std::set< State * > decoupledStates_
A list of states that are allocated during the decoupleFromPlanner method. These states are freed by ...
Definition: PlannerData.h:488
virtual const State * getState() const
Retrieve the state associated with this vertex.
Definition: PlannerData.h:176
bool markStartState(const State *st)
Mark the given state as a start vertex. If the given state does not exist in a vertex,...
PlannerDataVertex(const State *st, int tag=0)
Constructor. Takes a state pointer and an optional integer tag.
Definition: PlannerData.h:158
Base class for a PlannerData edge.
Definition: PlannerData.h:190
virtual bool removeVertex(const PlannerDataVertex &st)
Removes the vertex associated with the given data. If the vertex does not exist, false is returned....
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
Definition: PlannerData.h:474
std::vector< unsigned int > goalVertexIndices_
A mutable listing of the vertices marked as goal states. Stored in sorted order.
Definition: PlannerData.h:482
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,...
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...
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...
SpaceInformationPtr si_
The space information instance for this data.
Definition: PlannerData.h:485
std::map< const State *, unsigned int > stateIndexMap_
A mapping of states to vertex indexes. For fast lookup of vertex index.
Definition: PlannerData.h:478
virtual void clear()
Clears the entire data structure.
Definition: PlannerData.cpp:74
void computeEdgeWeights()
Computes all edge weights using state space distance (i.e. getSpaceInformation()->distance())
virtual bool operator==(const PlannerDataEdge &rhs) const
Returns true if the edges point to the same memory.
Definition: PlannerData.h:202
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...
const State * state_
The state represented by this vertex.
Definition: PlannerData.h:213
static const PlannerDataVertex NO_VERTEX
Representation for a non-existant vertex.
Definition: PlannerData.h:246
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...
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...
Graph & toBoostGraph()
Extract a Boost.Graph object from this PlannerData.
int tag_
A generic integer tag for this state. Not used for equivalence checking.
Definition: PlannerData.h:215
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122
virtual ~PlannerData()
Destructor.
Definition: PlannerData.cpp:63
Main namespace. Contains everything in this library.
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance.
const PlannerDataEdge & getEdge(unsigned int v1, unsigned int v2) const
Retrieve a reference to the edge object connecting vertices with indexes v1 and v2....
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...