All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
PlannerDataStorage.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_STORAGE_
38 #define OMPL_BASE_PLANNER_DATA_STORAGE_
39 
40 // PlannerDataStorage requires Boost version >= 1.44
41 #include <boost/version.hpp>
42 #if BOOST_VERSION < 104400
43 #warning Boost version >= 1.44 is required for PlannerDataStorage classes
44 #else
45 
46 #include "ompl/base/PlannerData.h"
47 #include "ompl/util/Console.h"
48 #include <boost/archive/binary_oarchive.hpp>
49 #include <boost/archive/binary_iarchive.hpp>
50 #include <boost/serialization/vector.hpp>
51 #include <boost/serialization/utility.hpp>
52 #include <fstream>
53 
54 namespace ompl
55 {
56  namespace base
57  {
86  {
87  public:
88 
90  PlannerDataStorage(void);
92  virtual ~PlannerDataStorage(void);
93 
95  virtual void store(const PlannerData& pd, const char *filename);
96 
98  virtual void store(const PlannerData& pd, std::ostream &out);
99 
103  virtual void load(const char *filename, PlannerData& pd);
104 
108  virtual void load(std::istream &in, PlannerData& pd);
109 
110  protected:
112  struct Header
113  {
115  boost::uint32_t marker;
116 
118  std::size_t vertex_count;
119 
121  std::size_t edge_count;
122 
124  std::vector<int> signature;
125 
127  template<typename Archive>
128  void serialize(Archive & ar, const unsigned int /*version*/)
129  {
130  ar & marker;
131  ar & vertex_count;
132  ar & edge_count;
133  ar & signature;
134  }
135  };
136 
139  {
140  enum VertexType
141  {
142  STANDARD = 0,
143  START,
144  GOAL
145  };
146 
147  template<typename Archive>
148  void serialize(Archive & ar, const unsigned int /*version*/)
149  {
150  ar & v_;
151  ar & state_;
152  ar & type_;
153  }
154 
155  const PlannerDataVertex* v_;
156  std::vector<unsigned char> state_;
157  VertexType type_;
158  };
159 
162  {
163  template<typename Archive>
164  void serialize(Archive & ar, const unsigned int /*version*/)
165  {
166  ar & e_;
167  ar & endpoints_;
168  ar & weight_;
169  }
170 
171  const PlannerDataEdge* e_;
172  std::pair<unsigned int, unsigned int> endpoints_;
173  double weight_;
174  };
175 
177  virtual void loadVertices(PlannerData &pd, unsigned int numVertices, boost::archive::binary_iarchive &ia)
178  {
179  OMPL_DEBUG("Loading %d PlannerDataVertex objects", numVertices);
180 
181  const StateSpacePtr &space = pd.getSpaceInformation()->getStateSpace();
182  std::vector<State*> states;
183  for (unsigned int i = 0; i < numVertices; ++i)
184  {
185  PlannerDataVertexData vertexData;
186  ia >> vertexData;
187 
188  // Deserializing all data in the vertex (except the state)
189  const PlannerDataVertex *v = vertexData.v_;
190 
191  // Allocating a new state and deserializing it from the buffer
192  State* state = space->allocState();
193  states.push_back(state);
194  space->deserialize (state, &vertexData.state_[0]);
195  const_cast<PlannerDataVertex*>(v)->state_ = state;
196 
197  // Record the type of the vertex (i.e. start vertex).
198  if (vertexData.type_ == PlannerDataVertexData::START)
199  pd.addStartVertex(*v);
200  else if (vertexData.type_ == PlannerDataVertexData::GOAL)
201  pd.addGoalVertex(*v);
202  else
203  pd.addVertex(*v);
204 
205  // We deserialized the vertex object pointer, and we own it.
206  // Since addEdge copies the object, it is safe to free here.
207  delete vertexData.v_;
208  }
209 
210  // These vertices are using state pointers allocated here.
211  // To avoid a memory leak, we decouple planner data from the
212  // 'planner', which will clone all states and properly free the
213  // memory when PlannerData goes out of scope. Then it is safe
214  // to free all memory allocated here.
215  pd.decoupleFromPlanner();
216 
217  for (size_t i = 0; i < states.size(); ++i)
218  space->freeState(states[i]);
219  }
220 
222  virtual void storeVertices(const PlannerData &pd, boost::archive::binary_oarchive &oa)
223  {
224  OMPL_DEBUG("Storing %d PlannerDataVertex objects", pd.numVertices());
225 
226  const StateSpacePtr &space = pd.getSpaceInformation()->getStateSpace();
227  std::vector<unsigned char> state (space->getSerializationLength());
228  for (unsigned int i = 0; i < pd.numVertices(); ++i)
229  {
230  PlannerDataVertexData vertexData;
231 
232  // Serializing all data in the vertex (except the state)
233  const PlannerDataVertex &v = pd.getVertex(i);
234  vertexData.v_ = &v;
235 
236  // Record the type of the vertex (i.e. start vertex).
237  if (pd.isStartVertex(i))
238  vertexData.type_ = PlannerDataVertexData::START;
239  else if (pd.isGoalVertex(i))
240  vertexData.type_ = PlannerDataVertexData::GOAL;
241  else vertexData.type_ = PlannerDataVertexData::STANDARD;
242 
243  // Serializing the state contained in this vertex
244  space->serialize (&state[0], v.getState());
245  vertexData.state_ = state;
246 
247  oa << vertexData;
248  }
249  }
250 
252  virtual void loadEdges(PlannerData &pd, unsigned int numEdges, boost::archive::binary_iarchive &ia)
253  {
254  OMPL_DEBUG("Loading %d PlannerDataEdge objects", numEdges);
255 
256  for (unsigned int i = 0; i < numEdges; ++i)
257  {
258  PlannerDataEdgeData edgeData;
259  ia >> edgeData;
260  pd.addEdge(edgeData.endpoints_.first, edgeData.endpoints_.second, *edgeData.e_, Cost(edgeData.weight_));
261 
262  // We deserialized the edge object pointer, and we own it.
263  // Since addEdge copies the object, it is safe to free here.
264  delete edgeData.e_;
265  }
266  }
267 
269  virtual void storeEdges(const PlannerData &pd, boost::archive::binary_oarchive &oa)
270  {
271  OMPL_DEBUG("Storing %d PlannerDataEdge objects", pd.numEdges());
272 
273  for (unsigned int i = 0; i < pd.numVertices(); ++i)
274  for (unsigned int j = 0; j < pd.numVertices(); ++j)
275  {
276  if(pd.edgeExists(i, j))
277  {
278  PlannerDataEdgeData edgeData;
279  edgeData.e_ = &pd.getEdge(i, j);
280  edgeData.endpoints_.first = i;
281  edgeData.endpoints_.second = j;
282  Cost weight;
283  pd.getEdgeWeight(i, j, &weight);
284  edgeData.weight_ = weight.v;
285 
286  oa << edgeData;
287  }
288  }
289  }
290  };
291  }
292 }
293 
294 #endif
295 
296 #endif
Object that handles loading/storing a PlannerData object to/from a binary stream. Serialization of ve...
unsigned int numVertices(void) const
Retrieve the number of vertices in this structure.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
The object containing all edge data that will be stored.
A boost shared pointer wrapper for ompl::base::StateSpace.
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...
virtual void storeEdges(const PlannerData &pd, boost::archive::binary_oarchive &oa)
Serialize and store all edges in pd to the binary archive.
double v
The value of the cost.
Definition: Cost.h:52
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added...
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...
virtual void load(const char *filename, PlannerData &pd)
Load the PlannerData structure from the given stream. The StateSpace that was used to store the data ...
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
virtual const State * getState(void) const
Retrieve the state associated with this vertex.
Definition: PlannerData.h:74
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
bool isGoalVertex(unsigned int index) const
Returns true if the given vertex index is marked as a goal vertex.
void serialize(Archive &ar, const unsigned int)
boost::serialization routine
virtual void loadVertices(PlannerData &pd, unsigned int numVertices, boost::archive::binary_iarchive &ia)
Read numVertices from the binary input ia and store them as PlannerData.
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...
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist...
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
Information stored at the beginning of the PlannerData archive.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
bool edgeExists(unsigned int v1, unsigned int v2) const
Check whether an edge between vertex index v1 and index v2 exists.
PlannerDataStorage(void)
Default constructor.
const SpaceInformationPtr & getSpaceInformation(void) const
Return the instance of SpaceInformation used in this PlannerData.
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.
virtual void storeVertices(const PlannerData &pd, boost::archive::binary_oarchive &oa)
Serialize and store all vertices in pd to the binary archive.
std::size_t edge_count
Number of edges stored in the archive.
std::size_t vertex_count
Number of vertices stored in the archive.
bool isStartVertex(unsigned int index) const
Returns true if the given vertex index is marked as a start vertex.
The object containing all vertex data that will be stored.
boost::uint32_t marker
OMPL PlannerData specific marker (fixed value)
Base class for a PlannerData edge.
Definition: PlannerData.h:117
std::vector< int > signature
Signature of state space that allocated the saved states in the vertices (see ompl::base::StateSpace:...
virtual ~PlannerDataStorage(void)
Destructor.
virtual void loadEdges(PlannerData &pd, unsigned int numEdges, boost::archive::binary_iarchive &ia)
Read numEdges from the binary input ia and store them as PlannerData.
virtual void decoupleFromPlanner(void)
Creates a deep copy of the states contained in the vertices of this PlannerData structure so that whe...
Definition: PlannerData.cpp:84
unsigned int numEdges(void) const
Retrieve the number of edges in this structure.
virtual void store(const PlannerData &pd, const char *filename)
Store (serialize) the PlannerData structure to the given filename.