37 #include "ompl/base/StateStorage.h"
38 #include "ompl/base/PrecomputedStateSampler.h"
39 #include "ompl/util/Exception.h"
42 #include <boost/bind.hpp>
44 #include <boost/serialization/binary_object.hpp>
45 #include <boost/archive/archive_exception.hpp>
49 const std::vector<int> &expectedSignature,
50 const std::vector<const ompl::base::State*> *states,
51 std::size_t minIndex, std::size_t maxIndex)
55 if (sig != expectedSignature)
58 ss <<
"Cannot allocate state sampler for a state space whose signature does not match that of the stored states. ";
59 ss <<
"Expected signature ";
60 for (std::size_t i = 0 ; i < expectedSignature.size() ; ++i)
61 ss << expectedSignature[i] <<
" ";
62 ss <<
"but space " << space->
getName() <<
" has signature ";
63 for (std::size_t i = 0 ; i < sig.size() ; ++i)
70 static const boost::uint32_t OMPL_ARCHIVE_MARKER = 0x4C504D4F;
77 ompl::base::StateStorage::~StateStorage(
void)
84 std::ifstream in(filename, std::ios::binary);
91 std::ofstream out(filename, std::ios::binary);
99 if (!in.good() || in.eof())
107 boost::archive::binary_iarchive ia(in);
110 if (h.marker != OMPL_ARCHIVE_MARKER)
116 std::vector<int> sig;
117 space_->computeSignature(sig);
118 if (h.signature != sig)
120 OMPL_ERROR(
"State space signatures do not match");
127 catch (boost::archive::archive_exception &ae)
129 OMPL_ERROR(
"Unable to load archive: %s", ae.what());
138 unsigned int l = space_->getSerializationLength();
139 char *buffer =
new char[l];
140 State *s = space_->allocState();
143 ia >> boost::serialization::make_binary_object(buffer, l);
144 space_->deserialize(s, buffer);
147 space_->freeState(s);
166 h.
marker = OMPL_ARCHIVE_MARKER;
170 boost::archive::binary_oarchive oa(out);
174 storeMetadata(h, oa);
176 catch (boost::archive::archive_exception &ae)
178 OMPL_ERROR(
"Unable to save archive: %s", ae.what());
184 OMPL_DEBUG(
"Serializing %u states", (
unsigned int)states_.size());
186 unsigned int l = space_->getSerializationLength();
187 char *buffer =
new char[l];
188 for (std::size_t i = 0 ; i < states_.size() ; ++i)
190 space_->serialize(buffer, states_[i]);
191 oa << boost::serialization::make_binary_object(buffer, l);
202 State *copy = space_->allocState();
203 space_->copyState(copy, state);
204 states_.push_back(copy);
210 states_.reserve(states_.size() + count);
211 State *s = space_->allocState();
212 for (
unsigned int i = 0 ; i < count ; ++i)
214 ss->sampleUniform(s);
217 space_->freeState(s);
222 for (std::size_t i = 0 ; i < states_.size() ; ++i)
223 space_->freeState(const_cast<State*>(states_[i]));
234 std::sort(states_.begin(), states_.end(), op);
239 return getStateSamplerAllocatorRange(0, states_.empty() ? 0 : states_.size() - 1);
244 return getStateSamplerAllocatorRange(0, until);
249 return getStateSamplerAllocatorRange(after, states_.empty() ? 0 : states_.size() - 1);
255 throw Exception(
"Cannot allocate state sampler from empty state storage");
256 std::vector<int> sig;
257 space_->computeSignature(sig);
258 return boost::bind(&allocPrecomputedStateSampler, _1, sig, &states_, from, to);
263 for (std::size_t i = 0 ; i < states_.size() ; ++i)
264 space_->printState(states_[i], out);