[jmguerreroh]

navmap_core / navmap_ros: four fixes found by benchmarking large maps

Branch base: rolling. Files: navmap_core (DistanceField, NavMap::set_area, NavMap::operator=)
and navmap_ros (navmap_io). Independent of the navmap_tools PR; neither needs the other.

These came out of a comparison of NavMap and a level-of-detail maps manager on maps of 100 to
10 000 cells per side (50 m to 5 km at 0.5 m). Each one is a separate commit-sized change with its
own regression test.

1. DistanceField: indexing a seed cost the square of its length

DistanceField::index_seed put a segment seed (an open edge of the mesh) into every bucket of its
bounding box. For an axis-aligned edge that is a line of buckets, but for a diagonal one it is a
square: an edge of 16 m at 45° filled ~1 000 buckets instead of ~40, and every query near it then
scanned them. Meshes with long diagonal open edges (coarse NavCels of a level-of-detail map, a
rotated map with holes) spent seconds in compute().

Now a seed is indexed by points sampled along it every bucket_ metres (deduplicated), and the
queries in update() look one bucket further on each side, so no seed within reach is missed
(every point of the segment is within half a bucket of a sample).

Measured (the same mesh, 22 944 NavCels, 9 586 open edges, with legs of 0.5 to 16 m; time of
compute()). Rotated 45°, before / after:

leg	before	after	(axis-aligned, after)
0.5 m	2.9 ms	2.4 ms	3.2 ms
4 m	39.0 ms	6.7 ms	8.0 ms
8 m	303.4 ms	13.3 ms	16.8 ms
16 m	1 705.1 ms	34.6 ms	36.0 ms

A diagonal mesh now costs what an axis-aligned one does (linear in the edge length, as the
number of NavCels it touches).

Test: test_incremental gets a fourth map in its equivalence sweep (4 m cells with holes, rotated
30°: the incremental field must equal the full recomputation there too), and
LongDiagonalOpenEdgesCostAboutAsMuchAsShortOnes (the 16 m mesh within 25× the 0.5 m one; it was
~600×).

2. NavMap::set_area: O(map) per call, and its writes were invisible to incremental filters

• The BFS kept std::vector<char> visited(navcels.size()): writing a 0.5 m disc on a map of
  50 M NavCels allocated and zeroed 50 MB per call. It is now an unordered_set of the NavCels the
  area actually visits: the cost is the area's.
• It wrote mutable_data() directly, so the layer's change list (changed(), used by the
  incremental InflationFilter of EasyNav) did not see the new values: an obstacle written with
  set_area was not inflated until something else marked the layer. Every NavCel written is now
  recorded with mark_changed, as layer_set does.

Test: NavMap_SetArea_Systematic.TheWrittenNavCelsAreRecordedAsChangedForIncrementalConsumers
(the change list equals exactly the NavCels written). The existing set_area tests are unchanged and
pass.

3. NavMap::operator=: crash when a layer changed type

When the destination already had a layer of the same name but another type (the source removed it
and re-created it, e.g. U8 → F32), operator= asked add_or_get<T> for a view of the new type,
got nullptr and dereferenced it. This happens in EasyNav when a node copies the map from the
NavState, re-creates a layer with another type and sets it back. The old layer is now removed
before the copy when the types differ.

Test: NavMapAssign.ALayerReCreatedWithAnotherTypeIsReplacedNotDereferencedAsNull (U8 → F32 and
back). It segfaulted before the fix.

4. navmap_io: a map too big for one CDR message failed with a bare I/O error

Fast-CDR cannot serialise more than 4 GiB. save_msg_to_file / save_to_file of a bigger map
(about 180 M NavCels with one layer) allocated gigabytes, then failed deep inside the serialiser
with a non-std exception that was reported as EIO. Now:

• SaveOptions::max_serialized_bytes (default 4 GiB − 1 MiB) and
  estimated_serialized_size(const msg::NavMap &), a close upper estimate computed from the array
  sizes, without serialising;
