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);
boost::function< StateSamplerPtr(const StateSpace *)> StateSamplerAllocator
Definition of a function that can allocate a state sampler.
virtual void generateSamples(unsigned int count)
Generate count states uniformly at random and store them in this structure.
StateSamplerAllocator getStateSamplerAllocatorRangeAfter(std::size_t after) const
Get a sampler allocator to a sampler that can be specified for a StateSpace, such that all sampled st...
void store(const char *filename)
Save a set of states to a file.
A boost shared pointer wrapper for ompl::base::StateSpace.
A boost shared pointer wrapper for ompl::base::StateSampler.
StateStorage(const StateSpacePtr &space)
The state space to store states for is specified as argument.
virtual void loadStates(const Header &h, boost::archive::binary_iarchive &ia)
Load the states from a binary archive ia, given the loaded header is h.
void computeSignature(std::vector< int > &signature) const
Compute an array of ints that uniquely identifies the structure of the state space. The first element of the signature is the number of integers that follow.
StateSamplerAllocator getStateSamplerAllocator(void) const
Get a sampler allocator to a sampler that can be specified for a StateSpace, such that all sampled st...
StateSamplerAllocator getStateSamplerAllocatorRangeUntil(std::size_t until) const
Get a sampler allocator to a sampler that can be specified for a StateSpace, such that all sampled st...
State space sampler for discrete states.
virtual void loadMetadata(const Header &h, boost::archive::binary_iarchive &ia)
Load the state metadata from a binary archive ia, given the loaded header is h. No metadata is actual...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
const std::string & getName(void) const
Get the name of the state space.
void freeMemory(void)
Free the memory allocated for states.
virtual void addState(const State *state)
Add a state to the set of states maintained by this storage structure. The state is copied to interna...
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition of an abstract state.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
virtual void storeStates(const Header &h, boost::archive::binary_oarchive &oa)
Store the states to a binary archive oa, given the stored header is h.
The exception type for ompl.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
void load(const char *filename)
Load a set of states from a specified file.
virtual void storeMetadata(const Header &h, boost::archive::binary_oarchive &oa)
Save the state metadata to a binary archive oa, given the stored header is h. No metadata is actually...
void sort(const boost::function< bool(const State *, const State *)> &op)
Sort the states according to the less-equal operator op. Metadata is NOT sorted; if metadata was adde...
virtual StateSamplerAllocator getStateSamplerAllocatorRange(std::size_t from, std::size_t to) const
Get a sampler allocator to a sampler that can be specified for a StateSpace, such that all sampled st...
virtual void print(std::ostream &out=std::cout) const
Output the set of states to a specified stream, in a human readable fashion.
virtual void clear(void)
Clear the stored states. This frees all the memory.