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/control/PlannerDataStorage.h"
38 #include <boost/archive/archive_exception.hpp>
39 
41 static const boost::uint32_t OMPL_PLANNER_DATA_CONTROL_ARCHIVE_MARKER = 0x5044434D; // this spells PDCM
43 
45 {
46  base::PlannerDataStorage::load(filename, pd);
47 }
48 
50 {
51  if (!pd.hasControls())
52  {
53  OMPL_WARN("PlannerData does not have controls. Invoking base::PlannerDataStorage::load");
55  return;
56  }
57 
58  auto *pdc = static_cast<control::PlannerData *>(&pd);
59  pdc->clear();
60 
61  const SpaceInformationPtr &si = pdc->getSpaceInformation();
62  if (!in.good())
63  {
64  OMPL_ERROR("Failed to load PlannerData: input stream is invalid");
65  return;
66  }
67  if (!si)
68  {
69  OMPL_ERROR("Failed to load PlannerData: SpaceInformation is invalid");
70  return;
71  }
72  // Loading the planner data:
73  try
74  {
75  boost::archive::binary_iarchive ia(in);
76 
77  // Read the header
78  Header h;
79  ia >> h;
80 
81  // Checking the archive marker
82  if (h.marker != OMPL_PLANNER_DATA_CONTROL_ARCHIVE_MARKER)
83  {
84  OMPL_ERROR("Failed to load PlannerData: PlannerData control archive marker not found");
85  return;
86  }
87 
88  // Verify that the state space is the same
89  std::vector<int> sig;
90  si->getStateSpace()->computeSignature(sig);
91  if (h.signature != sig)
92  {
93  OMPL_ERROR("Failed to load PlannerData: StateSpace signature mismatch");
94  return;
95  }
96 
97  // Verify that the control space is the same
98  sig.clear();
99  si->getControlSpace()->computeSignature(sig);
100  if (h.control_signature != sig)
101  {
102  OMPL_ERROR("Failed to load PlannerData: ControlSpace signature mismatch");
103  return;
104  }
105 
106  // File seems ok... loading vertices and edges
107  loadVertices(pd, h.vertex_count, ia);
108  loadEdges(pd, h.edge_count, ia);
109  }
110  catch (boost::archive::archive_exception &ae)
111  {
112  OMPL_ERROR("Failed to load PlannerData: %s", ae.what());
113  }
114 }
115 
116 void ompl::control::PlannerDataStorage::store(const base::PlannerData &pd, const char *filename)
117 {
118  base::PlannerDataStorage::store(pd, filename);
119 }
120 
122 {
123  const auto *pdc = static_cast<const control::PlannerData *>(&pd);
124  if (pdc == nullptr)
125  {
126  OMPL_WARN("Failed to cast PlannerData to control::PlannerData. Invoking base::PlannerDataStorage::store");
128  return;
129  }
130 
131  const SpaceInformationPtr &si = pdc->getSpaceInformation();
132  if (!out.good())
133  {
134  OMPL_ERROR("Failed to store PlannerData: output stream is invalid");
135  return;
136  }
137  if (!si)
138  {
139  OMPL_ERROR("Failed to store PlannerData: SpaceInformation is invalid");
140  return;
141  }
142  try
143  {
144  boost::archive::binary_oarchive oa(out);
145 
146  // Writing the header
147  Header h;
148  h.marker = OMPL_PLANNER_DATA_CONTROL_ARCHIVE_MARKER;
149  h.vertex_count = pdc->numVertices();
150  h.edge_count = pdc->numEdges();
151  si->getStateSpace()->computeSignature(h.signature);
152  si->getControlSpace()->computeSignature(h.control_signature);
153  oa << h;
154 
155  storeVertices(pd, oa);
156  storeEdges(pd, oa);
157  }
158  catch (boost::archive::archive_exception &ae)
159  {
160  OMPL_ERROR("Failed to store PlannerData: %s", ae.what());
161  }
162 }
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:121
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 ...
void loadEdges(base::PlannerData &pd, unsigned int numEdges, boost::archive::binary_iarchive &ia) override
Read numEdges from the binary input ia and store them as PlannerData. It is assumed that the edges ca...
#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.
void load(const char *filename, base::PlannerData &pd) override
Load the PlannerData structure from the given filename.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
A shared pointer wrapper for ompl::control::SpaceInformation.
Information stored at the beginning of the PlannerData archive.
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance...
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.
void clear() override
Clears the entire data structure.
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:...
void store(const base::PlannerData &pd, const char *filename) override
Store (serialize) the structure to the given filename. The StateSpace and ControlSpace that was used ...
virtual void store(const PlannerData &pd, const char *filename)
Store (serialize) the PlannerData structure to the given filename.
void storeEdges(const base::PlannerData &pd, boost::archive::binary_oarchive &oa) override
Serialize and store all edges in pd to the binary archive. It is assumed that the edges can be cast t...