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 
48 static 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 
71 static const std::uint_fast32_t OMPL_ARCHIVE_MARKER = 0x4C504D4F; // this spells OMPL
73 
74 ompl::base::StateStorage::StateStorage(StateSpacePtr space) : space_(std::move(space)), hasMetadata_(false)
75 {
76 }
77 
78 ompl::base::StateStorage::~StateStorage()
79 {
80  freeMemory();
81 }
82 
83 void ompl::base::StateStorage::load(const char *filename)
84 {
85  std::ifstream in(filename, std::ios::binary);
86  load(in);
87  in.close();
88 }
89 
90 void ompl::base::StateStorage::store(const char *filename)
91 {
92  std::ofstream out(filename, std::ios::binary);
93  store(out);
94  out.close();
95 }
96 
97 void 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 
133 void 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 
150 void ompl::base::StateStorage::loadMetadata(const Header & /*h*/, boost::archive::binary_iarchive & /*ia*/)
151 {
152 }
153 
154 void 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 
180 void 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 
194 void 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 
230 void 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 
263 void 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
Information stored at the beginning of the archive.
Definition: StateStorage.h:159