Loading...
Searching...
No Matches
StateStorage.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2012, Willow Garage
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Willow Garage nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Author: Ioan Sucan */
36
37#include "ompl/base/StateStorage.h"
38#include "ompl/base/PrecomputedStateSampler.h"
39#include "ompl/util/Exception.h"
40#include <fstream>
41#include <algorithm>
42
43#include <boost/serialization/binary_object.hpp>
44#include <boost/archive/archive_exception.hpp>
45#include <utility>
46
48static ompl::base::StateSamplerPtr allocPrecomputedStateSampler(const ompl::base::StateSpace *space,
49 const std::vector<int> &expectedSignature,
50 const std::vector<const ompl::base::State *> *states,
51 std::size_t minIndex, std::size_t maxIndex)
52{
53 std::vector<int> sig;
54 space->computeSignature(sig);
55 if (sig != expectedSignature)
56 {
57 std::stringstream ss;
58 ss << "Cannot allocate state sampler for a state space whose signature does not match that of the stored "
59 "states. ";
60 ss << "Expected signature ";
61 for (int i : expectedSignature)
62 ss << i << " ";
63 ss << "but space " << space->getName() << " has signature ";
64 for (int i : sig)
65 ss << i << " ";
66 throw ompl::Exception(ss.str());
67 }
68 return std::make_shared<ompl::base::PrecomputedStateSampler>(space, *states, minIndex, maxIndex);
69}
70
71static const std::uint_fast32_t OMPL_ARCHIVE_MARKER = 0x4C504D4F; // this spells OMPL
73
74ompl::base::StateStorage::StateStorage(StateSpacePtr space) : space_(std::move(space)), hasMetadata_(false)
75{
76}
77
78ompl::base::StateStorage::~StateStorage()
79{
80 freeMemory();
81}
82
83void ompl::base::StateStorage::load(const char *filename)
84{
85 std::ifstream in(filename, std::ios::binary);
86 load(in);
87 in.close();
88}
89
90void ompl::base::StateStorage::store(const char *filename)
91{
92 std::ofstream out(filename, std::ios::binary);
93 store(out);
94 out.close();
95}
96
97void ompl::base::StateStorage::load(std::istream &in)
98{
99 clear();
100 if (!in.good() || in.eof())
101 {
102 OMPL_WARN("Unable to load states");
103 return;
104 }
105 try
106 {
107 boost::archive::binary_iarchive ia(in);
108 Header h;
109 ia >> h;
110 if (h.marker != OMPL_ARCHIVE_MARKER)
111 {
112 OMPL_ERROR("OMPL archive marker not found");
113 return;
114 }
115
116 std::vector<int> sig;
117 space_->computeSignature(sig);
118 if (h.signature != sig)
119 {
120 OMPL_ERROR("State space signatures do not match");
121 return;
122 }
123 loadStates(h, ia);
124 loadMetadata(h, ia);
125 }
126
127 catch (boost::archive::archive_exception &ae)
128 {
129 OMPL_ERROR("Unable to load archive: %s", ae.what());
130 }
131}
132
133void ompl::base::StateStorage::loadStates(const Header &h, boost::archive::binary_iarchive &ia)
134{
135 OMPL_DEBUG("Deserializing %u states", h.state_count);
136 // load the file
137 unsigned int l = space_->getSerializationLength();
138 auto *buffer = new char[l];
139 State *s = space_->allocState();
140 for (std::size_t i = 0; i < h.state_count; ++i)
141 {
142 ia >> boost::serialization::make_binary_object(buffer, l);
143 space_->deserialize(s, buffer);
144 addState(s);
145 }
146 space_->freeState(s);
147 delete[] buffer;
148}
149
150void ompl::base::StateStorage::loadMetadata(const Header & /*h*/, boost::archive::binary_iarchive & /*ia*/)
151{
152}
153
154void ompl::base::StateStorage::store(std::ostream &out)
155{
156 if (!out.good())
157 {
158 OMPL_WARN("Unable to store states");
159 return;
160 }
161 try
162 {
163 Header h;
164 h.marker = OMPL_ARCHIVE_MARKER;
165 h.state_count = states_.size();
166 space_->computeSignature(h.signature);
167
168 boost::archive::binary_oarchive oa(out);
169 oa << h;
170
171 storeStates(h, oa);
172 storeMetadata(h, oa);
173 }
174 catch (boost::archive::archive_exception &ae)
175 {
176 OMPL_ERROR("Unable to save archive: %s", ae.what());
177 }
178}
179
180void ompl::base::StateStorage::storeStates(const Header & /*h*/, boost::archive::binary_oarchive &oa)
181{
182 OMPL_DEBUG("Serializing %u states", (unsigned int)states_.size());
183
184 unsigned int l = space_->getSerializationLength();
185 auto *buffer = new char[l];
186 for (auto &state : states_)
187 {
188 space_->serialize(buffer, state);
189 oa << boost::serialization::make_binary_object(buffer, l);
190 }
191 delete[] buffer;
192}
193
194void ompl::base::StateStorage::storeMetadata(const Header & /*h*/, boost::archive::binary_oarchive & /*oa*/)
195{
196}
197
199{
200 State *copy = space_->allocState();
201 space_->copyState(copy, state);
202 states_.push_back(copy);
203}
204
206{
207 StateSamplerPtr ss = space_->allocStateSampler();
208 states_.reserve(states_.size() + count);
209 State *s = space_->allocState();
210 for (unsigned int i = 0; i < count; ++i)
211 {
212 ss->sampleUniform(s);
213 addState(s);
214 }
215 space_->freeState(s);
216}
217
219{
220 for (auto &state : states_)
221 space_->freeState(const_cast<State *>(state));
222}
223
225{
226 freeMemory();
227 states_.clear();
228}
229
230void ompl::base::StateStorage::sort(const std::function<bool(const State *, const State *)> &op)
231{
232 std::sort(states_.begin(), states_.end(), op);
233}
234
236{
237 return getStateSamplerAllocatorRange(0, states_.empty() ? 0 : states_.size() - 1);
238}
239
241{
242 return getStateSamplerAllocatorRange(0, until);
243}
244
246{
247 return getStateSamplerAllocatorRange(after, states_.empty() ? 0 : states_.size() - 1);
248}
249
251 std::size_t to) const
252{
253 if (states_.empty())
254 throw Exception("Cannot allocate state sampler from empty state storage");
255 std::vector<int> sig;
256 space_->computeSignature(sig);
257 return [this, sig, from, to](const ompl::base::StateSpace *space)
258 {
259 return allocPrecomputedStateSampler(space, sig, &states_, from, to);
260 };
261}
262
263void ompl::base::StateStorage::print(std::ostream &out) const
264{
265 for (auto state : states_)
266 space_->printState(state, out);
267}
The exception type for ompl.
Definition: Exception.h:47
A shared pointer wrapper for ompl::base::StateSampler.
A shared pointer wrapper for ompl::base::StateSpace.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:71
virtual void computeSignature(std::vector< int > &signature) const
Compute an array of ints that uniquely identifies the structure of the state space....
Definition: StateSpace.cpp:219
const std::string & getName() const
Get the name of the state space.
Definition: StateSpace.cpp:196
StateSamplerAllocator getStateSamplerAllocator() const
Get a sampler allocator to a sampler that can be specified for a StateSpace, such that all sampled st...
virtual void generateSamples(unsigned int count)
Generate count states uniformly at random and store them in this structure.
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 loadStates(const Header &h, boost::archive::binary_iarchive &ia)
Load the states from a binary archive ia, given the loaded header is h.
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 load(const char *filename)
Load a set of states from a specified file.
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.
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...
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...
virtual void clear()
Clear the stored states. This frees all the memory.
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...
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...
StateStorage(StateSpacePtr space)
The state space to store states for is specified as argument.
void store(const char *filename)
Save a set of states to a file.
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...
void freeMemory()
Free the memory allocated for states.
void sort(const std::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...
Definition of an abstract state.
Definition: State.h:50
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
std::function< StateSamplerPtr(const StateSpace *)> StateSamplerAllocator
Definition of a function that can allocate a state sampler.
Definition: StateSampler.h:191
STL namespace.
Information stored at the beginning of the archive.
Definition: StateStorage.h:159