Skip to content

feat(mapping): consolidate canonical CDR navigation and map algorithms - #4737

Draft
TomCC7 wants to merge 8 commits into
cc/feat/cdr-unit-perceptionfrom
cc/feat/cdr-unit-mapping
Draft

TomCC7 wants to merge 8 commits into
cc/feat/cdr-unit-perceptionfrom
cc/feat/cdr-unit-mapping

Conversation

@TomCC7

@TomCC7 TomCC7 commented Oct 20, 2026 •

Copy link
Copy Markdown
Member

Migrate mapping and navigation messages to generated CDR types while keeping the existing algorithms. This includes occupancy grids, clouds, paths, goals, followers and explicit map region IDs.

Base: #4734. Region definitions and their tests stay here. Native mapper, MLS, local planner and trajectory follower now bind public SDK encoders and the shared integer-nanosecond TF adapters from #4774. Delete duplicate TF conversions and codec-only wrappers; retain real pointcloud layout conversion. Nix builds pin the corrected SDK source and include the independent message-adapter crate.

Final combined-stack validation: mapper 102 tests, MLS 135, local planner 93, follower 63. Six real Python/native integration tests pass with generated clouds, regions and paths. Source staging from the actual pinned archive passes locked Cargo dependency checks; full Nix builds were not run. The broader Python mapping suite and whole-graph cutover were not rerun for this codec change.

Existing algorithm and boundary review: #4737 (comment)

CDR dependencies and TF conventions

The stack consumes published dimos-cdrgen 0.1.2 and the immutable CDR library fix. SDK #4774 now uses standard ROS TFMessage, TransformStamped and integer Time, with no per-driver TF codec callbacks or parallel public transform type. The external transforms library, validated by its complete std/no_std/serde and ARM verification gate, owns the frame tree, common-time lookup, interpolation and static history. Its atomic batch patch is pinned to 5ae9563de0c02638c04e393aadfad28444a4398e.

Combined-stack local validation: SDK 121, macros 22, mapper 101, MLS 135, local planner 91, follower 63, V4L2 6 and depth2depth 12 Rust tests; all 24 native/DimSim/port-contract acceptance tests pass without skips. The mapper exchange includes Python CDR tf_static input; the follower exchange covers drive, arrival, a new goal and empty-path stop. Missing native binaries fail the dedicated job. RealSense passes cargo check --all-targets --features docs-only; device linking and hardware execution were not tested. Seven real source-staging layouts pass locked Cargo resolution. Native acceptance for tip cc463c163aed465f7c1d6808165bf640820b0182: workflow run. These are combined-tip results, with the SDK additionally tested standalone at its own layer.

Native Nix inputs use verified source archive f7723ad4d5b9aa5dfe63614b6b537d1ccc2d212e, SHA256 f90af8fa794a34d53ee9481c6d0bdb63138853b30f851d92960fd34224ff62b2. Full Nix builds remain unverified. SDK static edges persist after receipt; LCM/Zenoh do not acquire DDS transient-local durability from CDR. Existing Python mount publishers retain dynamic tf compatibility. Legacy modules remain explicitly pinned to SDK 0.1.0.

Stack #4776 preserves existing history and PR bases. Current main still has conflicts with the original stack; this change does not rebase onto main. No DimOS PR has been merged.

@TomCC7

TomCC7 commented Oct 21, 2026 •

Copy link
Copy Markdown
Member Author

Review report

Updated: 2026-10-21. Direct predecessor: #4734.

  • Head: f46ed9ce6b23a3c4ec1a48adaeeeef1eb4eca9b1
  • Named base branch: cc/feat/cdr-unit-perception at f59b609c27f5cc62124a438e7890c15e1fc8f38c
  • PR-recorded base: f59b609c27f5cc62124a438e7890c15e1fc8f38c; diff merge base: f59b609c27f5cc62124a438e7890c15e1fc8f38c

What changes and why the diff is large

Migrate mapping/navigation callers from wrapper-owned numerical methods to generated messages plus explicit helpers. The unit covers occupancy grids, map storage/CLI, relocalization, graph/loop closure, A* safety/planning, native planners and followers; it also adds explicit region definitions.
Most files migrate consumers of wrapper-owned numerical methods. Occupancy and pointcloud extensions now use the shared message_utils namespace; role totals are shown below.

