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 #include "ompl/base/PlannerData.h"
41 #include "ompl/util/Console.h"
42 #include <boost/archive/binary_oarchive.hpp>
43 #include <boost/archive/binary_iarchive.hpp>
44 #include <boost/serialization/vector.hpp>
45 #include <boost/serialization/utility.hpp>
46 #include <fstream>
47 
48 namespace ompl
49 {
50  namespace base
51  {
79  class PlannerDataStorage
80  {
81  public:
85  virtual ~PlannerDataStorage();
86 
88  virtual void store(const PlannerData &pd, const char *filename);
89 
91  virtual void store(const PlannerData &pd, std::ostream &out);
92 
96  virtual void load(const char *filename, PlannerData &pd);
97 
101  virtual void load(std::istream &in, PlannerData &pd);
102 
103  protected:
105  struct Header
106  {
108  std::uint_fast32_t marker;
109 
111  std::size_t vertex_count;
112 
114  std::size_t edge_count;
115 
118  std::vector<int> signature;
119 
121  template <typename Archive>
122  void serialize(Archive &ar, const unsigned int /*version*/)
123  {
124  ar &marker;
125  ar &vertex_count;
126  ar &edge_count;
127  ar &signature;
128  }
129  };
130 
132  struct PlannerDataVertexData
133  {
134  enum VertexType
135  {
136  STANDARD = 0,
137  START,
138  GOAL
139  };
140 
141  template <typename Archive>
142  void serialize(Archive &ar, const unsigned int /*version*/)
143  {
144  ar &v_;
145  ar &state_;
146  ar &type_;
147  }
148 
149  const PlannerDataVertex *v_;
150  std::vector<unsigned char> state_;
151  VertexType type_;
152  };
153 
155  struct PlannerDataEdgeData
156  {
157  template <typename Archive>
158  void serialize(Archive &ar, const unsigned int /*version*/)
159  {
160  ar &e_;
161  ar &endpoints_;
162  ar &weight_;
163  }
164 
165  const PlannerDataEdge *e_;
166  std::pair<unsigned int, unsigned int> endpoints_;
167  double weight_;
168  };
169 
171  virtual void loadVertices(PlannerData &pd, unsigned int numVertices, boost::archive::binary_iarchive &ia)
172  {
173  const StateSpacePtr &space = pd.getSpaceInformation()->getStateSpace();
174  std::vector<State *> states;
175  for (unsigned int i = 0; i < numVertices; ++i)
176  {
177  PlannerDataVertexData vertexData;
178  ia >> vertexData;
179 
180  // Deserializing all data in the vertex (except the state)
181  const PlannerDataVertex *v = vertexData.v_;
182 
183  // Allocating a new state and deserializing it from the buffer
184  State *state = space->allocState();
185  states.push_back(state);
186  space->deserialize(state, &vertexData.state_[0]);
187  const_cast<PlannerDataVertex *>(v)->state_ = state;
188 
189  // Record the type of the vertex (i.e. start vertex).
190  if (vertexData.type_ == PlannerDataVertexData::START)
191  pd.addStartVertex(*v);
192  else if (vertexData.type_ == PlannerDataVertexData::GOAL)
193  pd.addGoalVertex(*v);
194  else
195  pd.addVertex(*v);
196 
197  // We deserialized the vertex object pointer, and we own it.
198  // Since addEdge copies the object, it is safe to free here.
199  delete vertexData.v_;
200  }
201 
202  // These vertices are using state pointers allocated here.
203  // To avoid a memory leak, we decouple planner data from the
204  // 'planner', which will clone all states and properly free the
205  // memory when PlannerData goes out of scope. Then it is safe
206  // to free all memory allocated here.
207  pd.decoupleFromPlanner();
208 
209  for (auto &state : states)
210  space->freeState(state);
211  }
212 
214  virtual void storeVertices(const PlannerData &pd, boost::archive::binary_oarchive &oa)
215  {
216  const StateSpacePtr &space = pd.getSpaceInformation()->getStateSpace();
217  std::vector<unsigned char> state(space->getSerializationLength());
218  for (unsigned int i = 0; i < pd.numVertices(); ++i)
219  {
220  PlannerDataVertexData vertexData;
221 
222  // Serializing all data in the vertex (except the state)
223  const PlannerDataVertex &v = pd.getVertex(i);
224  vertexData.v_ = &v;
225 
226  // Record the type of the vertex (i.e. start vertex).
227  if (pd.isStartVertex(i))
228  vertexData.type_ = PlannerDataVertexData::START;
229  else if (pd.isGoalVertex(i))
230  vertexData.type_ = PlannerDataVertexData::GOAL;
231  else
232  vertexData.type_ = PlannerDataVertexData::STANDARD;
233 
234  // Serializing the state contained in this vertex
235  space->serialize(&state[0], v.getState());
236  vertexData.state_ = state;
237 
238  oa << vertexData;
239  }
240  }
241 
243  virtual void loadEdges(PlannerData &pd, unsigned int numEdges, boost::archive::binary_iarchive &ia)
244  {
245  for (unsigned int i = 0; i < numEdges; ++i)
246  {
247  PlannerDataEdgeData edgeData;
248  ia >> edgeData;
249  pd.addEdge(edgeData.endpoints_.first, edgeData.endpoints_.second, *edgeData.e_,
250  Cost(edgeData.weight_));
251 
252  // We deserialized the edge object pointer, and we own it.
253  // Since addEdge copies the object, it is safe to free here.
254  delete edgeData.e_;
255  }
256  }
257 
259  virtual void storeEdges(const PlannerData &pd, boost::archive::binary_oarchive &oa)
260  {
261  std::vector<unsigned int> edgeList;
262  for (unsigned int fromVertex = 0; fromVertex < pd.numVertices(); ++fromVertex)
263  {
264  edgeList.clear();
265  pd.getEdges(fromVertex, edgeList); // returns the id of each edge
266 
267  // Process edges
268  for (unsigned int toVertex : edgeList)
269  {
270  // Get cost
271  Cost weight;
272  if (!pd.getEdgeWeight(fromVertex, toVertex, &weight))
273  OMPL_ERROR("Unable to get edge weight");
274 
275  // Convert to new structure
276  PlannerDataEdgeData edgeData;
277  edgeData.e_ = &pd.getEdge(fromVertex, toVertex);
278  edgeData.endpoints_.first = fromVertex;
279  edgeData.endpoints_.second = toVertex;
280  edgeData.weight_ = weight.value();
281  oa << edgeData;
282 
283  } // for each edge
284  } // for each vertex
285  }
286  };
287  }
288 }
289 
290 #endif
std::size_t vertex_count
Number of vertices stored in the archive.
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 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)....
The object containing all vertex data that will be stored.
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.
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
double value() const
The value of the cost.
Definition: Cost.h:152
virtual ~PlannerDataStorage()
Destructor.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
std::uint_fast32_t marker
OMPL PlannerData specific marker (fixed value)
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
virtual void store(const PlannerData &pd, const char *filename)
Store (serialize) the PlannerData structure to the given filename.
std::vector< int > signature
Signature of state space that allocated the saved states in the vertices (see ompl::base::StateSpace:...
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
PlannerDataStorage()
Default constructor.
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 ...
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist,...
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 const State * getState() const
Retrieve the state associated with this vertex.
Definition: PlannerData.h:176
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 storeVertices(const PlannerData &pd, boost::archive::binary_oarchive &oa)
Serialize and store all vertices in pd to the binary archive.
virtual void storeEdges(const PlannerData &pd, boost::archive::binary_oarchive &oa)
Serialize and store all edges in pd to the binary archive.
A shared pointer wrapper for ompl::base::StateSpace.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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...
std::size_t edge_count
Number of edges stored in the archive.
The object containing all edge data that will be stored.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
const PlannerDataEdge & getEdge(unsigned int v1, unsigned int v2) const
Retrieve a reference to the edge object connecting vertices with indexes v1 and v2....