All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
PlannerDataStorage.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2012, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ryan Luna */
36 
37 // PlannerDataStorage requires Boost version >= 1.44
38 #include <boost/version.hpp>
39 #if BOOST_VERSION < 104400
40 #warning Boost version >= 1.44 is required for PlannerDataStorage classes
41 #else
42 
43 #include "ompl/base/PlannerDataStorage.h"
44 #include <boost/archive/archive_exception.hpp>
45 
46 static const boost::uint32_t OMPL_PLANNER_DATA_ARCHIVE_MARKER = 0x5044414D; // this spells PDAM
47 
49 {
50 }
51 
53 {
54 }
55 
56 void ompl::base::PlannerDataStorage::store(const PlannerData& pd, const char *filename)
57 {
58  std::ofstream out(filename, std::ios::binary);
59  store(pd, out);
60  out.close();
61 }
62 
63 void ompl::base::PlannerDataStorage::store(const PlannerData& pd, std::ostream &out)
64 {
66  if (!out.good())
67  {
68  logError("Failed to store PlannerData: output stream is invalid");
69  return;
70  }
71  if (!si)
72  {
73  logError("Failed to store PlannerData: SpaceInformation is invalid");
74  return;
75  }
76  try
77  {
78  boost::archive::binary_oarchive oa(out);
79 
80  // Writing the header
81  Header h;
82  h.marker = OMPL_PLANNER_DATA_ARCHIVE_MARKER;
83  h.vertex_count = pd.numVertices();
84  h.edge_count = pd.numEdges();
85  si->getStateSpace()->computeSignature(h.signature);
86  oa << h;
87 
88  storeVertices(pd, oa);
89  storeEdges(pd, oa);
90  }
91  catch (boost::archive::archive_exception &ae)
92  {
93  logError("Failed to store PlannerData: %s", ae.what());
94  }
95 }
96 
97 void ompl::base::PlannerDataStorage::load(const char *filename, PlannerData& pd)
98 {
99  std::ifstream in(filename, std::ios::binary);
100  load(in, pd);
101  in.close();
102 }
103 
105 {
106  pd.clear();
107 
108  const SpaceInformationPtr &si = pd.getSpaceInformation();
109  if (!in.good())
110  {
111  logError("Failed to load PlannerData: input stream is invalid");
112  return;
113  }
114  if (!si)
115  {
116  logError("Failed to load PlannerData: SpaceInformation is invalid");
117  return;
118  }
119  // Loading the planner data:
120  try
121  {
122  boost::archive::binary_iarchive ia(in);
123 
124  // Read the header
125  Header h;
126  ia >> h;
127 
128  // Checking the archive marker
129  if (h.marker != OMPL_PLANNER_DATA_ARCHIVE_MARKER)
130  {
131  logError("Failed to load PlannerData: PlannerData archive marker not found");
132  return;
133  }
134 
135  // Verify that the state space is the same
136  std::vector<int> sig;
137  si->getStateSpace()->computeSignature(sig);
138  if (h.signature != sig)
139  {
140  logError("Failed to load PlannerData: StateSpace signature mismatch");
141  return;
142  }
143 
144  // File seems ok... loading vertices and edges
145  loadVertices(pd, h.vertex_count, ia);
146  loadEdges(pd, h.edge_count, ia);
147  }
148  catch (boost::archive::archive_exception &ae)
149  {
150  logError("Failed to load PlannerData: %s", ae.what());
151  }
152 }
153 
154 #endif