Review map

Suggested entry points: dimos/message_utils/occupancy.py, dimos/message_utils/pointcloud.py, packages/dimos-generated/custom/dimos_msgs/msg/RegionBounds.msg, dimos/mapping/cli/map.py, dimos/navigation/go2/replanning_a_star/global_planner.py.
All 145 changed files are accounted for below. Grouping is by file role, not a claim that the implementation is minimal.

Complete file map

Implementation (85)

File Change Purpose
dimos/mapping/cli/map.py modified Use generated TransformStamped, CameraInfo, Image in _camera_info_static, _accumulate.
dimos/mapping/cli/pose_fill.py modified Use generated Transform, Quaternion, Vector3 in pose_fill, pose_fill_db.
dimos/mapping/cli/replay.py modified Use generated PointCloud2, PoseStamped, Odometry in _log_clouds, main.
dimos/mapping/cli/replay_marker.py modified Use generated Image, PointCloud2 in main.
dimos/mapping/cli/view.py modified Use generated PointCloud2 in main.
dimos/mapping/costmapper.py modified Use generated OccupancyGrid, PointCloud2 in _calculate_costmap, _apply_initial_safe_radius.
dimos/mapping/occupancy/extrude_occupancy.py modified Adapt identify_convex_shapes, generate_mujoco_scene for this unit's message/value flow.
dimos/mapping/occupancy/gradient.py modified Adapt gradient, voronoi_gradient for this unit's message/value flow.
dimos/mapping/occupancy/inflation.py modified Adapt simple_inflate for this unit's message/value flow.
dimos/mapping/occupancy/operations.py modified Use generated Point, OccupancyGrid in _grid_values, _grid_coordinates.
dimos/mapping/occupancy/path_map.py modified Adapt make_navigation_map for this unit's message/value flow.
dimos/mapping/occupancy/path_mask.py modified Use generated PoseStamped, OccupancyGrid, Path in make_path_mask.
dimos/mapping/occupancy/path_resampling.py modified Use generated Point, Pose, PoseStamped in _pose, _new_pose.
dimos/mapping/occupancy/visualizations.py modified Use generated PoseStamped, OccupancyGrid, Path in _map_image, visualize_occupancy_grid.
dimos/mapping/occupancy/visualize_path.py modified Use generated PoseStamped, OccupancyGrid, Path in visualize_path.
dimos/mapping/odometry_hist.py modified Use generated PoseStamped, Odometry, Path in path_at_true_height, __init__.
dimos/mapping/pointclouds/occupancy.py modified Use generated Point, Pose, Quaternion in _cloud_points, _legacy_stamp.
dimos/mapping/ray_tracing/module.py modified Declare/use generated PointCloudRegion, RegionBounds, PointCloud2 at this boundary.
dimos/mapping/ray_tracing/rust/src/lib.rs modified Update native lib message/TF handling; review the wire and field changes with its Python caller.
dimos/mapping/ray_tracing/rust/src/module.rs modified Update native module message/TF handling; review the wire and field changes with its Python caller.
dimos/mapping/ray_tracing/rust/src/wire.rs added Update native wire message/TF handling; review the wire and field changes with its Python caller.
dimos/mapping/ray_tracing/transformer.py modified Use generated PointCloud2 in pose_from_tf, attach.
dimos/mapping/ray_tracing/utils/loaded_map.py modified Use generated PointCloud2 in place_loaded_map.
dimos/mapping/ray_tracing/viz.py modified Use generated PointCloudRegion, PointCloud2 in render_voxel_map, render_map_region.
dimos/mapping/relocalization/blueprints.py modified Use generated PointCloud2, TFMessage in _fine_points.
dimos/mapping/relocalization/go2/module.py modified Use generated TransformStamped, PointCloud2 in _on_merge_input.
dimos/mapping/relocalization/lidar/module.py modified Use generated PointCloud2 in window, clouds.
dimos/mapping/relocalization/lidar/relocalize.py modified Use generated TransformStamped, Header in _prepare, relocalize.
dimos/mapping/relocalization/lidar/replay.py modified Use generated PointCloud2, TFMessage, TransformStamped in fix_error, recorded_fix.
dimos/mapping/relocalization/lidar/tune.md modified Update tune for the functional unit described above.
dimos/mapping/relocalization/lidar/tune.py modified Use generated PointCloud2, Header in fixtures, premap_index.
dimos/mapping/relocalization/module.py modified Use generated TransformStamped, PointCloud2, TFMessage in yaw_deg, fix_stream.
dimos/mapping/voxels/grid.py modified Use generated PointCloud2 in add_points, reproject.
dimos/mapping/voxels/impl/o3d.py modified Adapt add_frame, add_points for this unit's message/value flow.
dimos/mapping/voxels/impl/packed.py modified Use generated PointCloud2, Header in add_frame, add_points.
dimos/mapping/voxels/module.py modified Use generated PointCloud2 in _make_obs, __call__.
dimos/memory/type/observation.py modified Adapt _to_tuple, __repr__ for this unit's message/value flow.
dimos/message_utils/occupancy.py added Use generated Point, Pose, Quaternion in occupancy_view, block_max_reduce.
dimos/message_utils/pointcloud.py modified Use generated TransformStamped, CameraInfo, Image in transform_cloud, pointcloud_with_xyz.
dimos/navigation/base.py modified Use generated PoseStamped in set_goal, get_state.
dimos/navigation/global_planner/mls_planner/mls_planner_native.py modified Declare/use generated LineSegments3DRegion, PointCloudRegion, RegionBounds at this boundary.
dimos/navigation/global_planner/mls_planner/rust/src/lib.rs modified Update native lib message/TF handling; review the wire and field changes with its Python caller.
dimos/navigation/global_planner/mls_planner/rust/src/module.rs modified Update native module message/TF handling; review the wire and field changes with its Python caller.
dimos/navigation/global_planner/mls_planner/rust/src/wire.rs added Update native wire message/TF handling; review the wire and field changes with its Python caller.
dimos/navigation/global_planner/mls_planner/transformer.py modified Use generated Point, Pose, PoseStamped in _path_from_waypoints, __call__.
dimos/navigation/global_planner/mls_planner/viz.py modified Use generated LineSegments3D, LineSegments3DRegion, PointCloudRegion in surface_points, render_surface_region.
dimos/navigation/global_planner/viz.py modified Use generated PointStamped, Path in render_path, render_goal.
dimos/navigation/go2/loop_closure/module.py modified Use generated LineSegment3D, LineSegments3D, Point in _on_lidar, _publish_graph.
dimos/navigation/go2/loop_closure/pgo.py modified Use generated Pose, Quaternion, Transform in _obs_to_pose3, _pose3_to_transform.
dimos/navigation/go2/loop_closure/pgo_auto.py modified Use generated Quaternion, Transform, TransformStamped in keyframes_to_corrections, make_interpolator.
dimos/navigation/go2/loop_closure/pgo_map.py modified Use generated Point, Pose, Quaternion in main, __init__.
dimos/navigation/go2/replanning_a_star/controllers.py modified Use generated PoseStamped, Twist, Vector3 in _velocity, advance.
dimos/navigation/go2/replanning_a_star/global_planner.py modified Use generated Point, PoseStamped, Twist in cancel_goal, _empty_path.
dimos/navigation/go2/replanning_a_star/goal_validator.py modified Use generated Point, OccupancyGrid in _world_point, find_safe_goal.
dimos/navigation/go2/replanning_a_star/local_planner.py modified Use generated PoseStamped, Twist, OccupancyGrid in stop_planning, _thread_entrypoint.
dimos/navigation/go2/replanning_a_star/min_cost_astar.py modified Use generated Point, Pose, PoseStamped in _reconstruct_path, _reconstruct_path_from_coords.
dimos/navigation/go2/replanning_a_star/module.py modified Use generated PointStamped, Pose, PoseStamped in start, _handle_odometry.
dimos/navigation/go2/replanning_a_star/module_spec.py modified Declare/use generated PoseStamped at this boundary.
dimos/navigation/go2/replanning_a_star/navigation_map.py modified Declare/use generated OccupancyGrid at this boundary.
dimos/navigation/go2/replanning_a_star/path_clearance.py modified Use generated OccupancyGrid, Path in __init__, update_costmap.
dimos/navigation/go2/replanning_a_star/path_distancer.py modified Use generated PoseStamped, Path in __init__.
dimos/navigation/go2/replanning_a_star/position_tracker.py modified Use generated PoseStamped in add_position.
dimos/navigation/go2/replanning_a_star/replan_limiter.py modified Use generated Point in can_retry.
dimos/navigation/go2/replanning_a_star/spec.py modified Declare/use generated PoseStamped at this boundary.
dimos/navigation/local_planner/native.py modified Declare/use generated Path, PointCloud2, TFMessage at this boundary.
dimos/navigation/local_planner/profile.py modified Use generated Path in decode_ceilings.
dimos/navigation/local_planner/rust/src/module/msg.rs modified Update native msg message/TF handling; review the wire and field changes with its Python caller.
dimos/navigation/local_planner/rust/src/module/planner.rs modified Update native planner message/TF handling; review the wire and field changes with its Python caller.
dimos/navigation/local_planner/rust/src/module/tf_pose.rs modified Update native tf_pose message/TF handling; review the wire and field changes with its Python caller.
dimos/navigation/local_planner/viz.py modified Use generated PoseStamped, Path in _body_centre, render_body.
dimos/navigation/movement_manager/movement_manager.py modified Use generated Point, PointStamped, Twist in _on_click, _cancel_goal.
dimos/navigation/spec.py modified Declare/use generated PointStamped, Twist, Path at this boundary.
dimos/navigation/tf_pose.py modified Use generated PoseStamped in __init__, get.
dimos/navigation/trajectory_follower/basic/module.py modified Use generated PoseStamped, Twist, Vector3 in stop, _on_path.
dimos/navigation/trajectory_follower/fancy/controller.py modified Use generated PoseStamped, Twist, Path in path_xy_yaw.
dimos/navigation/trajectory_follower/fancy/laws/hinted.py modified Use generated PoseStamped, Twist, Vector3 in __init__, update.
dimos/navigation/trajectory_follower/fancy/laws/seed.py modified Use generated PoseStamped, Twist, Vector3 in __init__, reset.
dimos/navigation/trajectory_follower/fancy/module.py modified Use generated PoseStamped, Twist, Path in __init__, stop.
dimos/navigation/trajectory_follower/fancy/native.py modified Declare/use generated Twist, Path, Bool at this boundary.
dimos/navigation/trajectory_follower/fancy/rust/src/module/follower.rs modified Update native follower message/TF handling; review the wire and field changes with its Python caller.
dimos/robot/unitree/type/lidar.py modified Use generated PointCloud2 in repair_stale_ts, _repair.
dimos/spec/mapping.py modified Declare/use generated OccupancyGrid, PointCloud2 at this boundary.
packages/dimos-generated/custom/dimos_msgs/msg/LineSegments3DRegion.msg added Define the generated LineSegments3DRegion wire fields and explicit region metadata.
packages/dimos-generated/custom/dimos_msgs/msg/PointCloudRegion.msg added Define the generated PointCloudRegion wire fields and explicit region metadata.
packages/dimos-generated/custom/dimos_msgs/msg/RegionBounds.msg added Define the generated RegionBounds wire fields and explicit region metadata.

