Loading...
Searching...
No Matches
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
44namespace ompl
45{
46 namespace control
47 {
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
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.
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
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
double value() const
The value of the cost.
Definition: Cost.h:56
Object that handles loading/storing a PlannerData object to/from a binary stream. Serialization of ve...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
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...
unsigned int numEdges() const
Retrieve the number of edges in this structure.
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
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)....
const PlannerDataEdge & getEdge(unsigned int v1, unsigned int v2) const
Retrieve a reference to the edge object connecting vertices with indexes v1 and v2....
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...
virtual void decoupleFromPlanner()
Creates a deep copy of the states contained in the vertices of this PlannerData structure so that whe...
Definition: PlannerData.cpp:80
A shared pointer wrapper for ompl::control::ControlSpace.
Definition of an abstract control.
Definition: Control.h:48
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Definition: PlannerData.h:61
const Control * getControl() const
Return the control associated with this edge.
Definition: PlannerData.h:81
Object that handles loading/storing a PlannerData object to/from a binary stream. Serialization of ve...
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 ...
~PlannerDataStorage() override=default
Destructor.
void load(const char *filename, base::PlannerData &pd) override
Load the PlannerData structure from the given filename.
PlannerDataStorage()=default
Default constructor.
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...
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:122
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
Main namespace. Contains everything in this library.
Information stored at the beginning of the PlannerData archive.
void serialize(Archive &ar, const unsigned int)
boost::serialization routine
The object containing all edge data that will be stored.