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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines