37 #ifndef OMPL_CONTROL_PLANNER_DATA_STORAGE_
38 #define OMPL_CONTROL_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/PlannerDataStorage.h"
47 #include "ompl/control/PlannerData.h"
48 #include "ompl/control/SpaceInformation.h"
89 std::vector<int> control_signature;
92 template<
typename Archive>
93 void serialize(Archive & ar,
const unsigned int )
95 ar & boost::serialization::base_object<base::PlannerDataStorage::Header>(*this);
96 ar & control_signature;
103 template<
typename Archive>
104 void serialize(Archive & ar,
const unsigned int )
106 ar & boost::serialization::base_object<base::PlannerDataStorage::PlannerDataEdgeData>(*this);
110 std::vector<unsigned char> control_;
118 OMPL_DEBUG(
"Loading %d PlannerDataEdgeControl objects", numEdges);
121 std::vector<Control*> controls;
123 for (
unsigned int i = 0; i < numEdges; ++i)
125 PlannerDataEdgeControlData edgeData;
128 std::vector<unsigned char> ctrlBuf (space->getSerializationLength());
129 Control *ctrl = space->allocControl();
130 controls.push_back(ctrl);
131 space->deserialize(ctrl, &edgeData.control_[0]);
134 pd.
addEdge(edgeData.endpoints_.first, edgeData.endpoints_.second, *edgeData.e_,
base::Cost(edgeData.weight_));
148 for (
size_t i = 0; i < controls.size(); ++i)
149 space->freeControl(controls[i]);
159 std::vector<unsigned char> ctrl (space->getSerializationLength());
161 for (
unsigned int i = 0; i < pd.
numVertices(); ++i)
162 for (
unsigned int j = 0; j < pd.
numVertices(); ++j)
166 PlannerDataEdgeControlData edgeData;
167 edgeData.e_ = &pd.
getEdge(i, j);
168 edgeData.endpoints_.first = i;
169 edgeData.endpoints_.second = j;
172 edgeData.weight_ = weight.
v;
174 space->serialize(&ctrl[0], static_cast<const PlannerDataEdgeControl*>(edgeData.e_)->getControl());
175 edgeData.control_ = ctrl;
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.
virtual void storeEdges(const base::PlannerData &pd, boost::archive::binary_oarchive &oa)
Serialize and store all edges in pd to the binary archive. It is assumed that the edges can be cast t...
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.
Object that handles loading/storing a PlannerData object to/from a binary stream. Serialization of ve...
Definition of an abstract control.
PlannerDataStorage(void)
Default constructor.
double v
The value of the cost.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
virtual void store(const base::PlannerData &pd, const char *filename)
Store (serialize) the structure to the given filename. The StateSpace and ControlSpace that was used ...
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
virtual ~PlannerDataStorage(void)
Destructor.
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...
A boost shared pointer wrapper for ompl::control::ControlSpace.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
virtual void loadEdges(base::PlannerData &pd, unsigned int numEdges, boost::archive::binary_iarchive &ia)
Read numEdges from the binary input ia and store them as PlannerData. It is assumed that the edges ca...
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...
#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.
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 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 load(const char *filename, base::PlannerData &pd)
Load the PlannerData structure from the given filename.