• the saver refuses a map above the limit before allocating anything: std::errc::file_too_large
  (EFBIG) and a log line with the estimated size. Nothing is written.

Tests: NavMapIoMsg.TheSizeEstimateIsCloseAboveTheSerialisedSize (above the real size by less than
a few KB), NavMapIoMsg.AMapBeyondWhatAMessageHoldsIsRefusedAsTooLargeAndNothingIsWritten (a small
limit, so the test needs no 4 GB map).

No API is removed or changed in a breaking way: one field added to SaveOptions (with the old
behaviour's effective limit as default) and one free function added.

Tests

colcon test --packages-select navmap_core navmap_ros

navmap_core: test_incremental (8), test_navmap_easy (19), test_distance_field (8),
test_navmap_assign (7) and the rest of the suite pass; navmap_ros: test_navmap_io (6) and the
rest pass. Built and run on ROS 2 Rolling, Ubuntu 24.04.

[fmrico]

Thanks, the four fixes look right and each has its regression test. I checked the sampled seed indexing: every point of a segment is within bucket_/2 of a sample, update() is the only query on buckets_ and it widens the range by one bucket, and removal recomputes the same keys, so no seed is missed. A few points below, the first one the most relevant.

[fmrico]

This fixes operator=, but the root cause is in LayerRegistry::add_or_get: when the layer exists with another type it returns nullptr, and every caller dereferences it. layer_set, layer_clear, add_layer and the other add_or_get callers in NavMap.hpp still crash the same way, e.g. nm.layer_set<float>("obstacles", 0, 1.0f) on a uint8 layer. The doc of layer_set even says "creates/retypes the layer if needed".

Could the replacement go into add_or_get itself (replace the layer when the type differs)? Then operator= needs no special case and all callers are covered.

[jmguerreroh]

Done: LayerRegistry::add_or_get now replaces a layer that exists with another type, so every caller is covered and the special case in operator= is gone. Also fixed add_layer<T>(..., default), which kept zeros instead of the default on a replaced layer. Test: EveryWriterReplacesALayerOfAnotherType.

[fmrico]

"refused before anything is allocated" holds for save_msg_to_file, but save_to_file first builds the whole message with to_msg(map), so for a 180 M-NavCel map the gigabytes are already allocated (and may run out of memory) before the check. Either estimate the size from the NavMap in save_to_file before to_msg, or adjust the doc.

[jmguerreroh]

save_to_file now estimates the size from the NavMap (estimated_serialized_size(const NavMap &)) and refuses before to_msg; the doc says where each saver refuses. Test: the estimate is never below the real size, and nothing is written when refused.

[fmrico]

Right for the usual case (a small disc on a huge map), but an area covering most of the map now costs ~40-60 bytes plus a hash insert per visited NavCel instead of 1 byte: on 50 M NavCels that is 2-3 GB instead of 50 MB. A member buffer reused with an epoch counter (as the planner does in easynav_plugins#178) keeps O(area) work for both cases.

[jmguerreroh]

A hash set while the area is small, switching to one flag per NavCel once it exceeds 1/32 of the map: O(area) work, at most ~2.5 bytes per NavCel in the worst case. I did not use a member buffer with an epoch because the method is const (not thread-safe) and the buffer would be copied with the map. Test with small and large areas.

[fmrico]

Minor: a wall-clock ratio may be flaky in Debug, coverage or sanitizer builds, even with the best of 3. Counting the bucket entries per seed would test the same thing deterministically.

[jmguerreroh]

Now it counts the index entries (new index_entries()) instead of timing, so it is deterministic.

[fmrico]

Minor: one extra bucket on every side, z included, goes from 5³ to 7³ lookups per removed seed. Widening lo/hi by 0.5f * bucket_ gives the same guarantee and often fewer buckets. Also, index_seed allocates a new std::vector per call (~10 k in compute()); a reused scratch buffer would avoid it.

[jmguerreroh]

The range is widened by 0.51 * bucket instead of one bucket per side (the extra 0.01 covers the rounding of the samples), and index_seed reuses a scratch buffer.