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_CONTROL_PLANNER_DATA_STORAGE_
00038 #define OMPL_CONTROL_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/PlannerDataStorage.h"
00047 #include "ompl/control/PlannerData.h"
00048 #include "ompl/control/SpaceInformation.h"
00049
00050 namespace ompl
00051 {
00052 namespace control
00053 {
00058 class PlannerDataStorage : public base::PlannerDataStorage
00059 {
00060 public:
00062 PlannerDataStorage(void);
00064 virtual ~PlannerDataStorage(void);
00065
00067 virtual void load(const char *filename, base::PlannerData& pd);
00068
00070 virtual void load(std::istream &in, base::PlannerData& pd);
00071
00075 virtual void store(const base::PlannerData& pd, const char *filename);
00076
00080 virtual void store(const base::PlannerData& pd, std::ostream &out);
00081
00082 protected:
00083
00085
00086 struct Header : base::PlannerDataStorage::Header
00087 {
00089 std::vector<int> control_signature;
00090
00092 template<typename Archive>
00093 void serialize(Archive & ar, const unsigned int version)
00094 {
00095 ar & boost::serialization::base_object<base::PlannerDataStorage::Header>(*this);
00096 ar & control_signature;
00097 }
00098 };
00099
00100
00101 struct PlannerDataEdgeControlData : base::PlannerDataStorage::PlannerDataEdgeData
00102 {
00103 template<typename Archive>
00104 void serialize(Archive & ar, const unsigned int version)
00105 {
00106 ar & boost::serialization::base_object<base::PlannerDataStorage::PlannerDataEdgeData>(*this);
00107 ar & control_;
00108 }
00109
00110 std::vector<unsigned char> control_;
00111 };
00113
00116 virtual void loadEdges(base::PlannerData &pd, unsigned int numEdges, boost::archive::binary_iarchive &ia)
00117 {
00118 logDebug("Loading %d PlannerDataEdgeControl objects", numEdges);
00119
00120 const ControlSpacePtr& space = static_cast<control::PlannerData&>(pd).getSpaceInformation()->getControlSpace();
00121 std::vector<Control*> controls;
00122
00123 for (unsigned int i = 0; i < numEdges; ++i)
00124 {
00125 PlannerDataEdgeControlData edgeData;
00126 ia >> edgeData;
00127
00128 std::vector<unsigned char> ctrlBuf (space->getSerializationLength());
00129 Control *ctrl = space->allocControl();
00130 controls.push_back(ctrl);
00131 space->deserialize(ctrl, &edgeData.control_[0]);
00132 const_cast<PlannerDataEdgeControl*>(static_cast<const PlannerDataEdgeControl*>(edgeData.e_))->c_ = ctrl;
00133
00134 pd.addEdge(edgeData.endpoints_.first, edgeData.endpoints_.second, *edgeData.e_, edgeData.weight_);
00135
00136
00137
00138 delete edgeData.e_;
00139 }
00140
00141
00142
00143
00144
00145
00146 pd.decoupleFromPlanner();
00147
00148 for (size_t i = 0; i < controls.size(); ++i)
00149 space->freeControl(controls[i]);
00150 }
00151
00154 virtual void storeEdges(const base::PlannerData &pd, boost::archive::binary_oarchive &oa)
00155 {
00156 logDebug("Storing %d PlannerDataEdgeControl objects", pd.numEdges());
00157
00158 const ControlSpacePtr& space = static_cast<const control::PlannerData&>(pd).getSpaceInformation()->getControlSpace();
00159 std::vector<unsigned char> ctrl (space->getSerializationLength());
00160
00161 for (unsigned int i = 0; i < pd.numVertices(); ++i)
00162 for (unsigned int j = 0; j < pd.numVertices(); ++j)
00163 {
00164 if(pd.edgeExists(i, j))
00165 {
00166 PlannerDataEdgeControlData edgeData;
00167 edgeData.e_ = &pd.getEdge(i, j);
00168 edgeData.endpoints_.first = i;
00169 edgeData.endpoints_.second = j;
00170 edgeData.weight_ = pd.getEdgeWeight(i, j);
00171
00172 space->serialize(&ctrl[0], static_cast<const PlannerDataEdgeControl*>(edgeData.e_)->getControl());
00173 edgeData.control_ = ctrl;
00174
00175 oa << edgeData;
00176 }
00177 }
00178 }
00179
00180 };
00181 }
00182 }
00183
00184 #endif
00185
00186 #endif