Skip to content

NavMap filters: incremental, in place, and sparse layer updates - #124

Merged
fmrico merged 1 commit into
rollingfrom
navmap_incremental_filters
Oct 15, 2026
Merged

fmrico merged 1 commit into
rollingfrom
navmap_incremental_filters

Conversation

@fmrico

@fmrico fmrico commented Oct 15, 2026

Copy link
Copy Markdown
Contributor

Closes #123

  • In place. The filters update the NavMap in NavState through get_ptr instead of copying it in and out.
  • ObstacleFilter. It rebuilds its layer from the static map only when the map or its static layer changes. After that, it writes just the cells that become obstacles or stop being one.
  • InflationFilter. It keeps an exact navmap::DistanceField, updated with the obstacle cells that appear or disappear, and rewrites only the cells whose cost changes. inflate_layer_u8 uses the exact field too.
  • Maps manager publishing.
    • Each update carries only the changed cells, as a sparse NavMapLayer, or the whole layer if many changed.
    • Updates go out only when there are subscribers, and a new subscriber gets the whole layers.
    • The recorded changes are cleared at the end of each cycle.
  • Fixes. NavMapFilter gets a virtual destructor, since plugins are deleted through it. The README now gives the obstacle filter's real plugin name, ObstaclesFilter.

Time per cycle, with the robot moving and a 720-beam laser that sees the map's walls and a person walking around:

Map NavCels ObstacleFilter InflationFilter
home2, 5 cm 169 796 6.7 → 0.25 ms 13.8 → 0.66 ms
warehouse, 5 cm 297 012 11.8 → 0.24 ms 26.9 → 2.7 ms
parking garage 38 543 0.40 → 0.30 ms 1.20 → 0.01 ms

Two costs to know about:

  • The first cycle of a map is slower, because the exact field paints a disc around every obstacle cell: 67 ms instead of 25 ms on home2.
  • A scene where hundreds of cells next to dense static walls change every cycle is slower too: inflation takes 30 ms instead of 15 ms on home2.

New tests check that:

  • a moving obstacle changes only its cells, and repeating a scan changes nothing;
  • a new map rebuilds the layers;
  • incremental inflation equals a full recomputation, and records exactly the cells that changed, over 40 random cycles;
  • the maps manager publishes the whole layer to a new subscriber, nothing when nothing changes, and only the changed cells otherwise.

The simulation battery gives the same results in summit_warehouse_amcl, summit_bonxai_amcl and tiago_navmap_bonxai_amcl.

The filters update the NavMap in NavState in place instead of copying
it twice each cycle. ObstacleFilter rebuilds its layer from the static
map only when the map changes, and then writes just the cells that
become obstacles or stop being one. InflationFilter keeps an exact
navmap::DistanceField updated with the obstacle cells that come and go,
and rewrites only the cells whose cost changes. The maps manager
publishes sparse layer updates, only with subscribers (whole layers for
a new one), and clears the recorded changes at the end of each cycle.

NavMapFilter gets a virtual destructor: plugins are deleted through it.
README: the obstacle filter's plugin name is ObstaclesFilter.

Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>
@fmrico
fmrico merged commit 7128780 into rolling Oct 15, 2026
1 check passed
@fmrico
fmrico deleted the navmap_incremental_filters branch October 15, 2026 21:01
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

NavMap filters copy the whole map and recompute every layer each cycle

1 participant