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 
47 namespace 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  {
239  StateStorage::addState(state);
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.
Definition: StateStorage.h:215
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...
Definition: StateStorage.h:229
M & getMetadata(unsigned int index)
Get write access to the metadata of a state at a particular index.
Definition: StateStorage.h:257
M MetadataType
the datatype of the metadata
Definition: StateStorage.h:218
std::vector< M > metadata_
The metadata for each state.
Definition: StateStorage.h:277
StateStorageWithMetadata(const StateSpacePtr &space)
The state space to store states for is specified as argument.
Definition: StateStorage.h:221
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...
Definition: StateStorage.h:264
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...
Definition: StateStorage.h:237
const M & getMetadata(unsigned int index) const
Get const access to the metadata of a state at a particular index.
Definition: StateStorage.h:250
void clear() override
Clear the stored states. This frees all the memory.
Definition: StateStorage.h:243
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...
Definition: StateStorage.h:271
Manage loading and storing for a set of states of a specified state space.
Definition: StateStorage.h:62
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.
Definition: StateStorage.h:97
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.
Definition: StateStorage.h:208
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.
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.
Definition: StateStorage.h:124
const State * getState(unsigned int index) const
Get a particular state.
Definition: StateStorage.h:116
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.
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...
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.
Definition: StateStorage.h:202
State * getState(unsigned int index)
Get a particular state for non-const access.
Definition: StateStorage.h:109
const std::vector< const State * > & getStates() const
Get the stored states.
Definition: StateStorage.h:103
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...
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.
Definition: StateStorage.h:205
const StateSpacePtr & getStateSpace() const
Get the state space this class maintains states for.
Definition: StateStorage.h:69
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.
Definition: StateSampler.h:191
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
Information stored at the beginning of the archive.
Definition: StateStorage.h:159
std::size_t state_count
Number of states stored in the archive.
Definition: StateStorage.h:164
std::vector< int > signature
Signature of state space that allocated the saved states (see ompl::base::StateSpace::computeSignatur...
Definition: StateStorage.h:168
void serialize(Archive &ar, const unsigned int)
boost::serialization routine
Definition: StateStorage.h:172
std::uint_fast32_t marker
OMPL specific marker (fixed value)
Definition: StateStorage.h:161