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
00038 #include <boost/version.hpp>
00039 #if BOOST_VERSION < 104400
00040 #warning Boost version >= 1.44 is required for PlannerDataStorage classes
00041 #else
00042
00043 #include "ompl/base/PlannerDataStorage.h"
00044 #include <boost/archive/archive_exception.hpp>
00045
00046 static const boost::uint32_t OMPL_PLANNER_DATA_ARCHIVE_MARKER = 0x5044414D;
00047
00048 ompl::base::PlannerDataStorage::PlannerDataStorage(void)
00049 {
00050 }
00051
00052 ompl::base::PlannerDataStorage::~PlannerDataStorage(void)
00053 {
00054 }
00055
00056 void ompl::base::PlannerDataStorage::store(const PlannerData& pd, const char *filename)
00057 {
00058 std::ofstream out(filename, std::ios::binary);
00059 store(pd, out);
00060 out.close();
00061 }
00062
00063 void ompl::base::PlannerDataStorage::store(const PlannerData& pd, std::ostream &out)
00064 {
00065 const SpaceInformationPtr &si = pd.getSpaceInformation();
00066 if (!out.good())
00067 {
00068 logError("Failed to store PlannerData: output stream is invalid");
00069 return;
00070 }
00071 if (!si)
00072 {
00073 logError("Failed to store PlannerData: SpaceInformation is invalid");
00074 return;
00075 }
00076 try
00077 {
00078 boost::archive::binary_oarchive oa(out);
00079
00080
00081 Header h;
00082 h.marker = OMPL_PLANNER_DATA_ARCHIVE_MARKER;
00083 h.vertex_count = pd.numVertices();
00084 h.edge_count = pd.numEdges();
00085 si->getStateSpace()->computeSignature(h.signature);
00086 oa << h;
00087
00088 storeVertices(pd, oa);
00089 storeEdges(pd, oa);
00090 }
00091 catch (boost::archive::archive_exception &ae)
00092 {
00093 logError("Failed to store PlannerData: %s", ae.what());
00094 }
00095 }
00096
00097 void ompl::base::PlannerDataStorage::load(const char *filename, PlannerData& pd)
00098 {
00099 std::ifstream in(filename, std::ios::binary);
00100 load(in, pd);
00101 in.close();
00102 }
00103
00104 void ompl::base::PlannerDataStorage::load(std::istream &in, PlannerData& pd)
00105 {
00106 pd.clear();
00107
00108 const SpaceInformationPtr &si = pd.getSpaceInformation();
00109 if (!in.good())
00110 {
00111 logError("Failed to load PlannerData: input stream is invalid");
00112 return;
00113 }
00114 if (!si)
00115 {
00116 logError("Failed to load PlannerData: SpaceInformation is invalid");
00117 return;
00118 }
00119
00120 try
00121 {
00122 boost::archive::binary_iarchive ia(in);
00123
00124
00125 Header h;
00126 ia >> h;
00127
00128
00129 if (h.marker != OMPL_PLANNER_DATA_ARCHIVE_MARKER)
00130 {
00131 logError("Failed to load PlannerData: PlannerData archive marker not found");
00132 return;
00133 }
00134
00135
00136 std::vector<int> sig;
00137 si->getStateSpace()->computeSignature(sig);
00138 if (h.signature != sig)
00139 {
00140 logError("Failed to load PlannerData: StateSpace signature mismatch");
00141 return;
00142 }
00143
00144
00145 loadVertices(pd, h.vertex_count, ia);
00146 loadEdges(pd, h.edge_count, ia);
00147 }
00148 catch (boost::archive::archive_exception &ae)
00149 {
00150 logError("Failed to load PlannerData: %s", ae.what());
00151 }
00152 }
00153
00154 #endif