Build and dependency metadata (13)

File Change Purpose
dimos/mapping/ray_tracing/rust/Cargo.lock modified Update the dependency lock snapshot; LFS-managed contents were not hydrated in this report pass.
dimos/mapping/ray_tracing/rust/Cargo.toml modified Update Rust SDK/message dependencies for this native consumer.
dimos/mapping/ray_tracing/rust/flake.nix modified Wire native builds to the generated message source and declared dependencies.
dimos/navigation/global_planner/mls_planner/rust/Cargo.lock modified Update the dependency lock snapshot; LFS-managed contents were not hydrated in this report pass.
dimos/navigation/global_planner/mls_planner/rust/Cargo.toml modified Update Rust SDK/message dependencies for this native consumer.
dimos/navigation/global_planner/mls_planner/rust/flake.nix modified Wire native builds to the generated message source and declared dependencies.
dimos/navigation/local_planner/rust/Cargo.lock modified Update the dependency lock snapshot; LFS-managed contents were not hydrated in this report pass.
dimos/navigation/local_planner/rust/Cargo.toml modified Update Rust SDK/message dependencies for this native consumer.
dimos/navigation/local_planner/rust/flake.nix modified Wire native builds to the generated message source and declared dependencies.
dimos/navigation/trajectory_follower/fancy/rust/Cargo.lock modified Update the dependency lock snapshot; LFS-managed contents were not hydrated in this report pass.
dimos/navigation/trajectory_follower/fancy/rust/Cargo.toml modified Update Rust SDK/message dependencies for this native consumer.
dimos/navigation/trajectory_follower/fancy/rust/flake.lock modified Update the dependency lock snapshot; LFS-managed contents were not hydrated in this report pass.
dimos/navigation/trajectory_follower/fancy/rust/flake.nix modified Wire native builds to the generated message source and declared dependencies.

