diff --git a/navmap_core/include/navmap_core/DistanceField.hpp b/navmap_core/include/navmap_core/DistanceField.hpp index b898f7a..47f51b4 100644 --- a/navmap_core/include/navmap_core/DistanceField.hpp +++ b/navmap_core/include/navmap_core/DistanceField.hpp @@ -77,6 +77,9 @@ class DistanceField /// Whether @p cid is currently a seed cell. bool is_seed(NavCelId cid) const {return cid < seed_of_.size() && seed_of_[cid] != kNoSeed;} + /// Entries of the spatial index of the seeds (a seed has one per bucket it is sampled in). + std::size_t index_entries() const; + private: static constexpr std::uint32_t kNoSeed = std::numeric_limits::max(); struct Seed @@ -102,6 +105,7 @@ class DistanceField float max_distance_; float bucket_{1.0f}; std::unordered_map> buckets_; ///< seed index + std::vector keys_scratch_; ///< index_seed()'s buckets, reused std::vector centroid_; std::vector seeds_; ///< open-edge seeds first, then point seeds std::size_t n_boundary_{0}; diff --git a/navmap_core/include/navmap_core/NavMap.hpp b/navmap_core/include/navmap_core/NavMap.hpp index f07f26a..87732e1 100644 --- a/navmap_core/include/navmap_core/NavMap.hpp +++ b/navmap_core/include/navmap_core/NavMap.hpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -312,8 +313,9 @@ class LayerRegistry /** * \brief Add a new typed layer or return an existing one with the same name. * - * If a layer with \p name already exists, it is returned (no resize). - * Otherwise a new layer is created with \p nitems elements. + * If a layer with \p name already exists with the type \p T, it is returned (no resize). + * Otherwise (none, or one of another type) a new layer is created with \p nitems elements; a + * layer of another type is replaced, and its data is lost. * * \tparam T Storage type (e.g., uint8_t, float, double). * \param name Layer name (unique key). @@ -329,7 +331,7 @@ class LayerRegistry { auto it = layers_.find(name); if (it != layers_.end()) { - return std::dynamic_pointer_cast>(it->second); + if (auto same = std::dynamic_pointer_cast>(it->second)) {return same;} } auto view = std::make_shared>(name, nitems, type); layers_[name] = view; @@ -880,7 +882,8 @@ class NavMap const std::string & unit = {}, T default_value = T{}) { - const bool existed = has_layer(name); + // a layer of another type is replaced by a new one (add_or_get) + const bool existed = std::dynamic_pointer_cast>(layers.get(name)) != nullptr; auto v = layers.add_or_get(name, navcels.size(), layer_type_tag()); if (!existed || v->data().size() != navcels.size()) { // new layers start at default_value v->data().assign(navcels.size(), default_value); @@ -1172,7 +1175,11 @@ class NavMap * cannot be determined, returns false. Supports U8/F32/F64 layers. * * Efficiency: BFS from the seed NavCel with XY AABB pruning against the - * circumscribed square of the area. Inclusion test by triangle centroid. + * circumscribed square of the area. Inclusion test by triangle centroid. The work and the + * memory are those of the area, not of the map. + * + * Every NavCel written is recorded as changed in the layer (as layer_set() does), so that the + * incremental consumers of the layer (e.g. an inflation filter) see the new values. * * \tparam T uint8_t, int16_t, float, or double * \param p_world 3D point in world coordinates (projected to ground) @@ -1252,10 +1259,28 @@ class NavMap } }; - std::vector visited(navcels.size(), 0); + // Visited NavCels: a hash set while the area is small (a vector per map would cost O(map) on + // every call), a flag per NavCel once it is large (a hash set costs ~50 bytes per NavCel) + std::unordered_set seen; + std::vector flags; + const std::size_t to_flags = navcels.size() / 32 + 1; + auto first_visit = [&](NavCelId c) -> bool { + if (!flags.empty()) { + if (flags[c]) {return false;} + flags[c] = 1; + return true; + } + if (!seen.insert(c).second) {return false;} + if (seen.size() > to_flags) { + flags.assign(navcels.size(), 0); + for (const auto s : seen) {flags[s] = 1;} + std::unordered_set().swap(seen); + } + return true; + }; + first_visit(start_cid); std::deque q; q.push_back(start_cid); - visited[static_cast(start_cid)] = 1; auto & data = layer->mutable_data(); @@ -1269,14 +1294,13 @@ class NavMap const Eigen::Vector3f c = navcel_centroid(cid); if (inside_area(c)) { data[static_cast(cid)] = value; + layer->mark_changed(cid); } for (const auto ncid : navcel_neighbors(cid)) { const std::size_t nidx = static_cast(ncid); - if (nidx < navcels.size() && !visited[nidx] && tri_aabb_intersects_box(ncid)) { - visited[nidx] = 1; - q.push_back(ncid); - } + if (nidx >= navcels.size() || !tri_aabb_intersects_box(ncid)) {continue;} + if (first_visit(ncid)) {q.push_back(ncid);} } } diff --git a/navmap_core/src/navmap_core/DistanceField.cpp b/navmap_core/src/navmap_core/DistanceField.cpp index 76c8735..2f93101 100644 --- a/navmap_core/src/navmap_core/DistanceField.cpp +++ b/navmap_core/src/navmap_core/DistanceField.cpp @@ -134,34 +134,50 @@ void DistanceField::splat(std::uint32_t s) }); } +std::size_t DistanceField::index_entries() const +{ + std::size_t n = 0; + for (const auto & kv : buckets_) { + n += kv.second.size(); + } + return n; +} + std::int64_t DistanceField::bucket_key(int bx, int by, int bz) { return (static_cast(bx) * 73856093) ^ (static_cast(by) * 19349663) ^ (static_cast(bz) * 83492791); } +// A seed goes into the buckets of points sampled along it every bucket_ metres (one bucket for a +// point seed), not into every bucket of its bounding box: a long diagonal open edge (the coarse +// NavCels of a level-of-detail mesh have edges of tens of metres) would otherwise fill hundreds +// of buckets, and the index cost would grow with the square of its length. Every point of the +// segment is then within bucket_ / 2 of a sample: the queries widen their range by that (see +// update()). void DistanceField::index_seed(std::uint32_t s, bool insert) { - const Eigen::Vector3f lo = seeds_[s].a.cwiseMin(seeds_[s].b) / bucket_; - const Eigen::Vector3f hi = seeds_[s].a.cwiseMax(seeds_[s].b) / bucket_; - for (int bx = static_cast(std::floor(lo.x())); bx <= static_cast(std::floor(hi.x())); - ++bx) - { - for (int by = static_cast(std::floor(lo.y())); by <= static_cast(std::floor(hi.y())); - ++by) - { - for (int bz = - static_cast(std::floor(lo.z())); bz <= static_cast(std::floor(hi.z())); - ++bz) - { - auto & v = buckets_[bucket_key(bx, by, bz)]; - if (insert) { - v.push_back(s); - } else { - const auto it = std::find(v.begin(), v.end(), s); - if (it != v.end()) {*it = v.back(); v.pop_back();} - } - } + const Eigen::Vector3f a = seeds_[s].a, b = seeds_[s].b; + const int steps = std::max(1, static_cast(std::ceil((b - a).norm() / bucket_))); + auto & keys = keys_scratch_; + keys.clear(); + keys.reserve(static_cast(steps) + 1); + for (int k = 0; k <= steps; ++k) { + const Eigen::Vector3f p = (a + (b - a) * (static_cast(k) / steps)) / bucket_; + keys.push_back( + bucket_key( + static_cast(std::floor(p.x())), static_cast(std::floor(p.y())), + static_cast(std::floor(p.z())))); + } + std::sort(keys.begin(), keys.end()); + keys.erase(std::unique(keys.begin(), keys.end()), keys.end()); + for (const auto key : keys) { + auto & v = buckets_[key]; + if (insert) { + v.push_back(s); + } else { + const auto it = std::find(v.begin(), v.end(), s); + if (it != v.end()) {*it = v.back(); v.pop_back();} } } } @@ -268,14 +284,16 @@ void DistanceField::update( }); const Eigen::Vector3f p = seeds_[s].a; const float r2 = 2.0f * max_distance_; - const Eigen::Vector3f lo = (p.array() - r2).matrix() / bucket_; - const Eigen::Vector3f hi = (p.array() + r2).matrix() / bucket_; - for (int bx = static_cast(std::floor(lo.x())); bx <= static_cast(std::floor(hi.x())); - ++bx) + // a segment seed is indexed by samples along it, each within bucket_ / 2 of its points + // (index_seed): widen the range by that (a hair more, for the rounding of the samples) + const float reach = r2 + 0.51f * bucket_; + const Eigen::Vector3f lo = (p.array() - reach).matrix() / bucket_; + const Eigen::Vector3f hi = (p.array() + reach).matrix() / bucket_; + for (int bx = static_cast(std::floor(lo.x())); + bx <= static_cast(std::floor(hi.x())); ++bx) { - for (int by = - static_cast(std::floor(lo.y())); by <= static_cast(std::floor(hi.y())); - ++by) + for (int by = static_cast(std::floor(lo.y())); + by <= static_cast(std::floor(hi.y())); ++by) { for (int bz = static_cast(std::floor(lo.z())); bz <= static_cast(std::floor(hi.z())); ++bz) diff --git a/navmap_core/src/navmap_core/NavMap.cpp b/navmap_core/src/navmap_core/NavMap.cpp index c4bc85c..2401242 100644 --- a/navmap_core/src/navmap_core/NavMap.cpp +++ b/navmap_core/src/navmap_core/NavMap.cpp @@ -144,7 +144,7 @@ NavMap & NavMap::operator=(const NavMap & other) for (const auto & name : other.layers.list()) { auto src_base = other.layers.get(name); if (!src_base) {continue;} - + // (a layer of the same name and another type is replaced by add_or_get) switch (src_base->type()) { case LayerType::U8: { auto src = std::dynamic_pointer_cast>(src_base); diff --git a/navmap_core/tests/test_incremental.cpp b/navmap_core/tests/test_incremental.cpp index c6e01d0..2f0ca10 100644 --- a/navmap_core/tests/test_incremental.cpp +++ b/navmap_core/tests/test_incremental.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -60,7 +61,7 @@ void add_grid( std::vector maps() { - std::vector out(3); + std::vector out(4); add_grid( out[0], 20, 20, 0.5f, [](float, float) {return 0.0f;}, [](int i, int j) { @@ -69,6 +70,17 @@ std::vector maps() add_grid(out[1], 16, 16, 0.5f, [](float, float) {return 0.0f;}); add_grid(out[1], 16, 16, 0.5f, [](float, float) {return 3.0f;}); add_grid(out[2], 30, 6, 0.4f, [](float x, float) {return 0.25f * x;}); + // Coarse cells (4 m) with holes, turned 30 deg: long diagonal open edges, as the far NavCels of + // a level-of-detail mesh (the seed index samples along them, see DistanceField::index_seed) + add_grid( + out[3], 12, 12, 4.0f, [](float, float) {return 0.0f;}, + [](int i, int j) {return (i * 7 + j * 3) % 5 == 0;}); + const float c = std::cos(0.5236f), s = std::sin(0.5236f); + for (std::size_t v = 0; v < out[3].positions.x.size(); ++v) { + const float x = out[3].positions.x[v], y = out[3].positions.y[v]; + out[3].positions.x[v] = c * x - s * y; + out[3].positions.y[v] = s * x + c * y; + } for (auto & m : out) { m.rebuild_geometry_accels(); } @@ -279,6 +291,70 @@ TEST(DistanceFieldIncremental, AnUpdateOnlyTouchesItsNeighbourhood) } } +TEST(DistanceFieldIncremental, LongDiagonalOpenEdgesAreIndexedBySamplesNotBoundingBoxes) +{ + // The same mesh (a grid with 20 % holes) with cells of 0.5 m and of 16 m, turned 45 deg: only the + // length of the open edges changes. Indexed by their bounding boxes, a 16 m edge went into + // ~(16 / 0.6)^2 buckets; sampled along it, into ~16 / 0.6 (counted, so that no timing is needed) + auto build = [](float res) { + NavMap nm; + std::mt19937 rng(9); + std::vector hole(120 * 120); + for (auto & h : hole) { + h = rng() % 5 == 0; + } + add_grid( + nm, 120, 120, res, [](float, float) {return 0.0f;}, + [&hole](int i, int j) {return hole[static_cast(j) * 120 + i] != 0;}); + const float c = std::cos(0.7854f), s = std::sin(0.7854f); + for (std::size_t v = 0; v < nm.positions.x.size(); ++v) { + const float x = nm.positions.x[v], y = nm.positions.y[v]; + nm.positions.x[v] = c * x - s * y; + nm.positions.y[v] = s * x + c * y; + } + nm.rebuild_geometry_accels(); + return nm; + }; + auto entries = [](const NavMap & nm) { + DistanceField df(nm, true, 0.6f); + df.compute({}); + return df.index_entries(); + }; + const NavMap fine = build(0.5f), coarse = build(16.0f); + const double e_fine = static_cast(entries(fine)); + const double e_coarse = static_cast(entries(coarse)); + ASSERT_GT(e_fine, 0.0); + EXPECT_LT(e_coarse, 40.0 * e_fine) << "fine " << e_fine << ", coarse " << e_coarse; +} + +TEST(NavMapSetArea, SmallAndLargeAreasMarkExactlyTheNavCelsInsideThem) +{ + // A small area is followed with a hash set of visited NavCels, a large one with a flag per + // NavCel: both must mark the NavCels whose centroid is inside, and only them + NavMap nm; + add_grid(nm, 30, 30, 0.5f, [](float, float) {return 0.0f;}); + nm.rebuild_geometry_accels(); + nm.add_layer("area", "", "", 0); + for (const float radius : {1.0f, 3.0f, 100.0f}) { + nm.layer_clear("area", 0); + ASSERT_TRUE( + nm.set_area( + Eigen::Vector3f(7.0f, 7.0f, 0.0f), static_cast(1), "area", + navmap::AreaShape::CIRCULAR, radius)); + std::size_t marked = 0, expected = 0; + for (std::size_t c = 0; c < nm.navcels.size(); ++c) { + const auto p = nm.navcel_centroid(static_cast(c)); + const bool inside = std::hypot(p.x() - 7.0f, p.y() - 7.0f) <= radius; + marked += nm.layer_get("area", static_cast(c), 0); + expected += inside ? 1 : 0; + EXPECT_EQ(nm.layer_get("area", static_cast(c), 0), inside ? 1 : 0) + << "radius " << radius << ", NavCel " << c; + } + EXPECT_GT(expected, 0u); + EXPECT_EQ(marked, expected); + } +} + // ------------------------------------------------------------------ layer change tracking TEST(LayerChanges, SingleWritesAreRecordedOnceAndBulkWritesMarkTheLayer) diff --git a/navmap_core/tests/test_navmap_assign.cpp b/navmap_core/tests/test_navmap_assign.cpp index 7a33b03..8499dd2 100644 --- a/navmap_core/tests/test_navmap_assign.cpp +++ b/navmap_core/tests/test_navmap_assign.cpp @@ -284,3 +284,52 @@ TEST(NavMapAssign, CopiesNeverShareLayerData) EXPECT_EQ(moved.layer_get("occ", 0), 1); EXPECT_FLOAT_EQ(moved.layer_get("extra", 0), 3.0f); } + +TEST(NavMapAssign, ALayerReCreatedWithAnotherTypeIsReplacedNotDereferencedAsNull) +{ + // The source has "occ" as F32 where the destination (same geometry) has it as U8: the copy + // is a F32 layer. Before, operator= asked the destination for a F32 view of its U8 layer, got + // none and crashed (NavState::set of a map copied, edited and set back). + NavMap dst; + fill_one_tri_map(dst); + NavMap src = dst; + src.layers.remove("occ"); + src.layer_set("occ", 0, 3.5f); + dst = src; + ASSERT_TRUE(dst.layers.get("occ")); + EXPECT_EQ(dst.layers.get("occ")->type(), navmap::LayerType::F32); + EXPECT_FLOAT_EQ(dst.layer_get("occ", 0, 0.0f), 3.5f); + // and back to U8 + NavMap again = dst; + again.layers.remove("occ"); + again.layer_set("occ", 0, 7); + dst = again; + EXPECT_EQ(dst.layers.get("occ")->type(), navmap::LayerType::U8); + EXPECT_EQ(dst.layer_get("occ", 0, 0), 7); +} + +TEST(NavMapAssign, EveryWriterReplacesALayerOfAnotherType) +{ + // layer_set, layer_clear, add_layer and add_or_get all go through LayerRegistry::add_or_get, + // which used to return null for a layer of another type (and the callers dereferenced it) + NavMap m; + fill_one_tri_map(m); // "occ" is U8 + m.layer_set("occ", 0, 1.5f); + ASSERT_TRUE(m.layers.get("occ")); + EXPECT_EQ(m.layers.get("occ")->type(), navmap::LayerType::F32); + EXPECT_FLOAT_EQ(m.layer_get("occ", 0, 0.0f), 1.5f); + + m.layer_clear("occ", 9); + EXPECT_EQ(m.layers.get("occ")->type(), navmap::LayerType::U8); + EXPECT_EQ(m.layer_get("occ", 0, 0), 9); + + m.add_layer("occ", "", "", 2.0); + EXPECT_EQ(m.layers.get("occ")->type(), navmap::LayerType::F64); + EXPECT_DOUBLE_EQ(m.layer_get("occ", 0, 0.0), 2.0); + + auto view = m.layers.add_or_get("occ", m.navcels.size(), layer_type_tag()); + ASSERT_TRUE(view); + EXPECT_EQ(m.layers.get("occ")->type(), navmap::LayerType::I16); + // the same type: the layer itself, not a new one + EXPECT_EQ(m.layers.add_or_get("occ", m.navcels.size(), layer_type_tag()), view); +} diff --git a/navmap_core/tests/test_navmap_easy.cpp b/navmap_core/tests/test_navmap_easy.cpp index 5173030..64c57dd 100644 --- a/navmap_core/tests/test_navmap_easy.cpp +++ b/navmap_core/tests/test_navmap_easy.cpp @@ -484,3 +484,30 @@ TEST(NavMap_SetArea_Systematic, CircularNearBoundary) nm, "obstacles", static_cast(180), [&](const Eigen::Vector3f & cc) {return centroid_inside_circle(cc, cx, cy, r);}); } + +TEST(NavMap_SetArea_Systematic, TheWrittenNavCelsAreRecordedAsChangedForIncrementalConsumers) +{ + // An incremental consumer (an inflation filter) reads the layer's changed() list: set_area + // records there exactly the NavCels it wrote, as layer_set does + NavMap nm; + make_grid(nm, /*nx=*/ 20, /*ny=*/ 20); + nm.add_layer("obstacles", "occupancy obstacles", "%", 0); + const auto view = + std::dynamic_pointer_cast>(nm.layers.get("obstacles")); + ASSERT_TRUE(view); + view->clear_changes(); + const float cx = 0.5f, cy = 0.5f, r = 0.22f; + ASSERT_TRUE(nm.set_area( + Eigen::Vector3f(cx, cy, 1.0f), static_cast(254), "obstacles", + navmap::AreaShape::CIRCULAR, r)); + std::vector written; + for (navmap::NavCelId c = 0; c < nm.navcels.size(); ++c) { + if (nm.layer_get("obstacles", c, 0) == 254) {written.push_back(c);} + } + ASSERT_FALSE(written.empty()); + std::vector changed = view->changed(); + std::sort(changed.begin(), changed.end()); + changed.erase(std::unique(changed.begin(), changed.end()), changed.end()); + EXPECT_EQ(changed, written); + EXPECT_TRUE(view->has_changes()); +} diff --git a/navmap_ros/include/navmap_ros/navmap_io.hpp b/navmap_ros/include/navmap_ros/navmap_io.hpp index 6dabaf4..74bcfa2 100644 --- a/navmap_ros/include/navmap_ros/navmap_io.hpp +++ b/navmap_ros/include/navmap_ros/navmap_io.hpp @@ -61,8 +61,25 @@ struct SaveOptions bool binary{true}; Compression compression{Compression::ZLIB}; int level{6}; ///< zlib level, 1 (fast) to 9 (small) + /// The largest message (serialised CDR bytes) the saver attempts. The CDR serialiser of ROS 2 + /// (Fast-CDR) cannot hold more than 4 GiB: a bigger map is refused, with + /// std::errc::file_too_large (about 180 million NavCels with one layer). save_to_file refuses + /// it from the NavMap's size, before the message is built; save_msg_to_file, from the message's. + std::uint64_t max_serialized_bytes{(std::uint64_t{1} << 32) - (std::uint64_t{1} << 20)}; }; +/** + * @brief A close upper estimate of the CDR size of @p msg (bytes), without serialising it: the + * arrays and their length prefixes, plus a fixed allowance for the headers and strings. + */ +std::uint64_t estimated_serialized_size(const navmap_ros_interfaces::msg::NavMap & msg); + +/** + * @brief The same estimate from a NavMap, without building its message (`to_msg` would hold the + * map a second time). + */ +std::uint64_t estimated_serialized_size(const navmap::NavMap & map); + /** * @brief Save a `NavMap` message to disk using ROS 2 CDR serialization. * diff --git a/navmap_ros/src/navmap_ros/navmap_io.cpp b/navmap_ros/src/navmap_ros/navmap_io.cpp index b1fc74c..1693913 100644 --- a/navmap_ros/src/navmap_ros/navmap_io.cpp +++ b/navmap_ros/src/navmap_ros/navmap_io.cpp @@ -51,6 +51,93 @@ inline void set_error(std::error_code * ec, int ev = EIO) if (ec) {*ec = std::error_code(ev, std::generic_category());} } +// Each sequence: its elements (with alignment) and a 4-byte length; strings and headers: a +// generous fixed allowance each (kSmall) +std::uint64_t seq(std::size_t n, std::size_t elem) +{ + return static_cast(n) * elem + 4 + 8; +} +constexpr std::uint64_t kSmall = 512; + +std::size_t layer_element_size(navmap::LayerType t) +{ + switch (t) { + case navmap::LayerType::U8: return 1; + case navmap::LayerType::I16: return 2; + case navmap::LayerType::F64: return 8; + default: return 4; // F32 + } +} + +} // namespace + +std::uint64_t estimated_serialized_size(const navmap::NavMap & map) +{ + // The same arrays as to_msg(map) fills, counted without building them + std::uint64_t n = kSmall; + n += 3 * seq(map.positions.x.size(), 4); + if (map.colors.has_value()) {n += 4 * seq(map.positions.x.size(), 1);} + n += 3 * seq(map.navcels.size(), 4); + for (const auto & s : map.surfaces) { + n += kSmall + s.frame_id.size() + seq(s.navcels.size(), 4); + } + for (const auto & name : map.layers.list()) { + if (const auto l = map.layers.get(name)) { + const auto meta = map.get_layer_meta(name); + n += kSmall + name.size() + (meta ? meta->description.size() + meta->unit.size() : 0) + + seq(l->size(), layer_element_size(l->type())); + } + } + n += static_cast(map.links().size()) * kSmall; + return n; +} + +std::uint64_t estimated_serialized_size(const navmap_ros_interfaces::msg::NavMap & msg) +{ + std::uint64_t n = kSmall; // header, scalars + n += seq(msg.positions_x.size(), 4) + seq(msg.positions_y.size(), 4) + + seq(msg.positions_z.size(), 4); + n += seq(msg.colors_r.size(), 1) + seq(msg.colors_g.size(), 1) + seq(msg.colors_b.size(), 1) + + seq(msg.colors_a.size(), 1); + n += seq(msg.navcels_v0.size(), 4) + seq(msg.navcels_v1.size(), 4) + + seq(msg.navcels_v2.size(), 4); + for (const auto & s : msg.surfaces) { + n += kSmall + s.frame_id.size() + seq(s.navcels.size(), 4); + } + for (const auto & l : msg.layers) { + n += kSmall + l.name.size() + l.description.size() + l.unit.size() + + seq(l.indices.size(), 4) + seq(l.data_u8.size(), 1) + seq(l.data_f32.size(), 4) + + seq(l.data_f64.size(), 8) + seq(l.data_i16.size(), 2); + } + n += static_cast(msg.links.size()) * kSmall; + return n; +} + +namespace +{ + +// false (and the error set) if \p estimate bytes are more than the saver attempts +bool fits_in_a_message( + std::uint64_t estimate, const std::string & path, const SaveOptions & options, + std::error_code * ec) +{ + if (estimate <= options.max_serialized_bytes) {return true;} + RCLCPP_ERROR( + rclcpp::get_logger("navmap_io"), + "cannot save %s: the map serialises to about %.2f GB, more than the %.2f GB a ROS 2 (CDR) " + "message holds; save it in pieces or coarser", path.c_str(), estimate / 1e9, + options.max_serialized_bytes / 1e9); + set_error(ec, EFBIG); + return false; +} + +bool fits_in_a_message( + const navmap::NavMap & map, const std::string & path, const SaveOptions & options, + std::error_code * ec) +{ + return fits_in_a_message(estimated_serialized_size(map), path, options, ec); +} + } // namespace bool save_msg_to_file( @@ -60,6 +147,9 @@ bool save_msg_to_file( std::error_code * ec) { using MsgT = navmap_ros_interfaces::msg::NavMap; + // Beyond what the CDR serialiser holds: refused before allocating gigabytes, with the cause + // (it threw a non-std exception from deep inside, reported as a bare I/O error) + if (!fits_in_a_message(estimated_serialized_size(msg), path, options, ec)) {return false;} try { rclcpp::SerializedMessage serialized; rclcpp::Serialization ser; @@ -247,6 +337,8 @@ bool save_to_file( const SaveOptions & options, std::error_code * ec) { + // Refused before to_msg builds the message (gigabytes more than the map itself) + if (!fits_in_a_message(map, path, options, ec)) {return false;} auto msg = navmap_ros::to_msg(map); return save_msg_to_file(msg, path, options, ec); } diff --git a/navmap_ros/tests/test_navmap_io.cpp b/navmap_ros/tests/test_navmap_io.cpp index e3dc5f4..3f23c4f 100644 --- a/navmap_ros/tests/test_navmap_io.cpp +++ b/navmap_ros/tests/test_navmap_io.cpp @@ -315,3 +315,87 @@ TEST(NavMapIoCore, RoundtripViaCoreAndMsgCompare) std::filesystem::remove(path); } + +TEST(NavMapIoMsg, TheSizeEstimateIsCloseAboveTheSerialisedSize) +{ + navmap_ros_interfaces::msg::NavMap msg; + msg.header.frame_id = "map"; + const std::size_t nv = 20000, nc = 39000; + msg.positions_x.assign(nv, 1.0f); + msg.positions_y.assign(nv, 2.0f); + msg.positions_z.assign(nv, 3.0f); + msg.navcels_v0.assign(nc, 1); + msg.navcels_v1.assign(nc, 2); + msg.navcels_v2.assign(nc, 3); + msg.surfaces.resize(1); + msg.surfaces[0].frame_id = "map"; + msg.surfaces[0].navcels.assign(nc, 0); + navmap_ros_interfaces::msg::NavMapLayer l; + l.name = "obstacles"; + l.type = 0; + l.data_u8.assign(nc, 0); + msg.layers = {l}; + rclcpp::SerializedMessage s; + rclcpp::Serialization().serialize_message(&msg, &s); + const auto real = s.get_rcl_serialized_message().buffer_length; + const auto estimate = navmap_ros::io::estimated_serialized_size(msg); + EXPECT_GE(estimate, real); + EXPECT_LE(estimate, real + 4096); +} + +TEST(NavMapIoMsg, AMapBeyondWhatAMessageHoldsIsRefusedAsTooLargeAndNothingIsWritten) +{ + // The real limit is 4 GiB (Fast-CDR): a lower one stands in for it, so that the test needs + // no gigabytes. Before, such a map got a bare EIO from deep inside the serialiser. + navmap_ros_interfaces::msg::NavMap msg; + msg.positions_x.assign(1000, 0.0f); + msg.positions_y.assign(1000, 0.0f); + msg.positions_z.assign(1000, 0.0f); + const std::string path = (std::filesystem::temp_directory_path() / + ("too_large_" + std::to_string(::getpid()) + ".navmap")).string(); + std::filesystem::remove(path); + navmap_ros::io::SaveOptions o; + o.max_serialized_bytes = 1000; + std::error_code ec; + EXPECT_FALSE(navmap_ros::io::save_msg_to_file(msg, path, o, &ec)); + EXPECT_EQ(ec, std::errc::file_too_large) << ec.message(); + EXPECT_FALSE(std::filesystem::exists(path)); + // the default limit takes it + EXPECT_TRUE(navmap_ros::io::save_msg_to_file(msg, path, {}, &ec)) << ec.message(); + std::filesystem::remove(path); +} + +TEST(NavMapIoMsg, SaveToFileEstimatesFromTheMapAndRefusesBeforeBuildingTheMessage) +{ + navmap::NavMap nm; + const std::size_t s = nm.create_surface("map"); + for (int k = 0; k < 20; ++k) { + const auto a = nm.add_vertex({static_cast(k), 0.0f, 0.0f}); + const auto b = nm.add_vertex({static_cast(k), 1.0f, 0.0f}); + const auto c = nm.add_vertex({static_cast(k) + 1.0f, 1.0f, 0.0f}); + nm.add_navcel_to_surface(s, nm.add_navcel(a, b, c)); + } + nm.add_layer("obstacles", "", "", 0); + nm.add_layer("elevation", "", "m", 0.0f); + + // the estimate from the map is that of its message: never below the real size, and close to it + rclcpp::SerializedMessage ser; + const auto msg = navmap_ros::to_msg(nm); + rclcpp::Serialization().serialize_message(&msg, &ser); + const auto real = ser.get_rcl_serialized_message().buffer_length; + const auto estimate = navmap_ros::io::estimated_serialized_size(nm); + EXPECT_GE(estimate, real); + EXPECT_LE(estimate, real + 4096); + + const std::string path = (std::filesystem::temp_directory_path() / + ("too_large_map_" + std::to_string(::getpid()) + ".navmap")).string(); + std::filesystem::remove(path); + navmap_ros::io::SaveOptions o; + o.max_serialized_bytes = estimate - 1; + std::error_code ec; + EXPECT_FALSE(navmap_ros::io::save_to_file(nm, path, o, &ec)); + EXPECT_EQ(ec, std::errc::file_too_large) << ec.message(); + EXPECT_FALSE(std::filesystem::exists(path)); + EXPECT_TRUE(navmap_ros::io::save_to_file(nm, path, {}, &ec)) << ec.message(); + std::filesystem::remove(path); +}