All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
src/ompl/control/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 // PlannerDataStorage requires Boost version >= 1.44
00038 #include <boost/version.hpp>
00039 #if BOOST_VERSION < 104400
00040 #warning Boost version >= 1.44 is required for PlannerDataStorage classes
00041 #else
00042 
00043 #include "ompl/control/PlannerDataStorage.h"
00044 #include <boost/archive/archive_exception.hpp>
00045 
00047 static const boost::uint32_t OMPL_PLANNER_DATA_CONTROL_ARCHIVE_MARKER = 0x5044434D; // this spells PDCM
00049 
00050 ompl::control::PlannerDataStorage::PlannerDataStorage(void) : base::PlannerDataStorage()
00051 {
00052 }
00053 
00054 ompl::control::PlannerDataStorage::~PlannerDataStorage(void)
00055 {
00056 }
00057 
00058 void ompl::control::PlannerDataStorage::load(const char *filename, base::PlannerData& pd)
00059 {
00060     base::PlannerDataStorage::load(filename, pd);
00061 }
00062 
00063 void ompl::control::PlannerDataStorage::load(std::istream &in, base::PlannerData& pd)
00064 {
00065     if (!pd.hasControls())
00066     {
00067         logWarn("PlannerData does not have controls.  Invoking base::PlannerDataStorage::load");
00068         base::PlannerDataStorage::load(in, pd);
00069         return;
00070     }
00071 
00072     control::PlannerData* pdc = static_cast<control::PlannerData*>(&pd);
00073     pdc->clear();
00074 
00075     const SpaceInformationPtr &si = pdc->getSpaceInformation();
00076     if (!in.good())
00077     {
00078         logError("Failed to load PlannerData: input stream is invalid");
00079         return;
00080     }
00081     if (!si)
00082     {
00083         logError("Failed to load PlannerData: SpaceInformation is invalid");
00084         return;
00085     }
00086     // Loading the planner data:
00087     try
00088     {
00089         boost::archive::binary_iarchive ia(in);
00090 
00091         // Read the header
00092         Header h;
00093         ia >> h;
00094 
00095         // Checking the archive marker
00096         if (h.marker != OMPL_PLANNER_DATA_CONTROL_ARCHIVE_MARKER)
00097         {
00098             logError("Failed to load PlannerData: PlannerData control archive marker not found");
00099             return;
00100         }
00101 
00102         // Verify that the state space is the same
00103         std::vector<int> sig;
00104         si->getStateSpace()->computeSignature(sig);
00105         if (h.signature != sig)
00106         {
00107             logError("Failed to load PlannerData: StateSpace signature mismatch");
00108             return;
00109         }
00110 
00111         // Verify that the control space is the same
00112         sig.clear();
00113         si->getControlSpace()->computeSignature(sig);
00114         if (h.control_signature != sig)
00115         {
00116             logError("Failed to load PlannerData: ControlSpace signature mismatch");
00117             return;
00118         }
00119 
00120         // File seems ok... loading vertices and edges
00121         loadVertices(pd, h.vertex_count, ia);
00122         loadEdges(pd, h.edge_count, ia);
00123     }
00124     catch (boost::archive::archive_exception &ae)
00125     {
00126         logError("Failed to load PlannerData: %s", ae.what());
00127     }
00128 }
00129 
00130 void ompl::control::PlannerDataStorage::store(const base::PlannerData& pd, const char *filename)
00131 {
00132     base::PlannerDataStorage::store(pd, filename);
00133 }
00134 
00135 void ompl::control::PlannerDataStorage::store(const base::PlannerData& pd, std::ostream &out)
00136 {
00137     const control::PlannerData* pdc = static_cast<const control::PlannerData*>(&pd);
00138     if (!pdc)
00139     {
00140         logWarn("Failed to cast PlannerData to control::PlannerData.  Invoking base::PlannerDataStorage::store");
00141         base::PlannerDataStorage::store(pd, out);
00142         return;
00143     }
00144 
00145     const SpaceInformationPtr &si = pdc->getSpaceInformation();
00146     if (!out.good())
00147     {
00148         logError("Failed to store PlannerData: output stream is invalid");
00149         return;
00150     }
00151     if (!si)
00152     {
00153         logError("Failed to store PlannerData: SpaceInformation is invalid");
00154         return;
00155     }
00156     try
00157     {
00158         boost::archive::binary_oarchive oa(out);
00159 
00160         // Writing the header
00161         Header h;
00162         h.marker = OMPL_PLANNER_DATA_CONTROL_ARCHIVE_MARKER;
00163         h.vertex_count = pdc->numVertices();
00164         h.edge_count = pdc->numEdges();
00165         si->getStateSpace()->computeSignature(h.signature);
00166         si->getControlSpace()->computeSignature(h.control_signature);
00167         oa << h;
00168 
00169         storeVertices(pd, oa);
00170         storeEdges(pd, oa);
00171     }
00172     catch (boost::archive::archive_exception &ae)
00173     {
00174         logError("Failed to store PlannerData: %s", ae.what());
00175     }
00176 }
00177 
00178 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines