37 #ifndef OMPL_BASE_PLANNER_DATA_STORAGE_
38 #define OMPL_BASE_PLANNER_DATA_STORAGE_
41 #include <boost/version.hpp>
42 #if BOOST_VERSION < 104400
43 #warning Boost version >= 1.44 is required for PlannerDataStorage classes
46 #include "ompl/base/PlannerData.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>
127 template<
typename Archive>
147 template<
typename Archive>
148 void serialize(Archive & ar,
const unsigned int )
156 std::vector<unsigned char> state_;
163 template<
typename Archive>
164 void serialize(Archive & ar,
const unsigned int )
172 std::pair<unsigned int, unsigned int> endpoints_;
179 OMPL_DEBUG(
"Loading %d PlannerDataVertex objects", numVertices);
182 std::vector<State*> states;
183 for (
unsigned int i = 0; i < numVertices; ++i)
192 State* state = space->allocState();
193 states.push_back(state);
194 space->deserialize (state, &vertexData.state_[0]);
198 if (vertexData.type_ == PlannerDataVertexData::START)
200 else if (vertexData.type_ == PlannerDataVertexData::GOAL)
207 delete vertexData.v_;
217 for (
size_t i = 0; i < states.size(); ++i)
218 space->freeState(states[i]);
227 std::vector<unsigned char> state (space->getSerializationLength());
228 for (
unsigned int i = 0; i < pd.
numVertices(); ++i)
238 vertexData.type_ = PlannerDataVertexData::START;
240 vertexData.type_ = PlannerDataVertexData::GOAL;
241 else vertexData.type_ = PlannerDataVertexData::STANDARD;
244 space->serialize (&state[0], v.
getState());
245 vertexData.state_ = state;
254 OMPL_DEBUG(
"Loading %d PlannerDataEdge objects", numEdges);
256 for (
unsigned int i = 0; i < numEdges; ++i)
260 pd.
addEdge(edgeData.endpoints_.first, edgeData.endpoints_.second, *edgeData.e_,
Cost(edgeData.weight_));
273 for (
unsigned int i = 0; i < pd.
numVertices(); ++i)
274 for (
unsigned int j = 0; j < pd.
numVertices(); ++j)
279 edgeData.e_ = &pd.
getEdge(i, j);
280 edgeData.endpoints_.first = i;
281 edgeData.endpoints_.second = j;
284 edgeData.weight_ = weight.
v;
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...
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.
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...
virtual const State * getState(void) const
Retrieve the state associated with this vertex.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
bool isGoalVertex(unsigned int index) const
Returns true if the given vertex index is marked as a goal vertex.
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.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
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.
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.
Base class for a PlannerData edge.
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...
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.