All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 #include <boost/bind.hpp>
43 
44 #include <boost/serialization/binary_object.hpp>
45 #include <boost/archive/archive_exception.hpp>
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 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)
64  ss << sig[i] << " ";
65  throw ompl::Exception(ss.str());
66  }
67  return ompl::base::StateSamplerPtr(new ompl::base::PrecomputedStateSampler(space, *states, minIndex, maxIndex));
68 }
69 
70 static const boost::uint32_t OMPL_ARCHIVE_MARKER = 0x4C504D4F; // this spells OMPL
72 
73 ompl::base::StateStorage::StateStorage(const StateSpacePtr &space) : space_(space), hasMetadata_(false)
74 {
75 }
76 
77 ompl::base::StateStorage::~StateStorage(void)
78 {
79  freeMemory();
80 }
81 
82 void ompl::base::StateStorage::load(const char *filename)
83 {
84  std::ifstream in(filename, std::ios::binary);
85  load(in);
86  in.close();
87 }
88 
89 void ompl::base::StateStorage::store(const char *filename)
90 {
91  std::ofstream out(filename, std::ios::binary);
92  store(out);
93  out.close();
94 }
95 
96 void ompl::base::StateStorage::load(std::istream &in)
97 {
98  clear();
99  if (!in.good() || in.eof())
100  {
101  OMPL_WARN("Unable to load states");
102  return;
103  }
104  try
105  {
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 
134 void ompl::base::StateStorage::loadStates(const Header &h, boost::archive::binary_iarchive &ia)
135 {
136  OMPL_DEBUG("Deserializing %u states", h.state_count);
137  // load the file
138  unsigned int l = space_->getSerializationLength();
139  char *buffer = new char[l];
140  State *s = space_->allocState();
141  for (std::size_t i = 0 ; i < h.state_count ; ++i)
142  {
143  ia >> boost::serialization::make_binary_object(buffer, l);
144  space_->deserialize(s, buffer);
145  addState(s);
146  }
147  space_->freeState(s);
148  delete[] buffer;
149 }
150 
151 void ompl::base::StateStorage::loadMetadata(const Header& /*h*/, boost::archive::binary_iarchive& /*ia*/)
152 {
153 }
154 
155 
156 void ompl::base::StateStorage::store(std::ostream &out)
157 {
158  if (!out.good())
159  {
160  OMPL_WARN("Unable to store states");
161  return;
162  }
163  try
164  {
165  Header h;
166  h.marker = OMPL_ARCHIVE_MARKER;
167  h.state_count = states_.size();
168  space_->computeSignature(h.signature);
169 
170  boost::archive::binary_oarchive oa(out);
171  oa << h;
172 
173  storeStates(h, oa);
174  storeMetadata(h, oa);
175  }
176  catch (boost::archive::archive_exception &ae)
177  {
178  OMPL_ERROR("Unable to save archive: %s", ae.what());
179  }
180 }
181 
182 void ompl::base::StateStorage::storeStates(const Header& /*h*/, boost::archive::binary_oarchive &oa)
183 {
184  OMPL_DEBUG("Serializing %u states", (unsigned int)states_.size());
185 
186  unsigned int l = space_->getSerializationLength();
187  char *buffer = new char[l];
188  for (std::size_t i = 0 ; i < states_.size() ; ++i)
189  {
190  space_->serialize(buffer, states_[i]);
191  oa << boost::serialization::make_binary_object(buffer, l);
192  }
193  delete[] buffer;
194 }
195 
196 void ompl::base::StateStorage::storeMetadata(const Header& /*h*/, boost::archive::binary_oarchive& /*oa*/)
197 {
198 }
199 
201 {
202  State *copy = space_->allocState();
203  space_->copyState(copy, state);
204  states_.push_back(copy);
205 }
206 
208 {
209  StateSamplerPtr ss = space_->allocStateSampler();
210  states_.reserve(states_.size() + count);
211  State *s = space_->allocState();
212  for (unsigned int i = 0 ; i < count ; ++i)
213  {
214  ss->sampleUniform(s);
215  addState(s);
216  }
217  space_->freeState(s);
218 }
219 
221 {
222  for (std::size_t i = 0 ; i < states_.size() ; ++i)
223  space_->freeState(const_cast<State*>(states_[i]));
224 }
225 
227 {
228  freeMemory();
229  states_.clear();
230 }
231 
232 void ompl::base::StateStorage::sort(const boost::function<bool(const State*, const State*)> &op)
233 {
234  std::sort(states_.begin(), states_.end(), op);
235 }
236 
238 {
239  return getStateSamplerAllocatorRange(0, states_.empty() ? 0 : states_.size() - 1);
240 }
241 
243 {
244  return getStateSamplerAllocatorRange(0, until);
245 }
246 
248 {
249  return getStateSamplerAllocatorRange(after, states_.empty() ? 0 : states_.size() - 1);
250 }
251 
253 {
254  if (states_.empty())
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);
259 }
260 
261 void ompl::base::StateStorage::print(std::ostream &out) const
262 {
263  for (std::size_t i = 0 ; i < states_.size() ; ++i)
264  space_->printState(states_[i], out);
265 }
boost::function< StateSamplerPtr(const StateSpace *)> StateSamplerAllocator
Definition of a function that can allocate a state sampler.
Definition: StateSampler.h:184
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.
Definition: StateSpace.cpp:209
StateSamplerAllocator getStateSamplerAllocator(void) const
Get a sampler allocator to a sampler that can be specified for a StateSpace, such that all sampled st...
std::size_t state_count
Number of states stored in the archive.
Definition: StateStorage.h:158
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.
Definition: Console.h:64
const std::string & getName(void) const
Get the name of the state space.
Definition: StateSpace.cpp:187
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: StateSpace.h:73
Definition of an abstract state.
Definition: State.h:50
std::vector< int > signature
Signature of state space that allocated the saved states (see ompl::base::StateSpace::computeSignatur...
Definition: StateStorage.h:161
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
Information stored at the beginning of the archive.
Definition: StateStorage.h:152
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.
Definition: Exception.h:47
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
void load(const char *filename)
Load a set of states from a specified file.
boost::uint32_t marker
OMPL specific marker (fixed value)
Definition: StateStorage.h:155
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.