Tests (44)

File Change Purpose
dimos/mapping/cli/test_cli.py modified Checks summary.
dimos/mapping/cli/test_generated_cli.py added Checks global cli exports cdr and view reads it.
dimos/mapping/cli/test_pose_fill.py modified Checks pose fill attaches nearest pose.
dimos/mapping/occupancy/test_generated_operations.py added Checks existing algorithms preserve input identity and exact generated metadata.
dimos/mapping/occupancy/test_generated_path_resampling.py added Checks generated resampling keeps exact metadata and matches existing numerics.
dimos/mapping/occupancy/test_generated_rendering.py added Checks generated map pixels match existing palette and path overlay.
dimos/mapping/ray_tracing/test_generated_native.py added Checks native mapper declares canonical cloud and region ports.
dimos/mapping/ray_tracing/test_transformer.py modified Checks emit every n yields on cadence and flushes remainder.
dimos/mapping/ray_tracing/test_viz.py modified Checks the height ramp spans the range and clamps outside it.
dimos/mapping/ray_tracing/utils/test_loaded_map.py modified Checks first loaded map is none for a missing or empty stream.
dimos/mapping/relocalization/lidar/test_replay.py modified Checks fix error wraps yaw and measures translation.
dimos/mapping/relocalization/test_module.py modified Checks submit publishes and checks frames.
dimos/mapping/test_generated_costmapper.py added Checks generated costmaps match existing math and keep exact header.
dimos/mapping/test_generated_odometry_hist.py added Checks interval queue pose stamps replay restart and bound are preserved.
dimos/mapping/voxels/test_generated_stream.py added Checks array input matches legacy voxel ingestion.
dimos/mapping/voxels/test_stamped.py modified Checks stamped holds the same voxels.
dimos/memory/type/test_observation.py added Checks canonical pose payloads normalize metadata without loading or mutation.
dimos/message_utils/test_occupancy.py added Checks occupancy cells keep ros row order and do not alias message.
dimos/message_utils/test_pointcloud.py modified Checks field counts padding and endianness.
dimos/message_utils/test_regions.py added Checks region id and canonical dependency types survive cdr.
dimos/navigation/global_planner/mls_planner/test_generated_native.py added Checks mls native ports reuse canonical region and geometry types.
dimos/navigation/global_planner/mls_planner/test_transformer.py modified Checks flat floor yields populated path and planned true.
dimos/navigation/global_planner/mls_planner/test_viz.py modified Checks surface drops cells below the wall clearance.
dimos/navigation/global_planner/test_viz.py modified Checks goal placeholder is not drawn.
dimos/navigation/go2/loop_closure/test_generated_module.py added Checks actual pgo module ports publish generated map graph and telemetry.
dimos/navigation/go2/loop_closure/test_pgo.py modified Checks state reports keyframes anchored at the latest.
dimos/navigation/go2/loop_closure/test_pgo_auto.py added Checks generated corrections preserve frames time and nearest neighbor.
dimos/navigation/go2/loop_closure/test_pgo_map.py modified Checks rebuilds on loop closure and respects cooldown.
dimos/navigation/go2/replanning_a_star/test_generated_astar.py added Checks generated astar routes around obstacles and retains exact metadata.
dimos/navigation/go2/replanning_a_star/test_generated_planner.py added Checks generated p controller retains advance and rotate then drive commands.
dimos/navigation/go2/replanning_a_star/test_generated_safety.py added Checks generated safe goal preserves existing search choices and rotated coordinates.
dimos/navigation/go2/replanning_a_star/test_global_planner.py modified Checks find wide path with start inside inflation.
dimos/navigation/local_planner/test_generated_native.py added Checks native planner declares canonical generated port identities.
dimos/navigation/local_planner/test_profile.py modified Checks ceiling is segment length over stamp delta.
dimos/navigation/local_planner/test_viz.py modified Checks cdr plan draws without mutating exact source values.
dimos/navigation/movement_manager/test_movement_manager.py modified Checks teleop suppresses nav and cancels goal.
dimos/navigation/test_tf_pose.py modified Checks tf pose is the world to base edge.
dimos/navigation/trajectory_follower/basic/test_module.py modified Checks lookup pose steers from the tf base pose.
dimos/navigation/trajectory_follower/fancy/test_controller.py modified Checks on path drives forward.
dimos/navigation/trajectory_follower/fancy/test_generated_native.py added Checks native follower declares canonical generated port identities.
dimos/navigation/trajectory_follower/fancy/test_module.py modified Checks goal latch fires once then holds.
dimos/navigation/trajectory_follower/fancy/test_rust_parity.py modified Checks rust matches python.
dimos/navigation/trajectory_follower/test_generated_ports.py added Checks actual generated follower ports drive arrive and preempt over cdr.
dimos/robot/unitree/type/test_lidar.py modified Checks init.

