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  {
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  boost::uint32_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 
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 
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
Object that handles loading/storing a PlannerData object to/from a binary stream. Serialization of ve...
The object containing all edge data that will be stored.
virtual ~PlannerDataStorage()
Destructor.
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
A 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.
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 numVertices() const
Retrieve the number of vertices in this structure.
const SpaceInformationPtr & getSpaceInformation() const
Return the instance of SpaceInformation used in this PlannerData.
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added...
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.
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 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:58
PlannerDataStorage()
Default constructor.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
bool isStartVertex(unsigned int index) const
Returns true if the given vertex index is marked as a start vertex.
void serialize(Archive &ar, const unsigned int)
boost::serialization routine
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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...
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:49
Information stored at the beginning of the PlannerData archive.
virtual const State * getState() const
Retrieve the state associated with this vertex.
Definition: PlannerData.h:80
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.
double value() const
The value of the cost.
Definition: Cost.h:56
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.
boost::uint32_t marker
OMPL PlannerData specific marker (fixed value)
Base class for a PlannerData edge.
Definition: PlannerData.h:126
*std::vector< int > signature
Signature of state space that allocated the saved states in the vertices (see ompl::base::StateSpace:...
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.
bool isGoalVertex(unsigned int index) const
Returns true if the given vertex index is marked as a goal vertex.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
virtual void store(const PlannerData &pd, const char *filename)
Store (serialize) the PlannerData structure to the given filename.