Loading...
Searching...
No Matches
StateStorage.h
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2012, Willow Garage
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 Willow Garage 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: Ioan Sucan */
36
37#ifndef OMPL_BASE_STATE_STORAGE_
38#define OMPL_BASE_STATE_STORAGE_
39
40#include "ompl/base/StateSpace.h"
41#include <boost/archive/binary_oarchive.hpp>
42#include <boost/archive/binary_iarchive.hpp>
43#include <boost/serialization/vector.hpp>
44#include <functional>
45#include <iostream>
46
47namespace ompl
48{
49 namespace base
50 {
52
53 OMPL_CLASS_FORWARD(StateStorage);
55
62 {
63 public:
66 virtual ~StateStorage();
67
70 {
71 return space_;
72 }
73
75 void load(const char *filename);
76
78 virtual void load(std::istream &in);
79
81 void store(const char *filename);
82
84 virtual void store(std::ostream &out);
85
88 virtual void addState(const State *state);
89
91 virtual void generateSamples(unsigned int count);
92
94 virtual void clear();
95
97 std::size_t size() const
98 {
99 return states_.size();
100 }
101
103 const std::vector<const State *> &getStates() const
104 {
105 return states_;
106 }
107
109 State *getState(unsigned int index)
110 {
111 assert(states_.size() > index);
112 return const_cast<State *>(states_[index]);
113 }
114
116 const State *getState(unsigned int index) const
117 {
118 assert(states_.size() > index);
119 return states_[index];
120 }
121
124 bool hasMetadata() const
125 {
126 return hasMetadata_;
127 }
128
131 void sort(const std::function<bool(const State *, const State *)> &op);
132
137
142
147
151 virtual StateSamplerAllocator getStateSamplerAllocatorRange(std::size_t from, std::size_t to) const;
152
154 virtual void print(std::ostream &out = std::cout) const;
155
156 protected:
158 struct Header
159 {
161 std::uint_fast32_t marker;
162
164 std::size_t state_count;
165
168 std::vector<int> signature;
169
171 template <typename Archive>
172 void serialize(Archive &ar, const unsigned int /*version*/)
173 {
174 ar &marker;
175 ar &state_count;
176 ar &signature;
177 }
178 };
179
181 virtual void loadStates(const Header &h, boost::archive::binary_iarchive &ia);
182
187 virtual void loadMetadata(const Header &h, boost::archive::binary_iarchive &ia);
188
190 virtual void storeStates(const Header &h, boost::archive::binary_oarchive &oa);
191
196 virtual void storeMetadata(const Header &h, boost::archive::binary_oarchive &oa);
197
199 void freeMemory();
200
203
205 std::vector<const State *> states_;
206
209 };
210
213 template <typename M>
215 {
216 public:
218 using MetadataType = M;
219
222 {
223 hasMetadata_ = true;
224 }
225
229 void addState(const State *state) override
230 {
231 addState(state, M());
232 }
233
237 virtual void addState(const State *state, const M &metadata)
238 {
240 metadata_.push_back(metadata);
241 }
242
243 void clear() override
244 {
246 metadata_.clear();
247 }
248
250 const M &getMetadata(unsigned int index) const
251 {
252 assert(metadata_.size() > index);
253 return metadata_[index];
254 }
255
257 M &getMetadata(unsigned int index)
258 {
259 assert(metadata_.size() > index);
260 return metadata_[index];
261 }
262
263 protected:
264 void loadMetadata(const Header & /*h*/, boost::archive::binary_iarchive &ia) override
265 {
266 // clear default metadata that was added by StateStorage::loadStates()
267 metadata_.clear();
268 ia >> metadata_;
269 }
270
271 void storeMetadata(const Header & /*h*/, boost::archive::binary_oarchive &oa) override
272 {
273 oa << metadata_;
274 }
275
277 std::vector<M> metadata_;
278 };
279
283 using GraphStateStoragePtr = std::shared_ptr<GraphStateStorage>;
284 }
285}
286#endif
A shared pointer wrapper for ompl::base::StateSpace.
State storage that allows storing state metadata as well.
void addState(const State *state) override
Add a state to the set of states maintained by this storage structure. The state is copied to interna...
M & getMetadata(unsigned int index)
Get write access to the metadata of a state at a particular index.
const M & getMetadata(unsigned int index) const
Get const access to the metadata of a state at a particular index.
M MetadataType
the datatype of the metadata
std::vector< M > metadata_
The metadata for each state.
StateStorageWithMetadata(const StateSpacePtr &space)
The state space to store states for is specified as argument.
void loadMetadata(const Header &, boost::archive::binary_iarchive &ia) override
Load the state metadata from a binary archive ia, given the loaded header is h. No metadata is actual...
virtual void addState(const State *state, const M &metadata)
Add a state to the set of states maintained by this storage structure. The state is copied to interna...
void clear() override
Clear the stored states. This frees all the memory.
void storeMetadata(const Header &, boost::archive::binary_oarchive &oa) override
Save the state metadata to a binary archive oa, given the stored header is h. No metadata is actually...
Manage loading and storing for a set of states of a specified state space.
StateSamplerAllocator getStateSamplerAllocator() const
Get a sampler allocator to a sampler that can be specified for a StateSpace, such that all sampled st...
std::size_t size() const
Return the number of stored states.
virtual void generateSamples(unsigned int count)
Generate count states uniformly at random and store them in this structure.
bool hasMetadata_
Flag indicating whether there is metadata associated to the states in this storage.
virtual void print(std::ostream &out=std::cout) const
Output the set of states to a specified stream, in a human readable fashion.
virtual void loadStates(const Header &h, boost::archive::binary_iarchive &ia)
Load the states from a binary archive ia, given the loaded header is h.
const StateSpacePtr & getStateSpace() const
Get the state space this class maintains states for.
StateSamplerAllocator getStateSamplerAllocatorRangeAfter(std::size_t after) const
Get a sampler allocator to a sampler that can be specified for a StateSpace, such that all sampled st...
void load(const char *filename)
Load a set of states from a specified file.
bool hasMetadata() const
Return a flag that indicates whether there is metadata associated to the states in this storage.
virtual void storeStates(const Header &h, boost::archive::binary_oarchive &oa)
Store the states to a binary archive oa, given the stored header is h.
const std::vector< const State * > & getStates() const
Get the stored states.
StateSamplerAllocator getStateSamplerAllocatorRangeUntil(std::size_t until) const
Get a sampler allocator to a sampler that can be specified for a StateSpace, such that all sampled st...
const State * getState(unsigned int index) const
Get a particular state.
virtual void addState(const State *state)
Add a state to the set of states maintained by this storage structure. The state is copied to interna...
virtual void clear()
Clear the stored states. This frees all the memory.
StateSpacePtr space_
State space that corresponds to maintained states.
virtual void storeMetadata(const Header &h, boost::archive::binary_oarchive &oa)
Save the state metadata to a binary archive oa, given the stored header is h. No metadata is actually...
virtual void loadMetadata(const Header &h, boost::archive::binary_iarchive &ia)
Load the state metadata from a binary archive ia, given the loaded header is h. No metadata is actual...
State * getState(unsigned int index)
Get a particular state for non-const access.
StateStorage(StateSpacePtr space)
The state space to store states for is specified as argument.
void store(const char *filename)
Save a set of states to a file.
virtual StateSamplerAllocator getStateSamplerAllocatorRange(std::size_t from, std::size_t to) const
Get a sampler allocator to a sampler that can be specified for a StateSpace, such that all sampled st...
void freeMemory()
Free the memory allocated for states.
std::vector< const State * > states_
The list of maintained states.
void sort(const std::function< bool(const State *, const State *)> &op)
Sort the states according to the less-equal operator op. Metadata is NOT sorted; if metadata was adde...
Definition of an abstract state.
Definition State.h:50
std::function< StateSamplerPtr(const StateSpace *)> StateSamplerAllocator
Definition of a function that can allocate a state sampler.
Main namespace. Contains everything in this library.
Information stored at the beginning of the archive.
std::size_t state_count
Number of states stored in the archive.
std::vector< int > signature
Signature of state space that allocated the saved states (see ompl::base::StateSpace::computeSignatur...
void serialize(Archive &ar, const unsigned int)
boost::serialization routine
std::uint_fast32_t marker
OMPL specific marker (fixed value)