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/control/PlannerDataStorage.h"
00044 #include <boost/archive/archive_exception.hpp>
00045
00047 static const boost::uint32_t OMPL_PLANNER_DATA_CONTROL_ARCHIVE_MARKER = 0x5044434D;
00049
00050 ompl::control::PlannerDataStorage::PlannerDataStorage(void) : base::PlannerDataStorage()
00051 {
00052 }
00053
00054 ompl::control::PlannerDataStorage::~PlannerDataStorage(void)
00055 {
00056 }
00057
00058 void ompl::control::PlannerDataStorage::load(const char *filename, base::PlannerData& pd)
00059 {
00060 base::PlannerDataStorage::load(filename, pd);
00061 }
00062
00063 void ompl::control::PlannerDataStorage::load(std::istream &in, base::PlannerData& pd)
00064 {
00065 if (!pd.hasControls())
00066 {
00067 logWarn("PlannerData does not have controls. Invoking base::PlannerDataStorage::load");
00068 base::PlannerDataStorage::load(in, pd);
00069 return;
00070 }
00071
00072 control::PlannerData* pdc = static_cast<control::PlannerData*>(&pd);
00073 pdc->clear();
00074
00075 const SpaceInformationPtr &si = pdc->getSpaceInformation();
00076 if (!in.good())
00077 {
00078 logError("Failed to load PlannerData: input stream is invalid");
00079 return;
00080 }
00081 if (!si)
00082 {
00083 logError("Failed to load PlannerData: SpaceInformation is invalid");
00084 return;
00085 }
00086
00087 try
00088 {
00089 boost::archive::binary_iarchive ia(in);
00090
00091
00092 Header h;
00093 ia >> h;
00094
00095
00096 if (h.marker != OMPL_PLANNER_DATA_CONTROL_ARCHIVE_MARKER)
00097 {
00098 logError("Failed to load PlannerData: PlannerData control archive marker not found");
00099 return;
00100 }
00101
00102
00103 std::vector<int> sig;
00104 si->getStateSpace()->computeSignature(sig);
00105 if (h.signature != sig)
00106 {
00107 logError("Failed to load PlannerData: StateSpace signature mismatch");
00108 return;
00109 }
00110
00111
00112 sig.clear();
00113 si->getControlSpace()->computeSignature(sig);
00114 if (h.control_signature != sig)
00115 {
00116 logError("Failed to load PlannerData: ControlSpace signature mismatch");
00117 return;
00118 }
00119
00120
00121 loadVertices(pd, h.vertex_count, ia);
00122 loadEdges(pd, h.edge_count, ia);
00123 }
00124 catch (boost::archive::archive_exception &ae)
00125 {
00126 logError("Failed to load PlannerData: %s", ae.what());
00127 }
00128 }
00129
00130 void ompl::control::PlannerDataStorage::store(const base::PlannerData& pd, const char *filename)
00131 {
00132 base::PlannerDataStorage::store(pd, filename);
00133 }
00134
00135 void ompl::control::PlannerDataStorage::store(const base::PlannerData& pd, std::ostream &out)
00136 {
00137 const control::PlannerData* pdc = static_cast<const control::PlannerData*>(&pd);
00138 if (!pdc)
00139 {
00140 logWarn("Failed to cast PlannerData to control::PlannerData. Invoking base::PlannerDataStorage::store");
00141 base::PlannerDataStorage::store(pd, out);
00142 return;
00143 }
00144
00145 const SpaceInformationPtr &si = pdc->getSpaceInformation();
00146 if (!out.good())
00147 {
00148 logError("Failed to store PlannerData: output stream is invalid");
00149 return;
00150 }
00151 if (!si)
00152 {
00153 logError("Failed to store PlannerData: SpaceInformation is invalid");
00154 return;
00155 }
00156 try
00157 {
00158 boost::archive::binary_oarchive oa(out);
00159
00160
00161 Header h;
00162 h.marker = OMPL_PLANNER_DATA_CONTROL_ARCHIVE_MARKER;
00163 h.vertex_count = pdc->numVertices();
00164 h.edge_count = pdc->numEdges();
00165 si->getStateSpace()->computeSignature(h.signature);
00166 si->getControlSpace()->computeSignature(h.control_signature);
00167 oa << h;
00168
00169 storeVertices(pd, oa);
00170 storeEdges(pd, oa);
00171 }
00172 catch (boost::archive::archive_exception &ae)
00173 {
00174 logError("Failed to store PlannerData: %s", ae.what());
00175 }
00176 }
00177
00178 #endif