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 }
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...
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
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.
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...
StateSamplerAllocator getStateSamplerAllocator() 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...
Definition of an abstract state.
Definition: State.h:113
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.
virtual void generateSamples(unsigned int count)
Generate count states uniformly at random and store them in this structure.
void load(const char *filename)
Load a set of states from a specified file.
void freeMemory()
Free the memory allocated for states.
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...
Information stored at the beginning of the archive.
Definition: StateStorage.h:254
const std::string & getName() const
Get the name of the state space.
Definition: StateSpace.cpp:196
StateStorage(StateSpacePtr space)
The state space to store states for is specified as argument.
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
virtual void clear()
Clear the stored states. This frees all the memory.
std::function< StateSamplerPtr(const StateSpace *)> StateSamplerAllocator
Definition of a function that can allocate a state sampler.
Definition: StateSampler.h:255
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
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...
A shared pointer wrapper for ompl::base::StateSpace.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
A shared pointer wrapper for ompl::base::StateSampler.
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.
The exception type for ompl.
Definition: Exception.h:78
virtual void print(std::ostream &out=std::cout) const
Output the set of states to a specified stream, in a human readable fashion.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
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...