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/control/PlannerDataStorage.h"
44 #include <boost/archive/archive_exception.hpp>
45 
47 static const boost::uint32_t OMPL_PLANNER_DATA_CONTROL_ARCHIVE_MARKER = 0x5044434D; // this spells PDCM
49 
51 {
52 }
53 
55 {
56 }
57 
59 {
60  base::PlannerDataStorage::load(filename, pd);
61 }
62 
64 {
65  if (!pd.hasControls())
66  {
67  logWarn("PlannerData does not have controls. Invoking base::PlannerDataStorage::load");
69  return;
70  }
71 
72  control::PlannerData* pdc = static_cast<control::PlannerData*>(&pd);
73  pdc->clear();
74 
75  const SpaceInformationPtr &si = pdc->getSpaceInformation();
76  if (!in.good())
77  {
78  logError("Failed to load PlannerData: input stream is invalid");
79  return;
80  }
81  if (!si)
82  {
83  logError("Failed to load PlannerData: SpaceInformation is invalid");
84  return;
85  }
86  // Loading the planner data:
87  try
88  {
89  boost::archive::binary_iarchive ia(in);
90 
91  // Read the header
92  Header h;
93  ia >> h;
94 
95  // Checking the archive marker
96  if (h.marker != OMPL_PLANNER_DATA_CONTROL_ARCHIVE_MARKER)
97  {
98  logError("Failed to load PlannerData: PlannerData control archive marker not found");
99  return;
100  }
101 
102  // Verify that the state space is the same
103  std::vector<int> sig;
104  si->getStateSpace()->computeSignature(sig);
105  if (h.signature != sig)
106  {
107  logError("Failed to load PlannerData: StateSpace signature mismatch");
108  return;
109  }
110 
111  // Verify that the control space is the same
112  sig.clear();
113  si->getControlSpace()->computeSignature(sig);
114  if (h.control_signature != sig)
115  {
116  logError("Failed to load PlannerData: ControlSpace signature mismatch");
117  return;
118  }
119 
120  // File seems ok... loading vertices and edges
121  loadVertices(pd, h.vertex_count, ia);
122  loadEdges(pd, h.edge_count, ia);
123  }
124  catch (boost::archive::archive_exception &ae)
125  {
126  logError("Failed to load PlannerData: %s", ae.what());
127  }
128 }
129 
130 void ompl::control::PlannerDataStorage::store(const base::PlannerData& pd, const char *filename)
131 {
132  base::PlannerDataStorage::store(pd, filename);
133 }
134 
136 {
137  const control::PlannerData* pdc = static_cast<const control::PlannerData*>(&pd);
138  if (!pdc)
139  {
140  logWarn("Failed to cast PlannerData to control::PlannerData. Invoking base::PlannerDataStorage::store");
142  return;
143  }
144 
145  const SpaceInformationPtr &si = pdc->getSpaceInformation();
146  if (!out.good())
147  {
148  logError("Failed to store PlannerData: output stream is invalid");
149  return;
150  }
151  if (!si)
152  {
153  logError("Failed to store PlannerData: SpaceInformation is invalid");
154  return;
155  }
156  try
157  {
158  boost::archive::binary_oarchive oa(out);
159 
160  // Writing the header
161  Header h;
162  h.marker = OMPL_PLANNER_DATA_CONTROL_ARCHIVE_MARKER;
163  h.vertex_count = pdc->numVertices();
164  h.edge_count = pdc->numEdges();
165  si->getStateSpace()->computeSignature(h.signature);
166  si->getControlSpace()->computeSignature(h.control_signature);
167  oa << h;
168 
169  storeVertices(pd, oa);
170  storeEdges(pd, oa);
171  }
172  catch (boost::archive::archive_exception &ae)
173  {
174  logError("Failed to store PlannerData: %s", ae.what());
175  }
176 }
177 
178 #endif