Skip to content

easynav_bigmap_maps_manager: a large NavMap, fine around the robot and coarse far from it - #183

Open
jmguerreroh wants to merge 9 commits into
EasyNavigation:rollingfrom
jmguerreroh:bigmap-tiled-lod
Open

jmguerreroh wants to merge 9 commits into
EasyNavigation:rollingfrom
jmguerreroh:bigmap-tiled-lod

Conversation

@jmguerreroh

@jmguerreroh jmguerreroh commented Nov 17, 2026 •

Copy link
Copy Markdown
Contributor

Replaces #180 with a different design. That PR's implementation (RTIN meshes of a DEM, about
15 000 lines) is not continued here: this one builds on NavMap's own tools and is about 2 000
lines.

Depends on EasyNavigation/NavMap#47 (keep_vertices, cut_along_lines, refine).
The rolling CI builds NavMap from rolling, so it will fail on this PR until that one is merged.

What

easynav_bigmap_maps_manager is a maps manager for NavMaps too big to plan on at full detail
(a warehouse, a town). The map is fine around the robot and coarse far from it. The obstacles
the sensors see are added near the robot and forgotten when it is far from them.

bigmap:
  plugin: easynav_bigmap_maps_manager/BigMapMapsManager
  package: my_maps
  navmap_path_file: maps/town.navmap
  cell_size: {min: 0.5, max: 8.0}   # NavCels at the obstacles near the robot / the largest
  min_radius: 15.0                  # within it, the obstacles have NavCels of cell_size.min
  max_radius: 60.0                  # from it, the map is as coarse as it gets
  forget_distance: 60.0             # sensed obstacles farther than this are forgotten
  filters: [inflation]              # the NavMap maps manager's filters

How

Once, at start. The NavMap is cut into tiles (cut_along_lines) and simplified into a
quadtree of levels of detail (simplify, with the corners and border junctions of each tile kept
via keep_vertices). simplify keeps the topology: no hole closes, no wall opens, however
coarse the level. This takes 8 s for the 2.5 M NavCels of the warehouse and 42 s for the 14 M of
2 x 2 km of Madrid (cells of 0.5 to 16 m), using every core only during this step.

Every cycle, when the robot has moved a few metres or the sensed obstacles changed (at most
twice a second):

  1. The coarsest tile each place allows is selected around the robot.
  2. The tiles are assembled and stitched, with no cracks.
  3. Within min_radius, the borders of the map and the sensed obstacles are refined (refine) to
    cell_size.min, out to the inflation radius.
  4. The filters run on the result.

Assembling takes 7 ms for the warehouse and 45–65 ms for the town; adding the detail takes
30–60 ms and 70–190 ms.

Sensed obstacles are the perception points between obstacle_min_height (which grows with
distance) and obstacle_max_height above the map's surface. Each cloud is placed on the map with
the TF at its stamp (with the latest pose, a cloud taken while the robot turns lands rotated and
makes up obstacles that, once out of range, are never cleared), and the heights are measured along
the surface's normal, correcting only the tilt the robot's TF lacks, so the ramp ahead is ground
going down a ramp. Points next to a hole of the map are
the map's own obstacles and are ignored. They are kept as an occupancy memory in log-odds, as the
NavMap maps manager's obstacle filter: a point on a cell is a hit, a ray from the robot through it
a miss (memory.clearing, memory.p_hit, memory.p_miss, optional memory.decay_time), so
an obstacle that moves away stops blocking; they are forgotten beyond forget_distance.

Results

2 x 2 km around the Puerta del Sol, Madrid (ground from the IGN, buildings from the Catastro;
13.8 M NavCels at 0.5 m). 30 routes of 230 to 1481 m, with easynav_navmap_planner's A*, the same
safety band on every map, and the robot replanning every 30 m (the city_routes campaign of
bigmap_experiments):

Map NavCels MB Success Through a building Length A* per plan NavCels expanded
full, 0.5 m 13,765,636 321 30/30 0 1.000 354 ms 1,166,402
grid 1 m 3,275,294 77 21/30 0 1.006 59 ms 190,060
grid 2 m 744,768 18 28/30 0 1.008 17 ms 71,510
grid 4 m 156,118 3.9 13/30 0 1.354 6.9 ms 24,202
full map simplified once (0.4 m) 393,251 10.3 30/30 0 1.022 14 ms 35,082
bigmap ~135,000 3.5 30/30 0 1.007 5.1 ms 8,528

Paths within 1 % of those on the full map, planned 69 times faster; the map and the planner's
graph are made again in 263 ms every 30 m.

In Gazebo (Summit XL with its 3D lidar, RPP, A*, the true pose instead of a localization), against
the NavMap maps manager on the same map with its obstacle filter, three runs of each:

World Map Goals Route EasyNav memory EasyNav CPU
large_warehouse_world (700 x 450 m, 2.5 M NavCels) NavMap 24/24 490 m 1,400 MiB 13 %
bigmap 24/24 496 m 234 MiB 15 %
Madrid, 500 x 500 m (707 k NavCels) NavMap 12/12 780 m 509 MiB 45 %
bigmap 12/12 789 m 191 MiB 42 %
warehouse with 7 boxes not in the map NavMap 24/24 493 m 1,395 MiB 11 %
bigmap 24/24 504 m 228 MiB 13 %

