r1pro-nav-lio: 3D navigation on Point-LIO with the head's depth for the floor - #4231
Draft
jeff-hykin wants to merge 14 commits into
Draft
jeff-hykin wants to merge 14 commits into
jeff-hykin wants to merge 14 commits into
Conversation
The R1 Pro's voxel map was silently mapping from half its sensors. Every cloud
it dropped was logged as a warning, never an error, so the run looked healthy.
What happens. The map's input is a fan-in bus: a 9.3 Hz chassis lidar and a
4.5 Hz head stereo cloud arrive on the same port, each carrying its own
frame_id. Registering all of that costs more than the Jetson has spare beside
the vendor ROS stack -- load average 21 on 8 cores -- so a backlog builds.
Measured from the module's own log timestamps, cloud staleness at the handler
climbs from startup and then sits flat:
lag=1.38 lag=1.72 lag=1.90 lag=2.42 lag=13.33
lag=10.43 lag=10.45 lag=10.45 lag=10.38 (rest of the run)
It parks at 10.4 s because DEFAULT_TF_WINDOW_SECS is 10: past that the
transforms at the cloud's stamp have been pruned, the cloud is dropped for want
of one, and dropping is cheap -- so the queue drains back to the edge of the
window and refills. Self-stabilizing, at 1032 of 2537 lidar clouds thrown away.
None of the stages upstream look wrong, which is what made this hard to see. The
connection reports `lidar: rx=9.3/s drop=0.0/s age=0.1s`. The Livox header
stamps are a steady +0.104 to +0.112 s behind wall clock. chassis_speed, which
drives the tf edge, is 50 Hz at 0.002 s lag with no dt anomalies. The input
channel never reports itself full.
max_cloud_rate_hz thins at the source so the map stays current, which is what a
costmap is for. The cap is per frame_id so a fast sensor cannot starve a slow
one sharing the port, and it gates on the cloud stamp rather than the clock so a
replay thins the same frames the live run did. A stamp that goes backwards --
a replay looping, a sensor resetting its clock -- restarts the window instead of
blocking every cloud until the old stamp is passed again. Zero, the default,
keeps the old behaviour.
Set to 5 Hz on r1pro-kronknav, alongside max_range 15 -> 10 m and ray_subsample
5 -> 10. Both cuts were needed: emit_every 1 -> 5 on its own changed nothing
(258 drops in four minutes, staleness still parked at 10.4), so the cost is in
the ray tracing itself, not in publishing the map.
64 tests pass, clippy clean:
nix develop "path:." -c cargo clippy -p dimos-voxel-ray-tracing --all-targets -- -D warnings
Claude-Session: https://claude.ai/code/session_01ExBQYh7BJX5bnGHTKpNVDy
(cherry picked from commit 8fe35bf9f1c62b45beef352d6df3efca30b4d9a0)
codec_for() routes every Image to JpegCodec, depth included. A float32 metre map cannot be JPEG-encoded at all — turbojpeg raises "Invalid shape for image data", so recording head_depth was impossible — and a uint16 one would be rescaled to 8 bits and lossy-compressed, quietly destroying the metric values a cloud is unprojected from. Depth formats now store via lcm_encode, and decode goes through Image.lcm_decode, which dispatches on the envelope's own encoding so existing recordings still read. base.py is deliberately left alone: type-only dispatch cannot tell colour from depth. Claude-Session: https://claude.ai/code/session_01ExBQYh7BJX5bnGHTKpNVDy (cherry picked from commit 11da110) (cherry picked from commit acf967da813bb36fc337dc4d2c148f9ad3099ffe)
… the head for the floor The nav stack the R1 was mapped with all week, productionized: the robot is placed by Point-LIO (r1pro-pointlio), the head's stereo depth (r1pro-stereo) is cut to the band the Mid-360 cannot see -- from a little below the floor to just above the lidar's own plane, since the lidar sees only 7 degrees below it and the floor enters its view 2.4 m out -- and both clouds feed one ray-traced voxel map that the MLS planner plans over and the holonomic local planner and controller drive. The head's points above the lidar's plane are dropped on purpose: the lidar measures them better. r1pro-nav-lio-replay drives the same stack from a recording (R1ProReplay stands in for the connection, same stream names and frames). The voxel map's per-source registration cap and the lossless depth storage the replay needs come along as their own commits.
… time The global map was due every Nth accepted cloud, so anything that slowed the input slowed the map by the same factor: on the 2026-09-16 R1 run the input fell to 1.5 Hz, the 63rd global publish came due at frame 3150 and the run ended at 3117. It read as the global map dying and nothing had failed. global_max_interval_s bounds that in cloud-stamp seconds -- cloud time, not wall clock, so a replay emits on the cadence of the run it replays. Off by default: where the input is slow because the global emit is what is expensive, forcing it more often makes that worse, so a robot opts in.
❌ 1 Tests Failed:
View the full list of 1 ❄️ flaky test(s)
To view more test analytics, go to the Test Analytics Dashboard |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
The R1 nav stack on lidar-inertial odometry: Point-LIO's scan plus the head's stereo depth cut to the lidar's blind band, into a ray-traced voxel map, the MLS planner, and the holonomic local planner, with a replay variant.