All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
src/ompl/control/PlannerDataStorage.h
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 #ifndef OMPL_CONTROL_PLANNER_DATA_STORAGE_
00038 #define OMPL_CONTROL_PLANNER_DATA_STORAGE_
00039 
00040 // PlannerDataStorage requires Boost version >= 1.44
00041 #include <boost/version.hpp>
00042 #if BOOST_VERSION < 104400
00043 #warning Boost version >= 1.44 is required for PlannerDataStorage classes
00044 #else
00045 
00046 #include "ompl/base/PlannerDataStorage.h"
00047 #include "ompl/control/PlannerData.h"
00048 #include "ompl/control/SpaceInformation.h"
00049 
00050 namespace ompl
00051 {
00052     namespace control
00053     {
00058         class PlannerDataStorage : public base::PlannerDataStorage
00059         {
00060         public:
00062             PlannerDataStorage(void);
00064             virtual ~PlannerDataStorage(void);
00065 
00067             virtual void load(const char *filename, base::PlannerData& pd);
00068 
00070             virtual void load(std::istream &in, base::PlannerData& pd);
00071 
00075             virtual void store(const base::PlannerData& pd, const char *filename);
00076 
00080             virtual void store(const base::PlannerData& pd, std::ostream &out);
00081 
00082         protected:
00083 
00085             // Information stored at the beginning of the PlannerData archive
00086             struct Header : base::PlannerDataStorage::Header
00087             {
00089                 std::vector<int> control_signature;
00090 
00092                 template<typename Archive>
00093                 void serialize(Archive & ar, const unsigned int version)
00094                 {
00095                     ar & boost::serialization::base_object<base::PlannerDataStorage::Header>(*this);
00096                     ar & control_signature;
00097                 }
00098             };
00099 
00100             // The object containing all control edge data that will be stored
00101             struct PlannerDataEdgeControlData : base::PlannerDataStorage::PlannerDataEdgeData
00102             {
00103                 template<typename Archive>
00104                 void serialize(Archive & ar, const unsigned int version)
00105                 {
00106                     ar & boost::serialization::base_object<base::PlannerDataStorage::PlannerDataEdgeData>(*this);
00107                     ar & control_;
00108                 }
00109 
00110                 std::vector<unsigned char> control_;
00111             };
00113 
00116             virtual void loadEdges(base::PlannerData &pd, unsigned int numEdges, boost::archive::binary_iarchive &ia)
00117             {
00118                 logDebug("Loading %d PlannerDataEdgeControl objects", numEdges);
00119 
00120                 const ControlSpacePtr& space = static_cast<control::PlannerData&>(pd).getSpaceInformation()->getControlSpace();
00121                 std::vector<Control*> controls;
00122 
00123                 for (unsigned int i = 0; i < numEdges; ++i)
00124                 {
00125                     PlannerDataEdgeControlData edgeData;
00126                     ia >> edgeData;
00127 
00128                     std::vector<unsigned char> ctrlBuf (space->getSerializationLength());
00129                     Control *ctrl = space->allocControl();
00130                     controls.push_back(ctrl);
00131                     space->deserialize(ctrl, &edgeData.control_[0]);
00132                     const_cast<PlannerDataEdgeControl*>(static_cast<const PlannerDataEdgeControl*>(edgeData.e_))->c_ = ctrl;
00133 
00134                     pd.addEdge(edgeData.endpoints_.first, edgeData.endpoints_.second, *edgeData.e_, edgeData.weight_);
00135 
00136                     // We deserialized the edge object pointer, and we own it.
00137                     // Since addEdge copies the object, it is safe to free here.
00138                     delete edgeData.e_;
00139                 }
00140 
00141                 // These edges are using control pointers allocated here.
00142                 // To avoid a memory leak, we decouple planner data from the
00143                 // 'planner', which will clone all controls and properly free the
00144                 // memory when PlannerData goes out of scope.  Then it is safe
00145                 // to free all memory allocated here.
00146                 pd.decoupleFromPlanner();
00147 
00148                 for (size_t i = 0; i < controls.size(); ++i)
00149                     space->freeControl(controls[i]);
00150             }
00151 
00154             virtual void storeEdges(const base::PlannerData &pd, boost::archive::binary_oarchive &oa)
00155             {
00156                 logDebug("Storing %d PlannerDataEdgeControl objects", pd.numEdges());
00157 
00158                 const ControlSpacePtr& space = static_cast<const control::PlannerData&>(pd).getSpaceInformation()->getControlSpace();
00159                 std::vector<unsigned char> ctrl (space->getSerializationLength());
00160 
00161                 for (unsigned int i = 0; i < pd.numVertices(); ++i)
00162                     for (unsigned int j = 0; j < pd.numVertices(); ++j)
00163                     {
00164                         if(pd.edgeExists(i, j))
00165                         {
00166                             PlannerDataEdgeControlData edgeData;
00167                             edgeData.e_ = &pd.getEdge(i, j);
00168                             edgeData.endpoints_.first = i;
00169                             edgeData.endpoints_.second = j;
00170                             edgeData.weight_ = pd.getEdgeWeight(i, j);
00171 
00172                             space->serialize(&ctrl[0], static_cast<const PlannerDataEdgeControl*>(edgeData.e_)->getControl());
00173                             edgeData.control_ = ctrl;
00174 
00175                             oa << edgeData;
00176                         }
00177                     }
00178             }
00179 
00180         };
00181     }
00182 }
00183 
00184 #endif
00185 
00186 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines