ompl/base/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_BASE_PLANNER_DATA_STORAGE_
00038 #define OMPL_BASE_PLANNER_DATA_STORAGE_
00039 
00040 #include "ompl/base/PlannerData.h"
00041 #include "ompl/util/Console.h"
00042 #include <boost/archive/binary_oarchive.hpp>
00043 #include <boost/archive/binary_iarchive.hpp>
00044 #include <boost/serialization/vector.hpp>
00045 #include <boost/serialization/utility.hpp>
00046 #include <fstream>
00047 
00048 namespace ompl
00049 {
00050     namespace base
00051     {
00079         class PlannerDataStorage
00080         {
00081         public:
00082 
00084             PlannerDataStorage();
00086             virtual ~PlannerDataStorage();
00087 
00089             virtual void store(const PlannerData &pd, const char *filename);
00090 
00092             virtual void store(const PlannerData &pd, std::ostream &out);
00093 
00097             virtual void load(const char *filename, PlannerData &pd);
00098 
00102             virtual void load(std::istream &in, PlannerData &pd);
00103 
00104         protected:
00106             struct Header
00107             {
00109                 boost::uint32_t  marker;
00110 
00112                 std::size_t      vertex_count;
00113 
00115                 std::size_t      edge_count;
00116 
00118                 std::vector<int> signature;
00119 
00121                 template<typename Archive>
00122                 void serialize(Archive & ar, const unsigned int /*version*/)
00123                 {
00124                     ar & marker;
00125                     ar & vertex_count;
00126                     ar & edge_count;
00127                     ar & signature;
00128                 }
00129             };
00130 
00132             struct PlannerDataVertexData
00133             {
00134                 enum VertexType
00135                 {
00136                     STANDARD = 0,
00137                     START,
00138                     GOAL
00139                 };
00140 
00141                 template<typename Archive>
00142                 void serialize(Archive & ar, const unsigned int /*version*/)
00143                 {
00144                     ar & v_;
00145                     ar & state_;
00146                     ar & type_;
00147                 }
00148 
00149                 const PlannerDataVertex *v_;
00150                 std::vector<unsigned char> state_;
00151                 VertexType type_;
00152             };
00153 
00155             struct PlannerDataEdgeData
00156             {
00157                 template<typename Archive>
00158                 void serialize(Archive & ar, const unsigned int /*version*/)
00159                 {
00160                     ar & e_;
00161                     ar & endpoints_;
00162                     ar & weight_;
00163                 }
00164 
00165                 const PlannerDataEdge *e_;
00166                 std::pair<unsigned int, unsigned int> endpoints_;
00167                 double weight_;
00168             };
00169 
00171             virtual void loadVertices(PlannerData &pd, unsigned int numVertices, boost::archive::binary_iarchive &ia)
00172             {
00173                 const StateSpacePtr &space = pd.getSpaceInformation()->getStateSpace();
00174                 std::vector<State*> states;
00175                 for (unsigned int i = 0; i < numVertices; ++i)
00176                 {
00177                     PlannerDataVertexData vertexData;
00178                     ia >> vertexData;
00179 
00180                     // Deserializing all data in the vertex (except the state)
00181                     const PlannerDataVertex *v = vertexData.v_;
00182 
00183                     // Allocating a new state and deserializing it from the buffer
00184                     State *state = space->allocState();
00185                     states.push_back(state);
00186                     space->deserialize (state, &vertexData.state_[0]);
00187                     const_cast<PlannerDataVertex*>(v)->state_ = state;
00188 
00189                     // Record the type of the vertex (i.e. start vertex).
00190                     if (vertexData.type_ == PlannerDataVertexData::START)
00191                         pd.addStartVertex(*v);
00192                     else if (vertexData.type_ == PlannerDataVertexData::GOAL)
00193                         pd.addGoalVertex(*v);
00194                     else
00195                         pd.addVertex(*v);
00196 
00197                     // We deserialized the vertex object pointer, and we own it.
00198                     // Since addEdge copies the object, it is safe to free here.
00199                     delete vertexData.v_;
00200                 }
00201 
00202                 // These vertices are using state pointers allocated here.
00203                 // To avoid a memory leak, we decouple planner data from the
00204                 // 'planner', which will clone all states and properly free the
00205                 // memory when PlannerData goes out of scope.  Then it is safe
00206                 // to free all memory allocated here.
00207                 pd.decoupleFromPlanner();
00208 
00209                 for (size_t i = 0; i < states.size(); ++i)
00210                     space->freeState(states[i]);
00211             }
00212 
00214             virtual void storeVertices(const PlannerData &pd, boost::archive::binary_oarchive &oa)
00215             {
00216                 const StateSpacePtr &space = pd.getSpaceInformation()->getStateSpace();
00217                 std::vector<unsigned char> state (space->getSerializationLength());
00218                 for (unsigned int i = 0; i < pd.numVertices(); ++i)
00219                 {
00220                     PlannerDataVertexData vertexData;
00221 
00222                     // Serializing all data in the vertex (except the state)
00223                     const PlannerDataVertex &v = pd.getVertex(i);
00224                     vertexData.v_ = &v;
00225 
00226                     // Record the type of the vertex (i.e. start vertex).
00227                     if (pd.isStartVertex(i))
00228                         vertexData.type_ = PlannerDataVertexData::START;
00229                     else if (pd.isGoalVertex(i))
00230                         vertexData.type_ = PlannerDataVertexData::GOAL;
00231                     else vertexData.type_ = PlannerDataVertexData::STANDARD;
00232 
00233                     // Serializing the state contained in this vertex
00234                     space->serialize (&state[0], v.getState());
00235                     vertexData.state_ = state;
00236 
00237                     oa << vertexData;
00238                 }
00239             }
00240 
00242             virtual void loadEdges(PlannerData &pd, unsigned int numEdges, boost::archive::binary_iarchive &ia)
00243             {
00244                 for (unsigned int i = 0; i < numEdges; ++i)
00245                 {
00246                     PlannerDataEdgeData edgeData;
00247                     ia >> edgeData;
00248                     pd.addEdge(edgeData.endpoints_.first, edgeData.endpoints_.second, *edgeData.e_, Cost(edgeData.weight_));
00249 
00250                     // We deserialized the edge object pointer, and we own it.
00251                     // Since addEdge copies the object, it is safe to free here.
00252                     delete edgeData.e_;
00253                 }
00254             }
00255 
00257             virtual void storeEdges(const PlannerData &pd, boost::archive::binary_oarchive &oa)
00258             {
00259                 for (unsigned int i = 0; i < pd.numVertices(); ++i)
00260                     for (unsigned int j = 0; j < pd.numVertices(); ++j)
00261                     {
00262                         if(pd.edgeExists(i, j))
00263                         {
00264                             PlannerDataEdgeData edgeData;
00265                             edgeData.e_ = &pd.getEdge(i, j);
00266                             edgeData.endpoints_.first = i;
00267                             edgeData.endpoints_.second = j;
00268                             Cost weight;
00269                             pd.getEdgeWeight(i, j, &weight);
00270                             edgeData.weight_ = weight.value();
00271 
00272                             oa << edgeData;
00273                         }
00274                     }
00275             }
00276         };
00277     }
00278 }
00279 
00280 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines