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_STATE_STORAGE_
00038 #define OMPL_BASE_STATE_STORAGE_
00039
00040 #include "ompl/base/StateSpace.h"
00041 #include <boost/archive/binary_oarchive.hpp>
00042 #include <boost/archive/binary_iarchive.hpp>
00043 #include <boost/serialization/vector.hpp>
00044 #include <boost/function.hpp>
00045 #include <iostream>
00046
00047 namespace ompl
00048 {
00049 namespace base
00050 {
00051
00053
00054 ClassForward(StateStorage);
00056
00058 class StateStorage
00059 {
00060 public:
00061
00063 StateStorage(const StateSpacePtr &space);
00064 virtual ~StateStorage(void);
00065
00067 const StateSpacePtr& getStateSpace(void) const
00068 {
00069 return space_;
00070 }
00071
00073 void load(const char *filename);
00074
00076 virtual void load(std::istream &in);
00077
00079 void store(const char *filename);
00080
00082 virtual void store(std::ostream &out);
00083
00086 virtual void addState(const State *state);
00087
00089 virtual void generateSamples(unsigned int count);
00090
00092 virtual void clear(void);
00093
00095 std::size_t size(void) const
00096 {
00097 return states_.size();
00098 }
00099
00101 const std::vector<const State*>& getStates(void) const
00102 {
00103 return states_;
00104 }
00105
00107 State* getState(unsigned int index)
00108 {
00109 assert(states_.size() > index);
00110 return const_cast<State*>(states_[index]);
00111 }
00112
00114 const State* getState(unsigned int index) const
00115 {
00116 assert(states_.size() > index);
00117 return states_[index];
00118 }
00119
00122 void sort(const boost::function<bool(const State*, const State*)> &op);
00123
00126 StateSamplerAllocator getStateSamplerAllocator(void) const;
00127
00130 StateSamplerAllocator getStateSamplerAllocatorRangeUntil(std::size_t until) const;
00131
00134 StateSamplerAllocator getStateSamplerAllocatorRangeAfter(std::size_t after) const;
00135
00138 virtual StateSamplerAllocator getStateSamplerAllocatorRange(std::size_t from, std::size_t to) const;
00139
00141 virtual void print(std::ostream &out = std::cout) const;
00142
00143 protected:
00144
00146 struct Header
00147 {
00149 boost::uint32_t marker;
00150
00152 std::size_t state_count;
00153
00155 std::vector<int> signature;
00156
00158 template<typename Archive>
00159 void serialize(Archive & ar, const unsigned int version)
00160 {
00161 ar & marker;
00162 ar & state_count;
00163 ar & signature;
00164 }
00165 };
00166
00168 virtual void loadStates(const Header &h, boost::archive::binary_iarchive &ia);
00169
00174 virtual void loadMetadata(const Header &h, boost::archive::binary_iarchive &ia);
00175
00177 virtual void storeStates(const Header &h, boost::archive::binary_oarchive &oa);
00178
00183 virtual void storeMetadata(const Header &h, boost::archive::binary_oarchive &oa);
00184
00186 void freeMemory(void);
00187
00189 StateSpacePtr space_;
00190
00192 std::vector<const State*> states_;
00193
00195 msg::Interface msg_;
00196 };
00197
00200 template<typename M>
00201 class StateStorageWithMetadata : public StateStorage
00202 {
00203 public:
00204
00206 StateStorageWithMetadata(const StateSpacePtr &space) : StateStorage(space)
00207 {
00208 }
00209
00213 virtual void addState(const State *state)
00214 {
00215 addState(state, M());
00216 }
00217
00220 virtual void addState(const State *state, const M& metadata)
00221 {
00222 StateStorage::addState(state);
00223 metadata_.push_back(metadata);
00224 }
00225
00226 virtual void clear(void)
00227 {
00228 StateStorage::clear();
00229 metadata_.clear();
00230 }
00231
00233 const M& getMetadata(unsigned int index) const
00234 {
00235 assert(metadata_.size() > index);
00236 return metadata_[index];
00237 }
00238
00240 M& getMetadata(unsigned int index)
00241 {
00242 assert(metadata_.size() > index);
00243 return metadata_[index];
00244 }
00245
00246 protected:
00247
00248 virtual void loadMetadata(const Header &h, boost::archive::binary_iarchive &ia)
00249 {
00250 ia >> metadata_;
00251 }
00252
00253 virtual void storeMetadata(const Header &h, boost::archive::binary_oarchive &oa)
00254 {
00255 oa << metadata_;
00256 }
00257
00259 std::vector<M> metadata_;
00260
00261 };
00262
00263 }
00264 }
00265 #endif