ompl/base/src/StateStorage.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2012, Willow Garage 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan */ 00036 00037 #include "ompl/base/StateStorage.h" 00038 #include "ompl/base/PrecomputedStateSampler.h" 00039 #include "ompl/util/Exception.h" 00040 #include <fstream> 00041 #include <algorithm> 00042 #include <boost/bind.hpp> 00043 00044 #include <boost/serialization/binary_object.hpp> 00045 #include <boost/archive/archive_exception.hpp> 00046 00048 static ompl::base::StateSamplerPtr allocPrecomputedStateSampler(const ompl::base::StateSpace *space, 00049 const std::vector<int> &expectedSignature, 00050 const std::vector<const ompl::base::State*> *states, 00051 std::size_t minIndex, std::size_t maxIndex) 00052 { 00053 std::vector<int> sig; 00054 space->computeSignature(sig); 00055 if (sig != expectedSignature) 00056 { 00057 std::stringstream ss; 00058 ss << "Cannot allocate state sampler for a state space whose signature does not match that of the stored states. "; 00059 ss << "Expected signature "; 00060 for (std::size_t i = 0 ; i < expectedSignature.size() ; ++i) 00061 ss << expectedSignature[i] << " "; 00062 ss << "but space " << space->getName() << " has signature "; 00063 for (std::size_t i = 0 ; i < sig.size() ; ++i) 00064 ss << sig[i] << " "; 00065 throw ompl::Exception(ss.str()); 00066 } 00067 return ompl::base::StateSamplerPtr(new ompl::base::PrecomputedStateSampler(space, *states, minIndex, maxIndex)); 00068 } 00069 00070 static const boost::uint32_t OMPL_ARCHIVE_MARKER = 0x4C504D4F; // this spells OMPL 00072 00073 ompl::base::StateStorage::StateStorage(const StateSpacePtr &space) : space_(space), hasMetadata_(false) 00074 { 00075 } 00076 00077 ompl::base::StateStorage::~StateStorage() 00078 { 00079 freeMemory(); 00080 } 00081 00082 void ompl::base::StateStorage::load(const char *filename) 00083 { 00084 std::ifstream in(filename, std::ios::binary); 00085 load(in); 00086 in.close(); 00087 } 00088 00089 void ompl::base::StateStorage::store(const char *filename) 00090 { 00091 std::ofstream out(filename, std::ios::binary); 00092 store(out); 00093 out.close(); 00094 } 00095 00096 void ompl::base::StateStorage::load(std::istream &in) 00097 { 00098 clear(); 00099 if (!in.good() || in.eof()) 00100 { 00101 OMPL_WARN("Unable to load states"); 00102 return; 00103 } 00104 try 00105 { 00106 00107 boost::archive::binary_iarchive ia(in); 00108 Header h; 00109 ia >> h; 00110 if (h.marker != OMPL_ARCHIVE_MARKER) 00111 { 00112 OMPL_ERROR("OMPL archive marker not found"); 00113 return; 00114 } 00115 00116 std::vector<int> sig; 00117 space_->computeSignature(sig); 00118 if (h.signature != sig) 00119 { 00120 OMPL_ERROR("State space signatures do not match"); 00121 return; 00122 } 00123 loadStates(h, ia); 00124 loadMetadata(h, ia); 00125 } 00126 00127 catch (boost::archive::archive_exception &ae) 00128 { 00129 OMPL_ERROR("Unable to load archive: %s", ae.what()); 00130 } 00131 00132 } 00133 00134 void ompl::base::StateStorage::loadStates(const Header &h, boost::archive::binary_iarchive &ia) 00135 { 00136 OMPL_DEBUG("Deserializing %u states", h.state_count); 00137 // load the file 00138 unsigned int l = space_->getSerializationLength(); 00139 char *buffer = new char[l]; 00140 State *s = space_->allocState(); 00141 for (std::size_t i = 0 ; i < h.state_count ; ++i) 00142 { 00143 ia >> boost::serialization::make_binary_object(buffer, l); 00144 space_->deserialize(s, buffer); 00145 addState(s); 00146 } 00147 space_->freeState(s); 00148 delete[] buffer; 00149 } 00150 00151 void ompl::base::StateStorage::loadMetadata(const Header& /*h*/, boost::archive::binary_iarchive& /*ia*/) 00152 { 00153 } 00154 00155 00156 void ompl::base::StateStorage::store(std::ostream &out) 00157 { 00158 if (!out.good()) 00159 { 00160 OMPL_WARN("Unable to store states"); 00161 return; 00162 } 00163 try 00164 { 00165 Header h; 00166 h.marker = OMPL_ARCHIVE_MARKER; 00167 h.state_count = states_.size(); 00168 space_->computeSignature(h.signature); 00169 00170 boost::archive::binary_oarchive oa(out); 00171 oa << h; 00172 00173 storeStates(h, oa); 00174 storeMetadata(h, oa); 00175 } 00176 catch (boost::archive::archive_exception &ae) 00177 { 00178 OMPL_ERROR("Unable to save archive: %s", ae.what()); 00179 } 00180 } 00181 00182 void ompl::base::StateStorage::storeStates(const Header& /*h*/, boost::archive::binary_oarchive &oa) 00183 { 00184 OMPL_DEBUG("Serializing %u states", (unsigned int)states_.size()); 00185 00186 unsigned int l = space_->getSerializationLength(); 00187 char *buffer = new char[l]; 00188 for (std::size_t i = 0 ; i < states_.size() ; ++i) 00189 { 00190 space_->serialize(buffer, states_[i]); 00191 oa << boost::serialization::make_binary_object(buffer, l); 00192 } 00193 delete[] buffer; 00194 } 00195 00196 void ompl::base::StateStorage::storeMetadata(const Header& /*h*/, boost::archive::binary_oarchive& /*oa*/) 00197 { 00198 } 00199 00200 void ompl::base::StateStorage::addState(const State *state) 00201 { 00202 State *copy = space_->allocState(); 00203 space_->copyState(copy, state); 00204 states_.push_back(copy); 00205 } 00206 00207 void ompl::base::StateStorage::generateSamples(unsigned int count) 00208 { 00209 StateSamplerPtr ss = space_->allocStateSampler(); 00210 states_.reserve(states_.size() + count); 00211 State *s = space_->allocState(); 00212 for (unsigned int i = 0 ; i < count ; ++i) 00213 { 00214 ss->sampleUniform(s); 00215 addState(s); 00216 } 00217 space_->freeState(s); 00218 } 00219 00220 void ompl::base::StateStorage::freeMemory() 00221 { 00222 for (std::size_t i = 0 ; i < states_.size() ; ++i) 00223 space_->freeState(const_cast<State*>(states_[i])); 00224 } 00225 00226 void ompl::base::StateStorage::clear() 00227 { 00228 freeMemory(); 00229 states_.clear(); 00230 } 00231 00232 void ompl::base::StateStorage::sort(const boost::function<bool(const State*, const State*)> &op) 00233 { 00234 std::sort(states_.begin(), states_.end(), op); 00235 } 00236 00237 ompl::base::StateSamplerAllocator ompl::base::StateStorage::getStateSamplerAllocator() const 00238 { 00239 return getStateSamplerAllocatorRange(0, states_.empty() ? 0 : states_.size() - 1); 00240 } 00241 00242 ompl::base::StateSamplerAllocator ompl::base::StateStorage::getStateSamplerAllocatorRangeUntil(std::size_t until) const 00243 { 00244 return getStateSamplerAllocatorRange(0, until); 00245 } 00246 00247 ompl::base::StateSamplerAllocator ompl::base::StateStorage::getStateSamplerAllocatorRangeAfter(std::size_t after) const 00248 { 00249 return getStateSamplerAllocatorRange(after, states_.empty() ? 0 : states_.size() - 1); 00250 } 00251 00252 ompl::base::StateSamplerAllocator ompl::base::StateStorage::getStateSamplerAllocatorRange(std::size_t from, std::size_t to) const 00253 { 00254 if (states_.empty()) 00255 throw Exception("Cannot allocate state sampler from empty state storage"); 00256 std::vector<int> sig; 00257 space_->computeSignature(sig); 00258 return boost::bind(&allocPrecomputedStateSampler, _1, sig, &states_, from, to); 00259 } 00260 00261 void ompl::base::StateStorage::print(std::ostream &out) const 00262 { 00263 for (std::size_t i = 0 ; i < states_.size() ; ++i) 00264 space_->printState(states_[i], out); 00265 }