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  {
53  {
54  public:
58  virtual ~PlannerDataStorage();
59 
61  virtual void load(const char *filename, base::PlannerData &pd);
62 
64  virtual void load(std::istream &in, base::PlannerData &pd);
65 
69  virtual void store(const base::PlannerData &pd, const char *filename);
70 
74  virtual void store(const base::PlannerData &pd, std::ostream &out);
75 
76  protected:
77 
79  // Information stored at the beginning of the PlannerData archive
81  {
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  virtual void loadEdges(base::PlannerData &pd, unsigned int numEdges, boost::archive::binary_iarchive &ia)
111  {
112  OMPL_DEBUG("Loading %d PlannerDataEdgeControl objects", numEdges);
113 
114  const ControlSpacePtr& space = static_cast<control::PlannerData&>(pd).getSpaceInformation()->getControlSpace();
115  std::vector<Control*> controls;
116 
117  for (unsigned int i = 0; i < numEdges; ++i)
118  {
119  PlannerDataEdgeControlData edgeData;
120  ia >> edgeData;
121 
122  std::vector<unsigned char> ctrlBuf (space->getSerializationLength());
123  Control *ctrl = space->allocControl();
124  controls.push_back(ctrl);
125  space->deserialize(ctrl, &edgeData.control_[0]);
126  const_cast<PlannerDataEdgeControl*>(static_cast<const PlannerDataEdgeControl*>(edgeData.e_))->c_ = ctrl;
127 
128  pd.addEdge(edgeData.endpoints_.first, edgeData.endpoints_.second, *edgeData.e_, base::Cost(edgeData.weight_));
129 
130  // We deserialized the edge object pointer, and we own it.
131  // Since addEdge copies the object, it is safe to free here.
132  delete edgeData.e_;
133  }
134 
135  // These edges are using control pointers allocated here.
136  // To avoid a memory leak, we decouple planner data from the
137  // 'planner', which will clone all controls and properly free the
138  // memory when PlannerData goes out of scope. Then it is safe
139  // to free all memory allocated here.
140  pd.decoupleFromPlanner();
141 
142  for (size_t i = 0; i < controls.size(); ++i)
143  space->freeControl(controls[i]);
144  }
145 
148  virtual void storeEdges(const base::PlannerData &pd, boost::archive::binary_oarchive &oa)
149  {
150  OMPL_DEBUG("Storing %d PlannerDataEdgeControl objects", pd.numEdges());
151 
152  const ControlSpacePtr& space = static_cast<const control::PlannerData&>(pd).getSpaceInformation()->getControlSpace();
153  std::vector<unsigned char> ctrl (space->getSerializationLength());
154 
155  for (unsigned int i = 0; i < pd.numVertices(); ++i)
156  for (unsigned int j = 0; j < pd.numVertices(); ++j)
157  {
158  if(pd.edgeExists(i, j))
159  {
160  PlannerDataEdgeControlData edgeData;
161  edgeData.e_ = &pd.getEdge(i, j);
162  edgeData.endpoints_.first = i;
163  edgeData.endpoints_.second = j;
164  base::Cost weight;
165  pd.getEdgeWeight(i, j, &weight);
166  edgeData.weight_ = weight.value();
167 
168  space->serialize(&ctrl[0], static_cast<const PlannerDataEdgeControl*>(edgeData.e_)->getControl());
169  edgeData.control_ = ctrl;
170 
171  oa << edgeData;
172  }
173  }
174  }
175 
176  };
177  }
178 }
179 
180 #endif
Object that handles loading/storing a PlannerData object to/from a binary stream. Serialization of ve...
virtual void storeEdges(const base::PlannerData &pd, boost::archive::binary_oarchive &oa)
Serialize and store all edges in pd to the binary archive. It is assumed that the edges can be cast t...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
The object containing all edge data that will be stored.
virtual void decoupleFromPlanner()
Creates a deep copy of the states contained in the vertices of this PlannerData structure so that whe...
Definition: PlannerData.cpp:84
Object that handles loading/storing a PlannerData object to/from a binary stream. Serialization of ve...
Definition of an abstract control.
Definition: Control.h:48
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:111
virtual void store(const base::PlannerData &pd, const char *filename)
Store (serialize) the structure to the given filename. The StateSpace and ControlSpace that was used ...
virtual ~PlannerDataStorage()
Destructor.
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Definition: PlannerData.h:60
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...
A boost shared pointer wrapper for ompl::control::ControlSpace.
Main namespace. Contains everything in this library.
Definition: Cost.h:42
virtual void loadEdges(base::PlannerData &pd, unsigned int numEdges, boost::archive::binary_iarchive &ia)
Read numEdges from the binary input ia and store them as PlannerData. It is assumed that the edges ca...
void serialize(Archive &ar, const unsigned int)
boost::serialization routine
unsigned int numEdges() const
Retrieve the number of edges in this structure.
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
double value() const
The value of the cost.
Definition: Cost.h:54
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
Information stored at the beginning of the PlannerData archive.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
bool edgeExists(unsigned int v1, unsigned int v2) const
Check whether an edge between vertex index v1 and index v2 exists.
const PlannerDataEdge & getEdge(unsigned int v1, unsigned int v2) const
Retrieve a reference to the edge object connecting vertices with indexes v1 and v2. If this edge does not exist, NO_EDGE is returned.
PlannerDataStorage()
Default constructor.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
virtual void load(const char *filename, base::PlannerData &pd)
Load the PlannerData structure from the given filename.