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 #include "ompl/base/PlannerDataStorage.h"
41 #include "ompl/control/PlannerData.h"
42 #include "ompl/control/SpaceInformation.h"
43 
44 namespace ompl
45 {
46  namespace control
47  {
52  class PlannerDataStorage : public base::PlannerDataStorage
53  {
54  public:
56  PlannerDataStorage() = default;
58  ~PlannerDataStorage() override = default;
59 
61  void load(const char *filename, base::PlannerData &pd) override;
62 
64  void load(std::istream &in, base::PlannerData &pd) override;
65 
69  void store(const base::PlannerData &pd, const char *filename) override;
70 
74  void store(const base::PlannerData &pd, std::ostream &out) override;
75 
76  protected:
78  // Information stored at the beginning of the PlannerData archive
79  struct Header : base::PlannerDataStorage::Header
80  {
83  std::vector<int> control_signature;
84 
86  template <typename Archive>
87  void serialize(Archive &ar, const unsigned int /*version*/)
88  {
89  ar &boost::serialization::base_object<base::PlannerDataStorage::Header>(*this);
90  ar &control_signature;
91  }
92  };
93 
94  // The object containing all control edge data that will be stored
95  struct PlannerDataEdgeControlData : base::PlannerDataStorage::PlannerDataEdgeData
96  {
97  template <typename Archive>
98  void serialize(Archive &ar, const unsigned int /*version*/)
99  {
100  ar &boost::serialization::base_object<base::PlannerDataStorage::PlannerDataEdgeData>(*this);
101  ar &control_;
102  }
103 
104  std::vector<unsigned char> control_;
105  };
107 
110  void loadEdges(base::PlannerData &pd, unsigned int numEdges, boost::archive::binary_iarchive &ia) override
111  {
112  OMPL_DEBUG("Loading %d PlannerDataEdgeControl objects", numEdges);
113 
114  const ControlSpacePtr &space =
115  static_cast<control::PlannerData &>(pd).getSpaceInformation()->getControlSpace();
116  std::vector<Control *> controls;
117 
118  for (unsigned int i = 0; i < numEdges; ++i)
119  {
120  PlannerDataEdgeControlData edgeData;
121  ia >> edgeData;
122 
123  std::vector<unsigned char> ctrlBuf(space->getSerializationLength());
124  Control *ctrl = space->allocControl();
125  controls.push_back(ctrl);
126  space->deserialize(ctrl, &edgeData.control_[0]);
127  const_cast<PlannerDataEdgeControl *>(static_cast<const PlannerDataEdgeControl *>(edgeData.e_))->c_ =
128  ctrl;
129 
130  pd.addEdge(edgeData.endpoints_.first, edgeData.endpoints_.second, *edgeData.e_,
131  base::Cost(edgeData.weight_));
132 
133  // We deserialized the edge object pointer, and we own it.
134  // Since addEdge copies the object, it is safe to free here.
135  delete edgeData.e_;
136  }
137 
138  // These edges are using control pointers allocated here.
139  // To avoid a memory leak, we decouple planner data from the
140  // 'planner', which will clone all controls and properly free the
141  // memory when PlannerData goes out of scope. Then it is safe
142  // to free all memory allocated here.
143  pd.decoupleFromPlanner();
144 
145  for (auto &control : controls)
146  space->freeControl(control);
147  }
148 
151  void storeEdges(const base::PlannerData &pd, boost::archive::binary_oarchive &oa) override
152  {
153  OMPL_DEBUG("Storing %d PlannerDataEdgeControl objects", pd.numEdges());
154 
155  const ControlSpacePtr &space =
156  static_cast<const control::PlannerData &>(pd).getSpaceInformation()->getControlSpace();
157  std::vector<unsigned char> ctrl(space->getSerializationLength());
158 
159  std::vector<unsigned int> edgeList;
160  for (unsigned int fromVertex = 0; fromVertex < pd.numVertices(); ++fromVertex)
161  {
162  edgeList.clear();
163  pd.getEdges(fromVertex, edgeList); // returns the id of each edge
164 
165  // Process edges
166  for (unsigned int toVertex : edgeList)
167  {
168  // Get cost
169  base::Cost weight;
170  if (!pd.getEdgeWeight(fromVertex, toVertex, &weight))
171  OMPL_ERROR("Unable to get edge weight");
172 
173  // Convert to new structure
174  PlannerDataEdgeControlData edgeData;
175  edgeData.e_ = &pd.getEdge(fromVertex, toVertex);
176  edgeData.endpoints_.first = fromVertex;
177  edgeData.endpoints_.second = toVertex;
178  edgeData.weight_ = weight.value();
179  space->serialize(&ctrl[0],
180  static_cast<const PlannerDataEdgeControl *>(edgeData.e_)->getControl());
181  edgeData.control_ = ctrl;
182  oa << edgeData;
183 
184  } // for each edge
185  } // for each vertex
186  }
187  };
188  }
189 }
190 
191 #endif
bool getEdgeWeight(unsigned int v1, unsigned int v2, Cost *weight) const
Returns the weight of the edge between the given vertex indices. If there exists an edge between v1 a...
Definition of an abstract control.
Definition: Control.h:111
unsigned int getEdges(unsigned int v, std::vector< unsigned int > &edgeList) const
Returns a list of the vertex indexes directly connected to vertex with index v (outgoing edges)....
PlannerDataStorage()=default
Default constructor.
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Definition: PlannerData.h:124
A shared pointer wrapper for ompl::control::ControlSpace.
void serialize(Archive &ar, const unsigned int)
boost::serialization routine
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 ...
double value() const
The value of the cost.
Definition: Cost.h:152
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
unsigned int numEdges() const
Retrieve the number of edges in this structure.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
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...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:185
void load(const char *filename, base::PlannerData &pd) override
Load the PlannerData structure from the given filename.
~PlannerDataStorage() override=default
Destructor.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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...
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
const PlannerDataEdge & getEdge(unsigned int v1, unsigned int v2) const
Retrieve a reference to the edge object connecting vertices with indexes v1 and v2....