From 4a2823a9f6428e26d837e55c62bb58a09cee5c13 Mon Sep 17 00:00:00 2001 From: Jose Miguel Date: Tue, 3 Nov 2026 23:37:38 +0100 Subject: [PATCH] navmap_core, navmap_ros: fixes for large maps (DistanceField, set_area, operator=, save size limit) --- navmap_core/include/navmap_core/NavMap.hpp | 18 +++--- navmap_core/src/navmap_core/DistanceField.cpp | 60 ++++++++++--------- navmap_core/src/navmap_core/NavMap.cpp | 5 ++ navmap_core/tests/test_incremental.cpp | 54 ++++++++++++++++- navmap_core/tests/test_navmap_assign.cpp | 23 +++++++ navmap_core/tests/test_navmap_easy.cpp | 27 +++++++++ navmap_ros/include/navmap_ros/navmap_io.hpp | 10 ++++ navmap_ros/src/navmap_ros/navmap_io.cpp | 39 ++++++++++++ navmap_ros/tests/test_navmap_io.cpp | 49 +++++++++++++++ 9 files changed, 250 insertions(+), 35 deletions(-) diff --git a/navmap_core/include/navmap_core/NavMap.hpp b/navmap_core/include/navmap_core/NavMap.hpp index f07f26a..7f9df82 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 @@ -1172,7 +1173,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 +1257,10 @@ class NavMap } }; - std::vector visited(navcels.size(), 0); + // visited: only the NavCels of the area (a per-map vector cost O(map) on every call) + std::unordered_set visited{start_cid}; std::deque q; q.push_back(start_cid); - visited[static_cast(start_cid)] = 1; auto & data = layer->mutable_data(); @@ -1269,14 +1274,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 (visited.insert(ncid).second) {q.push_back(ncid);} } } diff --git a/navmap_core/src/navmap_core/DistanceField.cpp b/navmap_core/src/navmap_core/DistanceField.cpp index 76c8735..2ffcf0d 100644 --- a/navmap_core/src/navmap_core/DistanceField.cpp +++ b/navmap_core/src/navmap_core/DistanceField.cpp @@ -140,28 +140,34 @@ std::int64_t DistanceField::bucket_key(int bx, int by, int bz) (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, so within one bucket of an indexed one: the +// queries look one bucket further (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_))); + std::vector keys; + 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();} } } } @@ -270,15 +276,15 @@ void DistanceField::update( 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) + // one bucket more on every side: a segment seed is indexed by samples along it (index_seed) + for (int bx = static_cast(std::floor(lo.x())) - 1; + bx <= static_cast(std::floor(hi.x())) + 1; ++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())) - 1; + by <= static_cast(std::floor(hi.y())) + 1; ++by) { - for (int bz = static_cast(std::floor(lo.z())); - bz <= static_cast(std::floor(hi.z())); ++bz) + for (int bz = static_cast(std::floor(lo.z())) - 1; + bz <= static_cast(std::floor(hi.z())) + 1; ++bz) { const auto it = buckets_.find(bucket_key(bx, by, bz)); if (it == buckets_.end()) {continue;} diff --git a/navmap_core/src/navmap_core/NavMap.cpp b/navmap_core/src/navmap_core/NavMap.cpp index c4bc85c..fee3791 100644 --- a/navmap_core/src/navmap_core/NavMap.cpp +++ b/navmap_core/src/navmap_core/NavMap.cpp @@ -144,6 +144,11 @@ 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 (re-created by the source): replaced, not + // synchronised (add_or_get would give no view for the new type) + if (const auto old = layers.get(name); old && old->type() != src_base->type()) { + layers.remove(name); + } switch (src_base->type()) { case LayerType::U8: { diff --git a/navmap_core/tests/test_incremental.cpp b/navmap_core/tests/test_incremental.cpp index c6e01d0..a7b750d 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,46 @@ TEST(DistanceFieldIncremental, AnUpdateOnlyTouchesItsNeighbourhood) } } +TEST(DistanceFieldIncremental, LongDiagonalOpenEdgesCostAboutAsMuchAsShortOnes) +{ + // 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, the 16 m one took ~600 + // times longer; sampled along them, it is within a small factor. + 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 seconds = [](const NavMap & nm) { + double best = 1e9; + for (int k = 0; k < 3; ++k) { + const auto t0 = std::chrono::steady_clock::now(); + DistanceField df(nm, true, 0.6f); + df.compute({}); + best = std::min( + best, std::chrono::duration(std::chrono::steady_clock::now() - t0).count()); + } + return best; + }; + const NavMap fine = build(0.5f), coarse = build(16.0f); + const double t_fine = seconds(fine), t_coarse = seconds(coarse); + EXPECT_LT(t_coarse, 25.0 * t_fine + 0.01) << "fine " << t_fine << " s, coarse " << t_coarse; +} + // ------------------------------------------------------------------ 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..2cfca5a 100644 --- a/navmap_core/tests/test_navmap_assign.cpp +++ b/navmap_core/tests/test_navmap_assign.cpp @@ -284,3 +284,26 @@ 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); +} 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..f32d701 100644 --- a/navmap_ros/include/navmap_ros/navmap_io.hpp +++ b/navmap_ros/include/navmap_ros/navmap_io.hpp @@ -61,8 +61,18 @@ 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 before anything is + /// allocated, with std::errc::file_too_large (about 180 million NavCels with one layer). + 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 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..c077279 100644 --- a/navmap_ros/src/navmap_ros/navmap_io.cpp +++ b/navmap_ros/src/navmap_ros/navmap_io.cpp @@ -53,6 +53,33 @@ inline void set_error(std::error_code * ec, int ev = EIO) } // namespace +std::uint64_t estimated_serialized_size(const navmap_ros_interfaces::msg::NavMap & msg) +{ + // each sequence: its elements (with alignment) and a 4-byte length; strings and headers: a + // generous fixed allowance each + auto seq = [](std::size_t n, std::size_t elem) { + return static_cast(n) * elem + 4 + 8; + }; + constexpr std::uint64_t kSmall = 512; + 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; +} + bool save_msg_to_file( const navmap_ros_interfaces::msg::NavMap & msg, const std::string & path, @@ -60,6 +87,18 @@ 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) + const std::uint64_t estimate = estimated_serialized_size(msg); + if (estimate > options.max_serialized_bytes) { + 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; + } try { rclcpp::SerializedMessage serialized; rclcpp::Serialization ser; diff --git a/navmap_ros/tests/test_navmap_io.cpp b/navmap_ros/tests/test_navmap_io.cpp index e3dc5f4..29e9cb5 100644 --- a/navmap_ros/tests/test_navmap_io.cpp +++ b/navmap_ros/tests/test_navmap_io.cpp @@ -315,3 +315,52 @@ 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); +}