No stall and no contact (closest 0.94 m from the robot's centre). A rebuild there takes 10–21 ms to
assemble, 20–37 ms to add the detail and 9–18 ms for the inflation (about 31 k NavCels). Floors one
over another are not stitched together: in a three-floor parking garage the robot went up and down
the ramps and reached every goal on its floor; in two of three runs, though, sensed obstacles at
the top of the down ramp closed it for a while and the robot detoured (28–65 m). Not diagnosed yet.

Also in this PR: the NavMap maps manager's obstacle filter had the same problem with the
latest pose (turning in place for 40 s at 0.8 rad/s in the warehouse made up to 27 lethal cells), and
now also fuses each cloud with the TF at its stamp (fuse(..., true)).

Tests

  • test_tiled_lod: the levels, no cracks between tiles of different levels, holes and narrow
    passages kept far away, floors one over another kept apart, and layers kept.
  • test_bigmap_plugin: the plugin as EasyNav runs it:
    • the map is fine at the borders near the robot and coarse far away;
    • it follows the robot;
    • a sensed obstacle is added fine, inflated, and forgotten far away;
    • an obstacle that moves away is cleared; without clearing it is remembered; with
      decay_time one not seen again fades;
    • going down a ramp, the ramp ahead is not an obstacle;
    • the published map carries the filters' layers, and the levels are drawn as markers.

colcon test --packages-select easynav_bigmap_maps_manager on this branch, with the NavMap PR:
55 tests (with the linters), no failures; easynav_navmap_maps_manager, 144 tests, no failures.

…d coarse far from it

The NavMap is cut into tiles and simplified into a quadtree of levels of detail once; every
cycle the coarsest tile each place allows is put together around the robot (stitched, no
cracks), the borders of the map and the sensed obstacles near it refined as far as the
inflation reaches, and the sensed obstacles forgotten beyond forget_distance. Configuration:
the map, cell_size min / max, min_radius, max_radius, forget_distance and the filters.

Needs navmap_core's keep_vertices, cut_along_lines and refine (NavMap PR).
- Tiles kept without the NavMap accelerations (BVH, adjacency, locate grids), the map cut in
  place instead of copied, each tile taken from it just before it is simplified, and the memory
  the building threads leave free given back (malloc_trim): 40 to 70 % less resident memory.
- rebuild_period (default 0.5 s), and the README of the advanced parameters.
…together

Stitching inserted into a coarse edge on a grid line every vertex of the other side's tiles
within 0.5 m plus half the coarsest cell in height (8.5 m with cells up to 16 m), so on maps with
floors over each other a NavCel of one floor could be split to reach a vertex of another (in the
parking garage, the roof became reachable from the floor below). Now a vertex goes, on each side
of the line, into the covering edge nearest in height, and into an edge of another chain of the
line only if it is within 0.5 m of it in height. Maps with one floor give the same map as
before; test FloorsOverEachOtherStaySeparate.
…down; no layer updates before the map

Sensed points were placed at the height of the ground under the robot plus their height in the
footprint frame, which is flat in TF (2D odometry) while the robot stands tilted on a ramp: going
down, the ramp ahead looked higher than the map there, was taken for an obstacle and blocked the
ramp (in the parking garage the robot went back and forth between the two ramps down). The points
are now turned by the tilt of the NavCel under the robot; test
GoingDownARampTheRampAheadIsNotAnObstacle.

Layer updates (map_updates) are not published while a rebuilt map waits to be published: they
were of a geometry the subscribers did not have yet (RViz: "belongs to another geometry").
…away

The sensed obstacles were remembered until the robot was forget_distance from them, so one that
moved (a person crossing a corridor) left a trail of obstacles that could close passages around
the robot. They are now an occupancy memory in log-odds, as the NavMap maps manager's obstacle
filter: a point on a cell is a hit, a ray from the robot through it a miss (memory.clearing,
memory.p_hit, memory.p_miss), and memory.decay_time forgets what is not seen again. The rays
cross a cell if they pass within half a metre of its height. Tests:
AnObstacleThatMovesAwayIsCleared, WithoutClearingAnObstacleIsRemembered,
AnObstacleNotSeenAgainDecays.
…n at; the cost of a cycle

The perceptions were taken to the footprint frame and placed on the map with the robot's pose of
now: a cloud taken while the robot turned in place landed rotated, walls seen a fraction of a
second earlier fell on free ground as obstacles, and those behind racks, out of sight once the
robot stopped, were never cleared (in the large warehouse the robot stayed boxed in after its
first goal). They are now taken to the map frame with TF at their stamps; the tilt correction on
ramps turns each point about the robot as before.

For measurements: every cycle the NavState gets "map.bigmap_stats" (rebuilds so far, the last
rebuild's select, assemble and detail times and NavCels, this cycle's sensing and filter times),
also published on <node>/<plugin>/stats while someone listens.
With the perceptions taken to the map frame by TF, a localizer that gives the robot's pose in 3D
(its pitch on a ramp) already tilts them: the correction for a flat TF was applied twice and the
ramps were taken for obstacles again (the parking garage with a 3D ground truth). The points are
now turned from the robot's up in TF to the normal of the ground under it.
BigMap and the NavMap obstacle filter placed every cloud on the map with the robot's latest pose.
A cloud may be up to the sensors' forget_time (0.5 s) old and is used in every cycle until the next
one arrives, so while the robot turned in place its points landed rotated: walls 14 m away fell
across free aisles as lethal obstacles, and once the robot left, out of the sensor's range, they
were never cleared (in the large warehouse a robot on the fixed NavMap stayed boxed in). Both now
fuse each perception with the TF at its stamp. Turning in place at 0.8 rad/s for 40 s in the
warehouse, where the map has every obstacle: BigMap remembered 594 made-up obstacles before and 0
after; the NavMap filter peaked at 27 cells before and 2 after.

This branch has not been deployed

No deployments
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.

1 participant