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