| Directory: | ./ |
|---|---|
| File: | include/hpp/pinocchio/serialization.hh |
| Date: | 2025-05-04 12:09:19 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 79 | 86 | 91.9% |
| Branches: | 49 | 106 | 46.2% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2020 CNRS | ||
| 3 | // Author: Joseph Mirabel | ||
| 4 | // | ||
| 5 | |||
| 6 | // Redistribution and use in source and binary forms, with or without | ||
| 7 | // modification, are permitted provided that the following conditions are | ||
| 8 | // met: | ||
| 9 | // | ||
| 10 | // 1. Redistributions of source code must retain the above copyright | ||
| 11 | // notice, this list of conditions and the following disclaimer. | ||
| 12 | // | ||
| 13 | // 2. Redistributions in binary form must reproduce the above copyright | ||
| 14 | // notice, this list of conditions and the following disclaimer in the | ||
| 15 | // documentation and/or other materials provided with the distribution. | ||
| 16 | // | ||
| 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| 18 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| 19 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR | ||
| 20 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
| 21 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | ||
| 22 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT | ||
| 23 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, | ||
| 24 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY | ||
| 25 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
| 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
| 27 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH | ||
| 28 | // DAMAGE. | ||
| 29 | |||
| 30 | #ifndef HPP_PINOCCHIO_SERIALIZATION_HH | ||
| 31 | #define HPP_PINOCCHIO_SERIALIZATION_HH | ||
| 32 | |||
| 33 | #include <boost/serialization/shared_ptr.hpp> | ||
| 34 | #include <boost/serialization/split_free.hpp> | ||
| 35 | #include <boost/serialization/weak_ptr.hpp> | ||
| 36 | #include <hpp/pinocchio/device.hh> | ||
| 37 | #include <hpp/pinocchio/fwd.hh> | ||
| 38 | #include <hpp/pinocchio/humanoid-robot.hh> | ||
| 39 | #include <hpp/util/serialization.hh> | ||
| 40 | #include <pinocchio/fwd.hpp> | ||
| 41 | #include <pinocchio/serialization/eigen.hpp> | ||
| 42 | #include <set> | ||
| 43 | #include <type_traits> | ||
| 44 | |||
| 45 | 36 | BOOST_SERIALIZATION_SPLIT_FREE(hpp::pinocchio::DevicePtr_t) | |
| 46 | 16 | BOOST_SERIALIZATION_SPLIT_FREE(hpp::pinocchio::DeviceWkPtr_t) | |
| 47 | |||
| 48 | namespace boost { | ||
| 49 | namespace serialization { | ||
| 50 | template <class Archive> | ||
| 51 | 9 | inline void load(Archive& ar, hpp::pinocchio::DevicePtr_t& d, | |
| 52 | const unsigned int version) { | ||
| 53 | 9 | load<Archive, hpp::pinocchio::Device>(ar, d, version); | |
| 54 | 9 | auto* har = hpp::serialization::cast(&ar); | |
| 55 |
4/8✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
|
9 | if (d && har && har->contains(d->name())) |
| 56 | 9 | d = har->template get<hpp::pinocchio::Device>(d->name(), true)->self(); | |
| 57 | 9 | } | |
| 58 | template <class Archive> | ||
| 59 | 16 | inline void load(Archive& ar, hpp::pinocchio::DeviceWkPtr_t& d, | |
| 60 | const unsigned int version) { | ||
| 61 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
16 | load<Archive, hpp::pinocchio::Device>(ar, d, version); |
| 62 | 16 | auto* har = hpp::serialization::cast(&ar); | |
| 63 |
1/2✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | std::string name(d.lock()->name()); |
| 64 |
6/14✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 8 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 8 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 8 times.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
16 | if (d.lock() && har && har->contains(name)) |
| 65 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
16 | d = har->template get<hpp::pinocchio::Device>(name, true)->self(); |
| 66 | } | ||
| 67 | template <class Archive> | ||
| 68 | 2 | inline void load(Archive& ar, hpp::pinocchio::HumanoidRobotPtr_t& d, | |
| 69 | const unsigned int version) { | ||
| 70 | 2 | load<Archive, hpp::pinocchio::HumanoidRobot>(ar, d, version); | |
| 71 | 2 | auto* har = hpp::serialization::cast(&ar); | |
| 72 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
2 | if (d && har && har->contains(d->name())) |
| 73 | 2 | d = har->template getChildClass<hpp::pinocchio::Device, | |
| 74 | 2 | hpp::pinocchio::HumanoidRobot>(d->name(), | |
| 75 | true) | ||
| 76 | ->self(); | ||
| 77 | } | ||
| 78 | template <class Archive> | ||
| 79 | 2 | inline void load(Archive& ar, hpp::pinocchio::HumanoidRobotWkPtr_t& d, | |
| 80 | const unsigned int version) { | ||
| 81 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | load<Archive, hpp::pinocchio::HumanoidRobot>(ar, d, version); |
| 82 | 2 | auto* har = hpp::serialization::cast(&ar); | |
| 83 |
1/2✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | std::string name(d.lock()->name()); |
| 84 |
6/14✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
2 | if (d.lock() && har && har->contains(name)) |
| 85 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | d = har->template getChildClass<hpp::pinocchio::Device, |
| 86 | hpp::pinocchio::HumanoidRobot>(name, true) | ||
| 87 | ->self(); | ||
| 88 | } | ||
| 89 | |||
| 90 | template <class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, | ||
| 91 | int _MaxRows, int _MaxCols> | ||
| 92 | 4 | inline void serialize( | |
| 93 | Archive& ar, | ||
| 94 | Eigen::Array<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& m, | ||
| 95 | const unsigned int version) { | ||
| 96 | (void)version; | ||
| 97 | 4 | Eigen::DenseIndex rows(m.rows()), cols(m.cols()); | |
| 98 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
4 | ar& BOOST_SERIALIZATION_NVP(rows); |
| 99 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
4 | ar& BOOST_SERIALIZATION_NVP(cols); |
| 100 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | if (!Archive::is_saving::value) m.resize(rows, cols); |
| 101 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
|
4 | if (m.size() > 0) |
| 102 | ✗ | ar& make_nvp("data", make_array(m.data(), (size_t)m.size())); | |
| 103 | 4 | } | |
| 104 | |||
| 105 | } // namespace serialization | ||
| 106 | } // namespace boost | ||
| 107 | |||
| 108 | namespace hpp { | ||
| 109 | namespace serialization { | ||
| 110 | namespace remove_duplicate { | ||
| 111 | template <typename Key, typename Compare = std::less<Key>> | ||
| 112 | struct ptr_less : Compare { | ||
| 113 | 2 | inline bool operator()(Key const* t1, Key const* t2) const { | |
| 114 | 2 | return Compare::operator()(*t1, *t2); | |
| 115 | } | ||
| 116 | }; | ||
| 117 | |||
| 118 | template <typename Derived> | ||
| 119 | struct eigen_compare { | ||
| 120 | 22 | bool operator()(const Eigen::PlainObjectBase<Derived>& a, | |
| 121 | const Eigen::PlainObjectBase<Derived>& b) const { | ||
| 122 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 22 times.
|
22 | if (a.size() < b.size()) return true; |
| 123 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 22 times.
|
22 | if (a.size() > b.size()) return false; |
| 124 |
2/2✓ Branch 1 taken 110 times.
✓ Branch 2 taken 22 times.
|
132 | for (Eigen::Index i = 0; i < a.size(); ++i) { |
| 125 |
1/2✗ Branch 4 not taken.
✓ Branch 5 taken 110 times.
|
110 | if (a.derived().data()[i] < b.derived().data()[i]) return true; |
| 126 |
1/2✗ Branch 4 not taken.
✓ Branch 5 taken 110 times.
|
110 | if (a.derived().data()[i] > b.derived().data()[i]) return false; |
| 127 | } | ||
| 128 | 22 | return false; | |
| 129 | } | ||
| 130 | }; | ||
| 131 | |||
| 132 | template <typename Key, typename Compare = std::less<Key>> | ||
| 133 | struct archive { | ||
| 134 | typedef Compare compare_type; | ||
| 135 | typedef ptr_less<Key, Compare> ptr_compare_type; | ||
| 136 | std::set<Key const*, ptr_compare_type> datas; | ||
| 137 | int hitcount; | ||
| 138 | |||
| 139 | 13 | archive() : hitcount(0) {} | |
| 140 | }; | ||
| 141 | |||
| 142 | typedef archive<::hpp::pinocchio::vector_t, | ||
| 143 | eigen_compare<::hpp::pinocchio::vector_t>> | ||
| 144 | vector_archive; | ||
| 145 | |||
| 146 | template <class Archive, typename Key> | ||
| 147 | 36 | inline void load_or_save_no_remove_duplicate_check(Archive& ar, | |
| 148 | const char* name, Key& key, | ||
| 149 | const unsigned int version) { | ||
| 150 | (void)version; | ||
| 151 | 36 | Key* value = &key; | |
| 152 |
1/2✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
|
36 | ar& boost::serialization::make_nvp(name, value); |
| 153 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | if (!Archive::is_saving::value) key = *value; |
| 154 | 36 | } | |
| 155 | |||
| 156 | template <class Archive, typename Key> | ||
| 157 | ✗ | inline void save_impl(Archive& ar, const char* name, const Key& key, | |
| 158 | const unsigned int version) { | ||
| 159 | (void)version; | ||
| 160 | ✗ | Key const* value = &key; | |
| 161 | ✗ | ar << boost::serialization::make_nvp(name, value); | |
| 162 | } | ||
| 163 | |||
| 164 | template <class Archive, typename Key, typename Compare = std::less<Key>> | ||
| 165 | 28 | inline void save_impl(Archive& ar, | |
| 166 | std::set<Key const*, ptr_less<Key, Compare>>& set, | ||
| 167 | int& hitcount, const char* name, const Key& key, | ||
| 168 | const unsigned int version) { | ||
| 169 | (void)version; | ||
| 170 | if (!Archive::is_saving::value) | ||
| 171 | throw std::logic_error( | ||
| 172 | "HPP serialization: cannot load into a const element. This should " | ||
| 173 | "never happen."); | ||
| 174 |
1/2✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
|
28 | auto result = set.insert(&key); |
| 175 | 28 | bool inserted = result.second; | |
| 176 |
2/2✓ Branch 0 taken 13 times.
✓ Branch 1 taken 1 times.
|
28 | Key const* k = (inserted ? &key : *result.first); |
| 177 |
1/2✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
|
28 | ar& boost::serialization::make_nvp(name, k); |
| 178 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 13 times.
|
28 | if (!inserted) hitcount++; |
| 179 | } | ||
| 180 | |||
| 181 | template <class Archive, typename Key, typename Compare = std::less<Key>> | ||
| 182 | 4 | inline void serialize(Archive& ar, | |
| 183 | std::set<Key const*, ptr_less<Key, Compare>>& set, | ||
| 184 | int& hitcount, const char* name, Key& key, | ||
| 185 | const unsigned int version) { | ||
| 186 | if (Archive::is_saving::value) { | ||
| 187 | 4 | save_impl(ar, set, hitcount, name, key, version); | |
| 188 | } else { | ||
| 189 | ✗ | load_or_save_no_remove_duplicate_check(ar, name, key, version); | |
| 190 | } | ||
| 191 | } | ||
| 192 | |||
| 193 | template <bool is_base> | ||
| 194 | struct serialiaze_impl { | ||
| 195 | template <typename Archive, typename Key, typename Compare = std::less<Key>> | ||
| 196 | 4 | static inline void run(Archive& ar, const char* name, Key& key, | |
| 197 | const unsigned int version) { | ||
| 198 |
0/2✗ Branch 0 not taken.
✗ Branch 1 not taken.
|
4 | archive<Key, Compare>& rda = dynamic_cast<archive<Key, Compare>&>(ar); |
| 199 | 4 | serialize(ar, rda.datas, rda.hitcount, name, key, version); | |
| 200 | } | ||
| 201 | template <typename Archive, typename Key, typename Compare = std::less<Key>> | ||
| 202 | 24 | static inline void save(Archive& ar, const char* name, const Key& key, | |
| 203 | const unsigned int version) { | ||
| 204 |
1/2✓ Branch 0 taken 12 times.
✗ Branch 1 not taken.
|
24 | archive<Key, Compare>& rda = dynamic_cast<archive<Key, Compare>&>(ar); |
| 205 | 24 | save_impl(ar, rda.datas, rda.hitcount, name, key, version); | |
| 206 | } | ||
| 207 | }; | ||
| 208 | |||
| 209 | template <> | ||
| 210 | struct serialiaze_impl<false> { | ||
| 211 | template <typename Archive, typename Key, typename Compare = std::less<Key>> | ||
| 212 | 36 | static inline void run(Archive& ar, const char* name, Key& key, | |
| 213 | const unsigned int version) { | ||
| 214 |
2/4✓ Branch 0 taken 18 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 18 times.
|
36 | if (dynamic_cast<archive<Key, Compare>*>(&ar) != NULL) |
| 215 | ✗ | serialiaze_impl<true>::run<Archive, Key, Compare>(ar, name, key, version); | |
| 216 | else | ||
| 217 | 36 | load_or_save_no_remove_duplicate_check(ar, name, key, version); | |
| 218 | 36 | } | |
| 219 | template <typename Archive, typename Key, typename Compare = std::less<Key>> | ||
| 220 | 24 | static inline void save(Archive& ar, const char* name, const Key& key, | |
| 221 | const unsigned int version) { | ||
| 222 |
2/4✓ Branch 0 taken 12 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
|
24 | if (dynamic_cast<archive<Key, Compare>*>(&ar) != NULL) |
| 223 | 24 | serialiaze_impl<true>::save<Archive, Key, Compare>(ar, name, key, | |
| 224 | version); | ||
| 225 | else | ||
| 226 | ✗ | save_impl(ar, name, key, version); | |
| 227 | } | ||
| 228 | }; | ||
| 229 | |||
| 230 | template <typename Archive, typename Key, typename Compare = std::less<Key>, | ||
| 231 | bool is_base = std::is_base_of<archive<Key, Compare>, Archive>::value> | ||
| 232 | 40 | inline void serialize(Archive& ar, const char* name, Key& key, | |
| 233 | const unsigned int version) { | ||
| 234 | 40 | serialiaze_impl<is_base>::template run<Archive, Key, Compare>(ar, name, key, | |
| 235 | version); | ||
| 236 | 40 | } | |
| 237 | |||
| 238 | template <typename Archive, typename Key, typename Compare = std::less<Key>, | ||
| 239 | bool is_base = std::is_base_of<archive<Key, Compare>, Archive>::value> | ||
| 240 | 24 | inline void save(Archive& ar, const char* name, const Key& key, | |
| 241 | const unsigned int version) { | ||
| 242 | 24 | serialiaze_impl<is_base>::template save<Archive, Key, Compare>(ar, name, key, | |
| 243 | version); | ||
| 244 | } | ||
| 245 | |||
| 246 | template <typename Archive> | ||
| 247 | 40 | inline void serialize_vector(Archive& ar, const char* name, | |
| 248 | ::hpp::pinocchio::vector_t& key, | ||
| 249 | const unsigned int version) { | ||
| 250 | 40 | serialize<Archive, ::hpp::pinocchio::vector_t, vector_archive::compare_type>( | |
| 251 | ar, name, key, version); | ||
| 252 | 40 | } | |
| 253 | |||
| 254 | template <typename Archive> | ||
| 255 | 24 | inline void save_vector(Archive& ar, const char* name, | |
| 256 | const ::hpp::pinocchio::vector_t& key, | ||
| 257 | const unsigned int version) { | ||
| 258 | 24 | save<Archive, ::hpp::pinocchio::vector_t, vector_archive::compare_type>( | |
| 259 | ar, name, key, version); | ||
| 260 | } | ||
| 261 | |||
| 262 | } // namespace remove_duplicate | ||
| 263 | } // namespace serialization | ||
| 264 | } // namespace hpp | ||
| 265 | |||
| 266 | #endif // HPP_PINOCCHIO_SERIALIZATION_HH | ||
| 267 |