00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef OMPL_BASE_PLANNER_DATA_STORAGE_
00038 #define OMPL_BASE_PLANNER_DATA_STORAGE_
00039
00040
00041 #include <boost/version.hpp>
00042 #if BOOST_VERSION < 104400
00043 #warning Boost version >= 1.44 is required for PlannerDataStorage classes
00044 #else
00045
00046 #include "ompl/base/PlannerData.h"
00047 #include "ompl/util/Console.h"
00048 #include <boost/archive/binary_oarchive.hpp>
00049 #include <boost/archive/binary_iarchive.hpp>
00050 #include <boost/serialization/vector.hpp>
00051 #include <boost/serialization/utility.hpp>
00052 #include <fstream>
00053
00054 namespace ompl
00055 {
00056 namespace base
00057 {
00085 class PlannerDataStorage
00086 {
00087 public:
00088
00090 PlannerDataStorage(void);
00092 virtual ~PlannerDataStorage(void);
00093
00095 virtual void store(const PlannerData& pd, const char *filename);
00096
00098 virtual void store(const PlannerData& pd, std::ostream &out);
00099
00103 virtual void load(const char *filename, PlannerData& pd);
00104
00108 virtual void load(std::istream &in, PlannerData& pd);
00109
00110 protected:
00112 struct Header
00113 {
00115 boost::uint32_t marker;
00116
00118 std::size_t vertex_count;
00119
00121 std::size_t edge_count;
00122
00124 std::vector<int> signature;
00125
00127 template<typename Archive>
00128 void serialize(Archive & ar, const unsigned int version)
00129 {
00130 ar & marker;
00131 ar & vertex_count;
00132 ar & edge_count;
00133 ar & signature;
00134 }
00135 };
00136
00138 struct PlannerDataVertexData
00139 {
00140 enum VertexType
00141 {
00142 STANDARD = 0,
00143 START,
00144 GOAL
00145 };
00146
00147 template<typename Archive>
00148 void serialize(Archive & ar, const unsigned int version)
00149 {
00150 ar & v_;
00151 ar & state_;
00152 ar & type_;
00153 }
00154
00155 const PlannerDataVertex* v_;
00156 std::vector<unsigned char> state_;
00157 VertexType type_;
00158 };
00159
00161 struct PlannerDataEdgeData
00162 {
00163 template<typename Archive>
00164 void serialize(Archive & ar, const unsigned int version)
00165 {
00166 ar & e_;
00167 ar & endpoints_;
00168 ar & weight_;
00169 }
00170
00171 const PlannerDataEdge* e_;
00172 std::pair<unsigned int, unsigned int> endpoints_;
00173 double weight_;
00174 };
00175
00177 virtual void loadVertices(PlannerData &pd, unsigned int numVertices, boost::archive::binary_iarchive &ia)
00178 {
00179 logDebug("Loading %d PlannerDataVertex objects", numVertices);
00180
00181 const StateSpacePtr &space = pd.getSpaceInformation()->getStateSpace();
00182 std::vector<State*> states;
00183 for (unsigned int i = 0; i < numVertices; ++i)
00184 {
00185 PlannerDataVertexData vertexData;
00186 ia >> vertexData;
00187
00188
00189 const PlannerDataVertex *v = vertexData.v_;
00190
00191
00192 State* state = space->allocState();
00193 states.push_back(state);
00194 space->deserialize (state, &vertexData.state_[0]);
00195 const_cast<PlannerDataVertex*>(v)->state_ = state;
00196
00197
00198 if (vertexData.type_ == PlannerDataVertexData::START)
00199 pd.addStartVertex(*v);
00200 else if (vertexData.type_ == PlannerDataVertexData::GOAL)
00201 pd.addGoalVertex(*v);
00202 else
00203 pd.addVertex(*v);
00204
00205
00206
00207 delete vertexData.v_;
00208 }
00209
00210
00211
00212
00213
00214
00215 pd.decoupleFromPlanner();
00216
00217 for (size_t i = 0; i < states.size(); ++i)
00218 space->freeState(states[i]);
00219 }
00220
00222 virtual void storeVertices(const PlannerData &pd, boost::archive::binary_oarchive &oa)
00223 {
00224 logDebug("Storing %d PlannerDataVertex objects", pd.numVertices());
00225
00226 const StateSpacePtr &space = pd.getSpaceInformation()->getStateSpace();
00227 std::vector<unsigned char> state (space->getSerializationLength());
00228 for (unsigned int i = 0; i < pd.numVertices(); ++i)
00229 {
00230 PlannerDataVertexData vertexData;
00231
00232
00233 const PlannerDataVertex &v = pd.getVertex(i);
00234 vertexData.v_ = &v;
00235
00236
00237 if (pd.isStartVertex(i))
00238 vertexData.type_ = PlannerDataVertexData::START;
00239 else if (pd.isGoalVertex(i))
00240 vertexData.type_ = PlannerDataVertexData::GOAL;
00241 else vertexData.type_ = PlannerDataVertexData::STANDARD;
00242
00243
00244 space->serialize (&state[0], v.getState());
00245 vertexData.state_ = state;
00246
00247 oa << vertexData;
00248 }
00249 }
00250
00252 virtual void loadEdges(PlannerData &pd, unsigned int numEdges, boost::archive::binary_iarchive &ia)
00253 {
00254 logDebug("Loading %d PlannerDataEdge objects", numEdges);
00255
00256 for (unsigned int i = 0; i < numEdges; ++i)
00257 {
00258 PlannerDataEdgeData edgeData;
00259 ia >> edgeData;
00260 pd.addEdge(edgeData.endpoints_.first, edgeData.endpoints_.second, *edgeData.e_, edgeData.weight_);
00261
00262
00263
00264 delete edgeData.e_;
00265 }
00266 }
00267
00269 virtual void storeEdges(const PlannerData &pd, boost::archive::binary_oarchive &oa)
00270 {
00271 logDebug("Storing %d PlannerDataEdge objects", pd.numEdges());
00272
00273 for (unsigned int i = 0; i < pd.numVertices(); ++i)
00274 for (unsigned int j = 0; j < pd.numVertices(); ++j)
00275 {
00276 if(pd.edgeExists(i, j))
00277 {
00278 PlannerDataEdgeData edgeData;
00279 edgeData.e_ = &pd.getEdge(i, j);
00280 edgeData.endpoints_.first = i;
00281 edgeData.endpoints_.second = j;
00282 edgeData.weight_ = pd.getEdgeWeight(i, j);
00283
00284 oa << edgeData;
00285 }
00286 }
00287 }
00288 };
00289 }
00290 }
00291
00292 #endif
00293
00294 #endif