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
48namespace ompl
49{
50 namespace base
51 {
80 {
81 public:
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
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.
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
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
double value() const
The value of the cost.
Definition: Cost.h:56
Base class for a PlannerData edge.
Definition: PlannerData.h:127
Object that handles loading/storing a PlannerData object to/from a binary stream. Serialization of ve...
PlannerDataStorage()
Default constructor.
virtual void storeEdges(const PlannerData &pd, boost::archive::binary_oarchive &oa)
Serialize and store all edges in pd to the binary archive.
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 void storeVertices(const PlannerData &pd, boost::archive::binary_oarchive &oa)
Serialize and store all vertices in pd to the binary archive.
virtual ~PlannerDataStorage()
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 store(const PlannerData &pd, const char *filename)
Store (serialize) the PlannerData structure to the given filename.
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:59
virtual const State * getState() const
Retrieve the state associated with this vertex.
Definition: PlannerData.h:80
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
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...
bool isGoalVertex(unsigned int index) const
Returns true if the given vertex index is marked as a goal vertex.
const SpaceInformationPtr & getSpaceInformation() const
Return the instance of SpaceInformation used in this PlannerData.
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...
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...
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
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)....
const PlannerDataEdge & getEdge(unsigned int v1, unsigned int v2) const
Retrieve a reference to the edge object connecting vertices with indexes v1 and v2....
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 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 addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
bool isStartVertex(unsigned int index) const
Returns true if the given vertex index is marked as a start vertex.
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.
Definition of an abstract state.
Definition: State.h:50
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
Main namespace. Contains everything in this library.
Information stored at the beginning of the PlannerData archive.
std::uint_fast32_t marker
OMPL PlannerData specific marker (fixed value)
std::size_t edge_count
Number of edges stored in the archive.
std::vector< int > signature
Signature of state space that allocated the saved states in the vertices (see ompl::base::StateSpace:...
std::size_t vertex_count
Number of vertices stored in the archive.
void serialize(Archive &ar, const unsigned int)
boost::serialization routine
The object containing all edge data that will be stored.
The object containing all vertex data that will be stored.