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 { 51 /// @cond IGNORE 52 /** \brief Forward declaration of ompl::base::StateStorage */ 53 OMPL_CLASS_FORWARD(StateStorage); 54 /// @endcond 55 56 /** \brief Manage loading and storing for a set of states of a specified state space 57 58 \deprecated This class is deprecated and will be removed in the future. Please use the improved 59 PlannerDataStorage. 60 */ 61 class StateStorage 62 { 63 public: 64 /** \brief The state space to store states for is specified as argument */ 65 StateStorage(StateSpacePtr space); 66 virtual ~StateStorage(); 67 68 /** \brief Get the state space this class maintains states for */ getStateSpace()69 const StateSpacePtr &getStateSpace() const 70 { 71 return space_; 72 } 73 74 /** \brief Load a set of states from a specified file */ 75 void load(const char *filename); 76 77 /** \brief Load a set of states from a stream */ 78 virtual void load(std::istream &in); 79 80 /** \brief Save a set of states to a file */ 81 void store(const char *filename); 82 83 /** \brief Save a set of states to a stream */ 84 virtual void store(std::ostream &out); 85 86 /** \brief Add a state to the set of states maintained by 87 this storage structure. The state is copied to internal storage */ 88 virtual void addState(const State *state); 89 90 /** \brief Generate \e count states uniformly at random and store them in this structure */ 91 virtual void generateSamples(unsigned int count); 92 93 /** \brief Clear the stored states. This frees all the memory */ 94 virtual void clear(); 95 96 /** \brief Return the number of stored states */ size()97 std::size_t size() const 98 { 99 return states_.size(); 100 } 101 102 /** \brief Get the stored states */ getStates()103 const std::vector<const State *> &getStates() const 104 { 105 return states_; 106 } 107 108 /** \brief Get a particular state for non-const access */ getState(unsigned int index)109 State *getState(unsigned int index) 110 { 111 assert(states_.size() > index); 112 return const_cast<State *>(states_[index]); 113 } 114 115 /** \brief Get a particular state */ getState(unsigned int index)116 const State *getState(unsigned int index) const 117 { 118 assert(states_.size() > index); 119 return states_[index]; 120 } 121 122 /** \brief Return a flag that indicates whether there is metadata associated to the states in this storage 123 */ hasMetadata()124 bool hasMetadata() const 125 { 126 return hasMetadata_; 127 } 128 129 /** \brief Sort the states according to the less-equal operator \e op. Metadata is NOT sorted; 130 if metadata was added, the index values of the metadata will not match after the sort. */ 131 void sort(const std::function<bool(const State *, const State *)> &op); 132 133 /** \brief Get a sampler allocator to a sampler that can be specified for a StateSpace, such that all 134 sampled 135 states are actually from this storage structure. */ 136 StateSamplerAllocator getStateSamplerAllocator() const; 137 138 /** \brief Get a sampler allocator to a sampler that can be specified for a StateSpace, such that all 139 sampled 140 states are actually from this storage structure at an index less than or equal to \e until */ 141 StateSamplerAllocator getStateSamplerAllocatorRangeUntil(std::size_t until) const; 142 143 /** \brief Get a sampler allocator to a sampler that can be specified for a StateSpace, such that all 144 sampled 145 states are actually from this storage structure at an index above or equal to \e after */ 146 StateSamplerAllocator getStateSamplerAllocatorRangeAfter(std::size_t after) const; 147 148 /** \brief Get a sampler allocator to a sampler that can be specified for a StateSpace, such that all 149 sampled 150 states are actually from this storage structure at an index in the range [\e from, \e to] (inclusive) */ 151 virtual StateSamplerAllocator getStateSamplerAllocatorRange(std::size_t from, std::size_t to) const; 152 153 /** \brief Output the set of states to a specified stream, in a human readable fashion */ 154 virtual void print(std::ostream &out = std::cout) const; 155 156 protected: 157 /** \brief Information stored at the beginning of the archive */ 158 struct Header 159 { 160 /** \brief OMPL specific marker (fixed value) */ 161 std::uint_fast32_t marker; 162 163 /** \brief Number of states stored in the archive */ 164 std::size_t state_count; 165 166 /** \brief Signature of state space that allocated the saved states (see 167 * ompl::base::StateSpace::computeSignature()) */ 168 std::vector<int> signature; 169 170 /** \brief boost::serialization routine */ 171 template <typename Archive> serializeHeader172 void serialize(Archive &ar, const unsigned int /*version*/) 173 { 174 ar ▮ 175 ar &state_count; 176 ar &signature; 177 } 178 }; 179 180 /** \brief Load the states from a binary archive \e ia, given the loaded header is \e h */ 181 virtual void loadStates(const Header &h, boost::archive::binary_iarchive &ia); 182 183 /** \brief Load the state metadata from a binary archive 184 \e ia, given the loaded header is \e h. No metadata is 185 actually loaded unless the StateStorageWithMetadata 186 class is used.*/ 187 virtual void loadMetadata(const Header &h, boost::archive::binary_iarchive &ia); 188 189 /** \brief Store the states to a binary archive \e oa, given the stored header is \e h */ 190 virtual void storeStates(const Header &h, boost::archive::binary_oarchive &oa); 191 192 /** \brief Save the state metadata to a binary archive 193 \e oa, given the stored header is \e h. No metadata is 194 actually saved unless the StateStorageWithMetadata 195 class is used.*/ 196 virtual void storeMetadata(const Header &h, boost::archive::binary_oarchive &oa); 197 198 /** \brief Free the memory allocated for states */ 199 void freeMemory(); 200 201 /** \brief State space that corresponds to maintained states */ 202 StateSpacePtr space_; 203 204 /** \brief The list of maintained states */ 205 std::vector<const State *> states_; 206 207 /** \brief Flag indicating whether there is metadata associated to the states in this storage */ 208 bool hasMetadata_; 209 }; 210 211 /** \brief State storage that allows storing state metadata as well 212 \tparam M the datatype for the stored metadata. boost::serialization operation needs to be defined */ 213 template <typename M> 214 class StateStorageWithMetadata : public StateStorage 215 { 216 public: 217 /** \brief the datatype of the metadata */ 218 using MetadataType = M; 219 220 /** \brief The state space to store states for is specified as argument */ StateStorageWithMetadata(const StateSpacePtr & space)221 StateStorageWithMetadata(const StateSpacePtr &space) : StateStorage(space) 222 { 223 hasMetadata_ = true; 224 } 225 226 /** \brief Add a state to the set of states maintained by 227 this storage structure. The state is copied to 228 internal storage and metadata with default values is stored as well. */ addState(const State * state)229 void addState(const State *state) override 230 { 231 addState(state, M()); 232 } 233 234 /** \brief Add a state to the set of states maintained by 235 this storage structure. The state is copied to internal storage. Corresponding metadata is stored too. 236 */ addState(const State * state,const M & metadata)237 virtual void addState(const State *state, const M &metadata) 238 { 239 StateStorage::addState(state); 240 metadata_.push_back(metadata); 241 } 242 clear()243 void clear() override 244 { 245 StateStorage::clear(); 246 metadata_.clear(); 247 } 248 249 /** \brief Get const access to the metadata of a state at a particular index */ getMetadata(unsigned int index)250 const M &getMetadata(unsigned int index) const 251 { 252 assert(metadata_.size() > index); 253 return metadata_[index]; 254 } 255 256 /** \brief Get write access to the metadata of a state at a particular index */ getMetadata(unsigned int index)257 M &getMetadata(unsigned int index) 258 { 259 assert(metadata_.size() > index); 260 return metadata_[index]; 261 } 262 263 protected: loadMetadata(const Header &,boost::archive::binary_iarchive & ia)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 storeMetadata(const Header &,boost::archive::binary_oarchive & oa)271 void storeMetadata(const Header & /*h*/, boost::archive::binary_oarchive &oa) override 272 { 273 oa << metadata_; 274 } 275 276 /** \brief The metadata for each state */ 277 std::vector<M> metadata_; 278 }; 279 280 /** \brief Storage of states where the metadata is a vector of indices. This is is typically used to store a 281 * graph */ 282 using GraphStateStorage = StateStorageWithMetadata<std::vector<std::size_t>>; 283 using GraphStateStoragePtr = std::shared_ptr<GraphStateStorage>; 284 } 285 } 286 #endif 287