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 #include "ompl/base/PlannerDataStorage.h"
38 #include <boost/archive/archive_exception.hpp>
39 
40 static const boost::uint32_t OMPL_PLANNER_DATA_ARCHIVE_MARKER = 0x5044414D; // this spells PDAM
41 
43 {
44 }
45 
47 {
48 }
49 
50 void ompl::base::PlannerDataStorage::store(const PlannerData &pd, const char *filename)
51 {
52  std::ofstream out(filename, std::ios::binary);
53  store(pd, out);
54  out.close();
55 }
56 
57 void ompl::base::PlannerDataStorage::store(const PlannerData &pd, std::ostream &out)
58 {
60  if (!out.good())
61  {
62  OMPL_ERROR("Failed to store PlannerData: output stream is invalid");
63  return;
64  }
65  if (!si)
66  {
67  OMPL_ERROR("Failed to store PlannerData: SpaceInformation is invalid");
68  return;
69  }
70  try
71  {
72  boost::archive::binary_oarchive oa(out);
73 
74  // Writing the header
75  Header h;
76  h.marker = OMPL_PLANNER_DATA_ARCHIVE_MARKER;
77  h.vertex_count = pd.numVertices();
78  h.edge_count = pd.numEdges();
79  si->getStateSpace()->computeSignature(h.signature);
80  oa << h;
81 
82  storeVertices(pd, oa);
83  storeEdges(pd, oa);
84  }
85  catch (boost::archive::archive_exception &ae)
86  {
87  OMPL_ERROR("Failed to store PlannerData: %s", ae.what());
88  }
89 }
90 
91 void ompl::base::PlannerDataStorage::load(const char *filename, PlannerData &pd)
92 {
93  std::ifstream in(filename, std::ios::binary);
94  load(in, pd);
95  in.close();
96 }
97 
99 {
100  pd.clear();
101 
102  const SpaceInformationPtr &si = pd.getSpaceInformation();
103  if (!in.good())
104  {
105  OMPL_ERROR("Failed to load PlannerData: input stream is invalid");
106  return;
107  }
108  if (!si)
109  {
110  OMPL_ERROR("Failed to load PlannerData: SpaceInformation is invalid");
111  return;
112  }
113  // Loading the planner data:
114  try
115  {
116  boost::archive::binary_iarchive ia(in);
117 
118  // Read the header
119  Header h;
120  ia >> h;
121 
122  // Checking the archive marker
123  if (h.marker != OMPL_PLANNER_DATA_ARCHIVE_MARKER)
124  {
125  OMPL_ERROR("Failed to load PlannerData: PlannerData archive marker not found");
126  return;
127  }
128 
129  // Verify that the state space is the same
130  std::vector<int> sig;
131  si->getStateSpace()->computeSignature(sig);
132  if (h.signature != sig)
133  {
134  OMPL_ERROR("Failed to load PlannerData: StateSpace signature mismatch");
135  return;
136  }
137 
138  // File seems ok... loading vertices and edges
139  loadVertices(pd, h.vertex_count, ia);
140  loadEdges(pd, h.edge_count, ia);
141  }
142  catch (boost::archive::archive_exception &ae)
143  {
144  OMPL_ERROR("Failed to load PlannerData: %s", ae.what());
145  }
146 }
virtual ~PlannerDataStorage()
Destructor.
unsigned int numEdges() const
Retrieve the number of edges in this structure.
virtual void storeEdges(const PlannerData &pd, boost::archive::binary_oarchive &oa)
Serialize and store all edges in pd to the binary archive.
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
const SpaceInformationPtr & getSpaceInformation() const
Return the instance of SpaceInformation used in this PlannerData.
virtual void load(const char *filename, PlannerData &pd)
Load the PlannerData structure from the given stream. The StateSpace that was used to store the data ...
PlannerDataStorage()
Default constructor.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
virtual void loadVertices(PlannerData &pd, unsigned int numVertices, boost::archive::binary_iarchive &ia)
Read numVertices from the binary input ia and store them as PlannerData.
A boost shared pointer wrapper for ompl::base::SpaceInformation.
virtual void clear()
Clears the entire data structure.
Definition: PlannerData.cpp:78
Information stored at the beginning of the PlannerData archive.
virtual void storeVertices(const PlannerData &pd, boost::archive::binary_oarchive &oa)
Serialize and store all vertices in pd to the binary archive.
std::size_t edge_count
Number of edges stored in the archive.
std::size_t vertex_count
Number of vertices stored in the archive.
boost::uint32_t marker
OMPL PlannerData specific marker (fixed value)
std::vector< int > signature
Signature of state space that allocated the saved states in the vertices (see ompl::base::StateSpace:...
virtual void loadEdges(PlannerData &pd, unsigned int numEdges, boost::archive::binary_iarchive &ia)
Read numEdges from the binary input ia and store them as PlannerData.
virtual void store(const PlannerData &pd, const char *filename)
Store (serialize) the PlannerData structure to the given filename.