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