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  {
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 
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 
367 
371  void extractMinimumSpanningTree(unsigned int v, const OptimizationObjective &opt, PlannerData &mst) const;
375  void extractReachable(unsigned int v, PlannerData &data) const;
376 
379  StateStoragePtr extractStateStorage() const;
380 
387  Graph &toBoostGraph();
394  const Graph &toBoostGraph() const;
395 
397 
399  const SpaceInformationPtr &getSpaceInformation() const;
400 
403  virtual bool hasControls() const;
404 
406  std::map<std::string, std::string> properties;
407 
408  protected:
410  std::map<const State *, unsigned int> stateIndexMap_;
412  std::vector<unsigned int> startVertexIndices_;
414  std::vector<unsigned int> goalVertexIndices_;
415 
420  std::set<State *> decoupledStates_;
421 
422  private:
423  void freeMemory();
424 
425  // Abstract pointer that points to the Boost.Graph structure.
426  // Obscured to prevent unnecessary inclusion of BGL throughout the
427  // rest of the code.
428  void *graphRaw_;
429  };
430  }
431 }
432 
433 #endif
const State * state_
The state represented by this vertex.
Definition: PlannerData.h:117
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:417
Manage loading and storing for a set of states of a specified state space.
Definition: StateStorage.h:61
std::map< const State *, unsigned int > stateIndexMap_
A mapping of states to vertex indexes. For fast lookup of vertex index.
Definition: PlannerData.h:410
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:412
int tag_
A generic integer tag for this state. Not used for equivalence checking.
Definition: PlannerData.h:119
PlannerDataVertex(const State *st, int tag=0)
Constructor. Takes a state pointer and an optional integer tag.
Definition: PlannerData.h:62
virtual bool operator==(const PlannerDataVertex &rhs) const
Equivalence operator. Return true if the state pointers are equal.
Definition: PlannerData.h:92
virtual bool operator==(const PlannerDataEdge &rhs) const
Returns true if the edges point to the same memory.
Definition: PlannerData.h:138
std::vector< unsigned int > goalVertexIndices_
A mutable listing of the vertices marked as goal states. Stored in sorted order.
Definition: PlannerData.h:414
virtual PlannerDataVertex * clone() const
Return a clone of this object, allocated from the heap.
Definition: PlannerData.h:86
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
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:145
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:49
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:100
static const PlannerDataVertex NO_VERTEX
Representation for a non-existant vertex.
Definition: PlannerData.h:182
Abstract definition of optimization objectives.
virtual const State * getState() const
Retrieve the state associated with this vertex.
Definition: PlannerData.h:80
static const unsigned int INVALID_INDEX
Representation of an invalid vertex index.
Definition: PlannerData.h:184
virtual PlannerDataEdge * clone() const
Return a clone of this object, allocated from the heap.
Definition: PlannerData.h:132
virtual void setTag(int tag)
Set the integer tag associated with this vertex.
Definition: PlannerData.h:75
Base class for a PlannerData edge.
Definition: PlannerData.h:126
virtual int getTag() const
Returns the integer tag associated with this vertex.
Definition: PlannerData.h:70
std::set< State * > decoupledStates_
A list of states that are allocated during the decoupleFromPlanner method. These states are freed by ...
Definition: PlannerData.h:420
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:177
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
Definition: PlannerData.h:406