All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
PlannerDataStorage.h
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 #ifndef OMPL_CONTROL_PLANNER_DATA_STORAGE_
38 #define OMPL_CONTROL_PLANNER_DATA_STORAGE_
39 
40 // PlannerDataStorage requires Boost version >= 1.44
41 #include <boost/version.hpp>
42 #if BOOST_VERSION < 104400
43 #warning Boost version >= 1.44 is required for PlannerDataStorage classes
44 #else
45 
46 #include "ompl/base/PlannerDataStorage.h"
47 #include "ompl/control/PlannerData.h"
48 #include "ompl/control/SpaceInformation.h"
49 
50 namespace ompl
51 {
52  namespace control
53  {
59  {
60  public:
62  PlannerDataStorage(void);
64  virtual ~PlannerDataStorage(void);
65 
67  virtual void load(const char *filename, base::PlannerData& pd);
68 
70  virtual void load(std::istream &in, base::PlannerData& pd);
71 
75  virtual void store(const base::PlannerData& pd, const char *filename);
76 
80  virtual void store(const base::PlannerData& pd, std::ostream &out);
81 
82  protected:
83 
85  // Information stored at the beginning of the PlannerData archive
87  {
89  std::vector<int> control_signature;
90 
92  template<typename Archive>
93  void serialize(Archive & ar, const unsigned int version)
94  {
95  ar & boost::serialization::base_object<base::PlannerDataStorage::Header>(*this);
96  ar & control_signature;
97  }
98  };
99 
100  // The object containing all control edge data that will be stored
101  struct PlannerDataEdgeControlData : base::PlannerDataStorage::PlannerDataEdgeData
102  {
103  template<typename Archive>
104  void serialize(Archive & ar, const unsigned int version)
105  {
106  ar & boost::serialization::base_object<base::PlannerDataStorage::PlannerDataEdgeData>(*this);
107  ar & control_;
108  }
109 
110  std::vector<unsigned char> control_;
111  };
113 
116  virtual void loadEdges(base::PlannerData &pd, unsigned int numEdges, boost::archive::binary_iarchive &ia)
117  {
118  logDebug("Loading %d PlannerDataEdgeControl objects", numEdges);
119 
120  const ControlSpacePtr& space = static_cast<control::PlannerData&>(pd).getSpaceInformation()->getControlSpace();
121  std::vector<Control*> controls;
122 
123  for (unsigned int i = 0; i < numEdges; ++i)
124  {
125  PlannerDataEdgeControlData edgeData;
126  ia >> edgeData;
127 
128  std::vector<unsigned char> ctrlBuf (space->getSerializationLength());
129  Control *ctrl = space->allocControl();
130  controls.push_back(ctrl);
131  space->deserialize(ctrl, &edgeData.control_[0]);
132  const_cast<PlannerDataEdgeControl*>(static_cast<const PlannerDataEdgeControl*>(edgeData.e_))->c_ = ctrl;
133 
134  pd.addEdge(edgeData.endpoints_.first, edgeData.endpoints_.second, *edgeData.e_, edgeData.weight_);
135 
136  // We deserialized the edge object pointer, and we own it.
137  // Since addEdge copies the object, it is safe to free here.
138  delete edgeData.e_;
139  }
140 
141  // These edges are using control pointers allocated here.
142  // To avoid a memory leak, we decouple planner data from the
143  // 'planner', which will clone all controls and properly free the
144  // memory when PlannerData goes out of scope. Then it is safe
145  // to free all memory allocated here.
146  pd.decoupleFromPlanner();
147 
148  for (size_t i = 0; i < controls.size(); ++i)
149  space->freeControl(controls[i]);
150  }
151 
154  virtual void storeEdges(const base::PlannerData &pd, boost::archive::binary_oarchive &oa)
155  {
156  logDebug("Storing %d PlannerDataEdgeControl objects", pd.numEdges());
157 
158  const ControlSpacePtr& space = static_cast<const control::PlannerData&>(pd).getSpaceInformation()->getControlSpace();
159  std::vector<unsigned char> ctrl (space->getSerializationLength());
160 
161  for (unsigned int i = 0; i < pd.numVertices(); ++i)
162  for (unsigned int j = 0; j < pd.numVertices(); ++j)
163  {
164  if(pd.edgeExists(i, j))
165  {
166  PlannerDataEdgeControlData edgeData;
167  edgeData.e_ = &pd.getEdge(i, j);
168  edgeData.endpoints_.first = i;
169  edgeData.endpoints_.second = j;
170  edgeData.weight_ = pd.getEdgeWeight(i, j);
171 
172  space->serialize(&ctrl[0], static_cast<const PlannerDataEdgeControl*>(edgeData.e_)->getControl());
173  edgeData.control_ = ctrl;
174 
175  oa << edgeData;
176  }
177  }
178  }
179 
180  };
181  }
182 }
183 
184 #endif
185 
186 #endif