ompl/base/src/PlannerDataStorage.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2012, Rice University
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 Rice University 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: Ryan Luna */
00036 
00037 #include "ompl/base/PlannerDataStorage.h"
00038 #include <boost/archive/archive_exception.hpp>
00039 
00040 static const boost::uint32_t OMPL_PLANNER_DATA_ARCHIVE_MARKER = 0x5044414D; // this spells PDAM
00041 
00042 ompl::base::PlannerDataStorage::PlannerDataStorage()
00043 {
00044 }
00045 
00046 ompl::base::PlannerDataStorage::~PlannerDataStorage()
00047 {
00048 }
00049 
00050 void ompl::base::PlannerDataStorage::store(const PlannerData &pd, const char *filename)
00051 {
00052     std::ofstream out(filename, std::ios::binary);
00053     store(pd, out);
00054     out.close();
00055 }
00056 
00057 void ompl::base::PlannerDataStorage::store(const PlannerData &pd, std::ostream &out)
00058 {
00059     const SpaceInformationPtr &si = pd.getSpaceInformation();
00060     if (!out.good())
00061     {
00062         OMPL_ERROR("Failed to store PlannerData: output stream is invalid");
00063         return;
00064     }
00065     if (!si)
00066     {
00067         OMPL_ERROR("Failed to store PlannerData: SpaceInformation is invalid");
00068         return;
00069     }
00070     try
00071     {
00072         boost::archive::binary_oarchive oa(out);
00073 
00074         // Writing the header
00075         Header h;
00076         h.marker = OMPL_PLANNER_DATA_ARCHIVE_MARKER;
00077         h.vertex_count = pd.numVertices();
00078         h.edge_count = pd.numEdges();
00079         si->getStateSpace()->computeSignature(h.signature);
00080         oa << h;
00081 
00082         storeVertices(pd, oa);
00083         storeEdges(pd, oa);
00084     }
00085     catch (boost::archive::archive_exception &ae)
00086     {
00087         OMPL_ERROR("Failed to store PlannerData: %s", ae.what());
00088     }
00089 }
00090 
00091 void ompl::base::PlannerDataStorage::load(const char *filename, PlannerData &pd)
00092 {
00093     std::ifstream in(filename, std::ios::binary);
00094     load(in, pd);
00095     in.close();
00096 }
00097 
00098 void ompl::base::PlannerDataStorage::load(std::istream &in, PlannerData &pd)
00099 {
00100     pd.clear();
00101 
00102     const SpaceInformationPtr &si = pd.getSpaceInformation();
00103     if (!in.good())
00104     {
00105         OMPL_ERROR("Failed to load PlannerData: input stream is invalid");
00106         return;
00107     }
00108     if (!si)
00109     {
00110         OMPL_ERROR("Failed to load PlannerData: SpaceInformation is invalid");
00111         return;
00112     }
00113     // Loading the planner data:
00114     try
00115     {
00116         boost::archive::binary_iarchive ia(in);
00117 
00118         // Read the header
00119         Header h;
00120         ia >> h;
00121 
00122         // Checking the archive marker
00123         if (h.marker != OMPL_PLANNER_DATA_ARCHIVE_MARKER)
00124         {
00125             OMPL_ERROR("Failed to load PlannerData: PlannerData archive marker not found");
00126             return;
00127         }
00128 
00129         // Verify that the state space is the same
00130         std::vector<int> sig;
00131         si->getStateSpace()->computeSignature(sig);
00132         if (h.signature != sig)
00133         {
00134             OMPL_ERROR("Failed to load PlannerData: StateSpace signature mismatch");
00135             return;
00136         }
00137 
00138         // File seems ok... loading vertices and edges
00139         loadVertices(pd, h.vertex_count, ia);
00140         loadEdges(pd, h.edge_count, ia);
00141     }
00142     catch (boost::archive::archive_exception &ae)
00143     {
00144         OMPL_ERROR("Failed to load PlannerData: %s", ae.what());
00145     }
00146 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines