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  logDebug("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  logDebug("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  logDebug("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_, 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  logDebug("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  edgeData.weight_ = pd.getEdgeWeight(i, j);
283 
284  oa << edgeData;
285  }
286  }
287  }
288  };
289  }
290 }
291 
292 #endif
293 
294 #endif