Documentation and replay inputs (3)

File Change Purpose
docs/capabilities/navigation/deep_dive.md modified Update the deep dive guide or replay input for the migrated boundaries.
docs/capabilities/navigation/relocalization.md modified Update the relocalization guide or replay input for the migrated boundaries.
docs/platforms/quadruped/go2/index.md modified Update the index guide or replay input for the migrated boundaries.

Review focus and scope reductions

  • Occupancy utilities and tests are colocated in dimos.message_utils; pointcloud extensions reuse the predecessor's canonical implementation. No helper aliases remain at the removed paths.
  • Review numerical parity separately: filtering, occupancy coordinates, frame composition and planner safety. A message-migration title does not prove these computations are unchanged.
  • Review region IDs, frame/time and nested schema dependencies with the producers/consumers.
  • Native planner codec/Nix changes remain substantive and need native validation.

Validation

Fresh checks on f46ed9ce6b23a3c4ec1a48adaeeeef1eb4eca9b1: 420 helper/occupancy/costmapper/voxel/planner-safety/lidar tests passed, four were deselected by existing markers. Normal isolated editable installation regenerated this layer's region messages and passed. All-files hooks passed with cargo/native lint excluded. A single native follower lockfile conflict was resolved according to this layer's removal of the old dependency; the predecessor bot commit and all original heads remain backed up.

