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  OMPL_DEBUG("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_, base::Cost(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  OMPL_DEBUG("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  base::Cost weight;
171  pd.getEdgeWeight(i, j, &weight);
172  edgeData.weight_ = weight.v;
173 
174  space->serialize(&ctrl[0], static_cast<const PlannerDataEdgeControl*>(edgeData.e_)->getControl());
175  edgeData.control_ = ctrl;
176 
177  oa << edgeData;
178  }
179  }
180  }
181 
182  };
183  }
184 }
185 
186 #endif
187 
188 #endif
Object that handles loading/storing a PlannerData object to/from a binary stream. Serialization of ve...
unsigned int numVertices(void) const
Retrieve the number of vertices in this structure.
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.
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
PlannerDataStorage(void)
Default constructor.
double v
The value of the cost.
Definition: Cost.h:52
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 ...
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Definition: PlannerData.h:60
virtual ~PlannerDataStorage(void)
Destructor.
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.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
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
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.
virtual void decoupleFromPlanner(void)
Creates a deep copy of the states contained in the vertices of this PlannerData structure so that whe...
Definition: PlannerData.cpp:84
unsigned int numEdges(void) const
Retrieve the number of edges in this structure.
virtual void load(const char *filename, base::PlannerData &pd)
Load the PlannerData structure from the given filename.