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