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 #include "ompl/base/StateStorage.h"
00038 #include "ompl/base/PrecomputedStateSampler.h"
00039 #include "ompl/util/Exception.h"
00040 #include <fstream>
00041 #include <algorithm>
00042 #include <boost/bind.hpp>
00043
00044 #include <boost/serialization/binary_object.hpp>
00045 #include <boost/archive/archive_exception.hpp>
00046
00048 static ompl::base::StateSamplerPtr allocPrecomputedStateSampler(const ompl::base::StateSpace *space,
00049 const std::vector<int> &expectedSignature,
00050 const std::vector<const ompl::base::State*> *states,
00051 std::size_t minIndex, std::size_t maxIndex)
00052 {
00053 std::vector<int> sig;
00054 space->computeSignature(sig);
00055 if (sig != expectedSignature)
00056 {
00057 std::stringstream ss;
00058 ss << "Cannot allocate state sampler for a state space whose signature does not match that of the stored states. ";
00059 ss << "Expected signature ";
00060 for (std::size_t i = 0 ; i < expectedSignature.size() ; ++i)
00061 ss << expectedSignature[i] << " ";
00062 ss << "but space " << space->getName() << " has signature ";
00063 for (std::size_t i = 0 ; i < sig.size() ; ++i)
00064 ss << sig[i] << " ";
00065 throw ompl::Exception(ss.str());
00066 }
00067 return ompl::base::StateSamplerPtr(new ompl::base::PrecomputedStateSampler(space, *states, minIndex, maxIndex));
00068 }
00069
00070 static const boost::uint32_t OMPL_ARCHIVE_MARKER = 0x4C504D4F;
00072
00073 ompl::base::StateStorage::StateStorage(const StateSpacePtr &space) : space_(space)
00074 {
00075 }
00076
00077 ompl::base::StateStorage::~StateStorage(void)
00078 {
00079 freeMemory();
00080 }
00081
00082 void ompl::base::StateStorage::load(const char *filename)
00083 {
00084 std::ifstream in(filename, std::ios::binary);
00085 load(in);
00086 in.close();
00087 }
00088
00089 void ompl::base::StateStorage::store(const char *filename)
00090 {
00091 std::ofstream out(filename, std::ios::binary);
00092 store(out);
00093 out.close();
00094 }
00095
00096 void ompl::base::StateStorage::load(std::istream &in)
00097 {
00098 clear();
00099 if (!in.good() || in.eof())
00100 {
00101 logWarn("Unable to load states");
00102 return;
00103 }
00104 try
00105 {
00106
00107 boost::archive::binary_iarchive ia(in);
00108 Header h;
00109 ia >> h;
00110 if (h.marker != OMPL_ARCHIVE_MARKER)
00111 {
00112 logError("OMPL archive marker not found");
00113 return;
00114 }
00115
00116 std::vector<int> sig;
00117 space_->computeSignature(sig);
00118 if (h.signature != sig)
00119 {
00120 logError("State space signatures do not match");
00121 return;
00122 }
00123 loadStates(h, ia);
00124 loadMetadata(h, ia);
00125 }
00126
00127 catch (boost::archive::archive_exception &ae)
00128 {
00129 logError("Unable to load archive: %s", ae.what());
00130 }
00131
00132 }
00133
00134 void ompl::base::StateStorage::loadStates(const Header &h, boost::archive::binary_iarchive &ia)
00135 {
00136 logDebug("Deserializing %u states", h.state_count);
00137
00138 unsigned int l = space_->getSerializationLength();
00139 char *buffer = new char[l];
00140 State *s = space_->allocState();
00141 for (std::size_t i = 0 ; i < h.state_count ; ++i)
00142 {
00143 ia >> boost::serialization::make_binary_object(buffer, l);
00144 space_->deserialize(s, buffer);
00145 addState(s);
00146 }
00147 space_->freeState(s);
00148 delete[] buffer;
00149 }
00150
00151 void ompl::base::StateStorage::loadMetadata(const Header &h, boost::archive::binary_iarchive &ia)
00152 {
00153 }
00154
00155
00156 void ompl::base::StateStorage::store(std::ostream &out)
00157 {
00158 if (!out.good())
00159 {
00160 logWarn("Unable to store states");
00161 return;
00162 }
00163 try
00164 {
00165 Header h;
00166 h.marker = OMPL_ARCHIVE_MARKER;
00167 h.state_count = states_.size();
00168 space_->computeSignature(h.signature);
00169
00170 boost::archive::binary_oarchive oa(out);
00171 oa << h;
00172
00173 storeStates(h, oa);
00174 storeMetadata(h, oa);
00175 }
00176 catch (boost::archive::archive_exception &ae)
00177 {
00178 logError("Unable to save archive: %s", ae.what());
00179 }
00180 }
00181
00182 void ompl::base::StateStorage::storeStates(const Header &h, boost::archive::binary_oarchive &oa)
00183 {
00184 logDebug("Serializing %u states", (unsigned int)states_.size());
00185
00186 unsigned int l = space_->getSerializationLength();
00187 char *buffer = new char[l];
00188 for (std::size_t i = 0 ; i < states_.size() ; ++i)
00189 {
00190 space_->serialize(buffer, states_[i]);
00191 oa << boost::serialization::make_binary_object(buffer, l);
00192 }
00193 delete[] buffer;
00194 }
00195
00196 void ompl::base::StateStorage::storeMetadata(const Header &h, boost::archive::binary_oarchive &oa)
00197 {
00198 }
00199
00200 void ompl::base::StateStorage::addState(const State *state)
00201 {
00202 State *copy = space_->allocState();
00203 space_->copyState(copy, state);
00204 states_.push_back(copy);
00205 }
00206
00207 void ompl::base::StateStorage::generateSamples(unsigned int count)
00208 {
00209 StateSamplerPtr ss = space_->allocStateSampler();
00210 states_.reserve(states_.size() + count);
00211 State *s = space_->allocState();
00212 for (unsigned int i = 0 ; i < count ; ++i)
00213 {
00214 ss->sampleUniform(s);
00215 addState(s);
00216 }
00217 space_->freeState(s);
00218 }
00219
00220 void ompl::base::StateStorage::freeMemory(void)
00221 {
00222 for (std::size_t i = 0 ; i < states_.size() ; ++i)
00223 space_->freeState(const_cast<State*>(states_[i]));
00224 }
00225
00226 void ompl::base::StateStorage::clear(void)
00227 {
00228 freeMemory();
00229 states_.clear();
00230 }
00231
00232 void ompl::base::StateStorage::sort(const boost::function<bool(const State*, const State*)> &op)
00233 {
00234 std::sort(states_.begin(), states_.end(), op);
00235 }
00236
00237 ompl::base::StateSamplerAllocator ompl::base::StateStorage::getStateSamplerAllocator(void) const
00238 {
00239 return getStateSamplerAllocatorRange(0, states_.empty() ? 0 : states_.size() - 1);
00240 }
00241
00242 ompl::base::StateSamplerAllocator ompl::base::StateStorage::getStateSamplerAllocatorRangeUntil(std::size_t until) const
00243 {
00244 return getStateSamplerAllocatorRange(0, until);
00245 }
00246
00247 ompl::base::StateSamplerAllocator ompl::base::StateStorage::getStateSamplerAllocatorRangeAfter(std::size_t after) const
00248 {
00249 return getStateSamplerAllocatorRange(after, states_.empty() ? 0 : states_.size() - 1);
00250 }
00251
00252 ompl::base::StateSamplerAllocator ompl::base::StateStorage::getStateSamplerAllocatorRange(std::size_t from, std::size_t to) const
00253 {
00254 if (states_.empty())
00255 throw Exception("Cannot allocate state sampler from empty state storage");
00256 std::vector<int> sig;
00257 space_->computeSignature(sig);
00258 return boost::bind(&allocPrecomputedStateSampler, _1, sig, &states_, from, to);
00259 }
00260
00261 void ompl::base::StateStorage::print(std::ostream &out) const
00262 {
00263 for (std::size_t i = 0 ; i < states_.size() ; ++i)
00264 space_->printState(states_[i], out);
00265 }