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 
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 }