Reproduction commands, not executed here unless stated above

Run from this PR's own checkout root after installing its documented test dependencies. These are selected review entry points, not a claim of full-stack coverage.

uv run pytest dimos/message_utils dimos/mapping/occupancy/test_generated_operations.py dimos/mapping/occupancy/test_generated_path_resampling.py dimos/mapping/occupancy/test_generated_rendering.py dimos/mapping/test_generated_costmapper.py dimos/mapping/test_generated_odometry_hist.py dimos/mapping/voxels/test_generated_stream.py dimos/navigation/go2/replanning_a_star/test_generated_astar.py dimos/navigation/go2/replanning_a_star/test_generated_planner.py dimos/navigation/go2/replanning_a_star/test_generated_safety.py dimos/robot/unitree/type/test_lidar.py

Remaining work

Current predecessor is incorporated. Native planner builds, Clippy/Nix and full robot graph validation remain unverified. The installed-wheel tip checks are described in #4734; they do not certify each intermediate package independently.

@TomCC7
TomCC7 force-pushed the cc/feat/cdr-unit-mapping branch from 8700f7a to f46ed9c Compare October 22, 2026 02:34
@TomCC7
TomCC7 removed this pull request from stack #4743 October 22, 2026 18:51
@TomCC7
TomCC7 added this pull request to stack #4776 October 22, 2026 18:52

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