feat: ray tracer and 3d planner mem2 transforms

[aclauer]

Problem

mem2 transforms allow for faster algo dev

Closes DIM-XXX

Solution

Wire the ray tracer and 3d planner together using the Rust bindings in to a mem2 pipeline.

How to Test

build FFI

uv run maturin develop --uv --release -m dimos/mapping/ray_tracing/rust/Cargo.toml
uv run maturin develop --uv --release -m dimos/navigation/nav_3d/mls_planner/rust/Cargo.toml

run

python -m dimos.navigation.nav_3d.mls_planner.utils.plan_rrd ~/Downloads/mem2-003.db --robot-height 0.3

Contributor License Agreement

• I have read and approved the CLA.

[codecov]

❌ 1 Tests Failed:

Tests completed	Failed	Passed	Skipped
2118	1	2117	75

View the top 2 failed test(s) by shortest run time

dimos.e2e_tests.test_dimsim_path_replaning::test_path_replanning

Stack Traces | 239s run time

lcm_spy = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x70f42e28e4e0>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x70f42d9f1ee0>
dim_sim = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x70f42e5fe0c0>
direct_cmd_vel_explorer = <dimos.simulation.mujoco.direct_cmd_vel_explorer.DirectCmdVelExplorer object at 0x70f42fdfa1e0>
spawn_wall_on_pose = <function spawn_wall_on_pose.<locals>.spawn at 0x70f42d9f2d40>

    @pytest.mark.self_hosted_large
    def test_path_replanning(
        lcm_spy, start_blueprint, dim_sim, direct_cmd_vel_explorer, spawn_wall_on_pose
    ) -> None:
        start_blueprint(
            "--dimsim-scene=empty",
            "run",
            "unitree-go2-agentic",
            simulator="dimsim",
        )
        lcm_spy.save_topic(".../McpClient/on_system_modules/res")
        lcm_spy.wait_for_saved_topic(".../McpClient/on_system_modules/res", timeout=1200.0)
    
        # robot spawns at (3, 2)
    
        # side wall
        dim_sim.add_wall(2, -2.5, 12, -2.5)
        # other side wall
        dim_sim.add_wall(2, 3.5, 12, 3.5)
        # back wall (behind robot)
        dim_sim.add_wall(2, -2.5, 2, 3.5)
        # forward wall (far end)
        dim_sim.add_wall(12, -2.5, 12, 3.5)
        # dividing wall at x=7 with doors at y=[-1.5,-0.5] and y=[1.5,2.5]
        dim_sim.add_wall(7, -2.5, 7, -1.5)
        dim_sim.add_wall(7, -0.5, 7, 1.5)
        dim_sim.add_wall(7, 2.5, 7, 3.5)
    
        direct_cmd_vel_explorer.linear_speed = 0.8
        direct_cmd_vel_explorer.follow_points([(10, 2), (2.5, 2), (3, 2)])
    
        # When the robot comes within 1.5 m of the left door's centre, drop a wall
        # in the opening so the planner has to bail out and route through the
        # right door at y=-1 instead.
        spawn_wall_on_pose(
            point=(7, 2),
            threshold=1.5,
            wall=(7, 1.5, 7, 2.5),
        )
    
        dim_sim.publish_goal(10.913, 0.588)
    
>       lcm_spy.wait_until_odom_position(10.913, 0.588, threshold=1, timeout=120)

dim_sim    = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x70f42e5fe0c0>
direct_cmd_vel_explorer = <dimos.simulation.mujoco.direct_cmd_vel_explorer.DirectCmdVelExplorer object at 0x70f42fdfa1e0>
lcm_spy    = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x70f42e28e4e0>
spawn_wall_on_pose = <function spawn_wall_on_pose.<locals>.spawn at 0x70f42d9f2d40>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x70f42d9f1ee0>

dimos/e2e_tests/test_dimsim_path_replaning.py:60: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
dimos/e2e_tests/lcm_spy.py:182: in wait_until_odom_position
    self.wait_for_message_result(
        predicate  = <function LcmSpy.wait_until_odom_position.<locals>.predicate at 0x70f42d9f36a0>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x70f42e28e4e0>
        threshold  = 1
        timeout    = 120
        x          = 10.913
        y          = 0.588
dimos/e2e_tests/lcm_spy.py:168: in wait_for_message_result
    self.wait_until(
        event      = <threading.Event at 0x70f42da1b380: unset>
        fail_message = 'Failed to get to position x=10.913, y=0.588'
        listener   = <function LcmSpy.wait_for_message_result.<locals>.listener at 0x70f42d9f3740>
        predicate  = <function LcmSpy.wait_until_odom_position.<locals>.predicate at 0x70f42d9f36a0>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x70f42e28e4e0>
        timeout    = 120
        topic      = '/odom#geometry_msgs.PoseStamped'
        type       = <class 'dimos.msgs.geometry_msgs.PoseStamped.PoseStamped'>
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

self = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x70f42e28e4e0>

    def wait_until(
        self,
        *,
        condition: Callable[[], bool],
        timeout: float,
        error_message: str,
        poll_interval: float = 0.1,
    ) -> None:
        start_time = time.time()
        while time.time() - start_time < timeout:
            if condition():
                return
            time.sleep(poll_interval)
>       raise TimeoutError(error_message)
E       TimeoutError: Failed to get to position x=10.913, y=0.588

condition  = <bound method Event.is_set of <threading.Event at 0x70f42da1b380: unset>>
error_message = 'Failed to get to position x=10.913, y=0.588'
poll_interval = 0.1
self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x70f42e28e4e0>
start_time = 1781030984.0688508
timeout    = 120

dimos/e2e_tests/lcm_spy.py:105: TimeoutError

dimos.e2e_tests.test_dimsim_spatial_memory::test_go_to_the_bed

Stack Traces | 563s run time

lcm_spy = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x761c9f480050>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x761c9e92ade0>
human_input = <function human_input.<locals>.send_human_input at 0x761c9e92b380>
dim_sim = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x761c9f4bb170>
explore_house = <function explore_house.<locals>.explore at 0x761c9e92bd80>

    @pytest.mark.self_hosted_large
    def test_go_to_the_bed(lcm_spy, start_blueprint, human_input, dim_sim, explore_house) -> None:
        start_blueprint(
            "run",
            "unitree-go2-agentic",
            simulator="dimsim",
        )
        lcm_spy.save_topic(".../McpClient/on_system_modules/res")
        lcm_spy.wait_for_saved_topic(".../McpClient/on_system_modules/res", timeout=1200.0)
    
        explore_house()
    
        human_input("go to the bed")
    
>       lcm_spy.wait_until_odom_position(-3.567, -1.332, threshold=2, timeout=180)

dim_sim    = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x761c9f4bb170>
explore_house = <function explore_house.<locals>.explore at 0x761c9e92bd80>
human_input = <function human_input.<locals>.send_human_input at 0x761c9e92b380>
lcm_spy    = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x761c9f480050>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x761c9e92ade0>

dimos/e2e_tests/test_dimsim_spatial_memory.py:32: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
dimos/e2e_tests/lcm_spy.py:182: in wait_until_odom_position
    self.wait_for_message_result(
        predicate  = <function LcmSpy.wait_until_odom_position.<locals>.predicate at 0x761c9e92bf60>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x761c9f480050>
        threshold  = 2
        timeout    = 180
        x          = -3.567
        y          = -1.332
dimos/e2e_tests/lcm_spy.py:168: in wait_for_message_result
    self.wait_until(
        event      = <threading.Event at 0x761c9f4bbb90: unset>
        fail_message = 'Failed to get to position x=-3.567, y=-1.332'
        listener   = <function LcmSpy.wait_for_message_result.<locals>.listener at 0x761c9e92b740>
        predicate  = <function LcmSpy.wait_until_odom_position.<locals>.predicate at 0x761c9e92bf60>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x761c9f480050>
        timeout    = 180
        topic      = '/odom#geometry_msgs.PoseStamped'
        type       = <class 'dimos.msgs.geometry_msgs.PoseStamped.PoseStamped'>
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

self = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x761c9f480050>

    def wait_until(
        self,
        *,
        condition: Callable[[], bool],
        timeout: float,
        error_message: str,
        poll_interval: float = 0.1,
    ) -> None:
        start_time = time.time()
        while time.time() - start_time < timeout:
            if condition():
                return
            time.sleep(poll_interval)
>       raise TimeoutError(error_message)
E       TimeoutError: Failed to get to position x=-3.567, y=-1.332

condition  = <bound method Event.is_set of <threading.Event at 0x761c9f4bbb90: unset>>
error_message = 'Failed to get to position x=-3.567, y=-1.332'
poll_interval = 0.1
self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x761c9f480050>
start_time = 1781024517.5574727
timeout    = 180

dimos/e2e_tests/lcm_spy.py:105: TimeoutError

To view more test analytics, go to the Test Analytics Dashboard
_{📋 Got 3 mins? Take this short survey to help us improve Test Analytics.}

[greptile-apps]

Greptile Summary

This PR wires the Rust voxel ray-tracer and MLS 3D planner into the mem2 pipeline as new Transformer subclasses, adds unit tests for both, a development replay utility (plan_rrd.py), and updates CI to install the full test dependency set and run the new tests.

• RayTraceMap accumulates world-frame lidar into a VoxelRayMapper and emits the merged map every emit_every frames (with a trailing flush for remainders); skips pose-less observations.
• MLSPlan plans a path from the robot's current pose to a fixed goal using MLSPlanner on each emitted voxel snapshot; tags the output with the raw surface map, graph nodes/edges, and planning status.
• plan_rrd.py composes both transformers into a full pipeline over an sqlite recording and logs results to rerun live or to a .rrd file.

Confidence Score: 5/5

Safe to merge — both transformers are stateless per pipeline call, the emit/flush logic is verified by tests, and the replay utility is a dev-only script with no production code paths.

The transformer logic is straightforward: RayTraceMap correctly accumulates lidar and flushes remainders, MLSPlan creates a fresh planner per call and derives outputs cleanly. Unit tests cover cadence, flush, pose-less skipping, and planning failure. CI changes are mechanical (dependency sync + two new test files). No data-loss or incorrect-state paths were found.

No files require special attention.

Important Files Changed

Filename	Overview
dimos/mapping/ray_tracing/transformer.py	New RayTraceMap transformer: accumulates lidar, emits accumulated voxel map every N frames with correct remainder flush logic; pose-less observations are correctly skipped.
dimos/navigation/nav_3d/mls_planner/transformer.py	New MLSPlan transformer: plans a path per voxel-map snapshot; creates a fresh MLSPlanner per pipeline call; correctly derives output observation with voxel_map, nodes, edges, and planned tags.
dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py	Dev replay utility composing both transformers over a sqlite recording; correctly uses pair[0]/pair[1] index access on AlignedPair; rerun logging looks correct.
dimos/mapping/ray_tracing/test_transformer.py	Tests cover emit cadence/flush, pose-less skipping, and negative emit_every validation; uses pytest.importorskip for optional Rust extension guard.
dimos/navigation/nav_3d/mls_planner/test_transformer.py	Tests cover planned/unplanned paths, pose-less skipping, and tag assertions; flat-floor fixture matches the z-subtraction convention for robot_height.
.github/workflows/ci.yml	Switches to uv sync --group tests --frozen for reproducibility; adds portaudio19-dev prerequisite for pyaudio; adds the two new transformer test files to the pytest invocation.

Sequence Diagram

sequenceDiagram
    participant DB as SqliteStore
    participant LA as lidar stream
    participant OA as odom stream
    participant AL as align()
    participant FT as FnTransformer<br/>(_attach_pose_from_odom)
    participant RT as RayTraceMap
    participant MP as MLSPlan
    participant RR as rerun

    DB->>LA: stream PointCloud2 obs
    DB->>OA: stream Odometry obs
    LA->>AL: primary (lidar)
    OA->>AL: secondary (odom)
    AL->>FT: AlignedPair obs (lidar[0], odom[1])
    FT->>RT: lidar Observation with pose from odom
    Note over RT: accumulate into VoxelRayMapper<br/>emit every N frames + flush remainder
    RT->>MP: Observation[PointCloud2] (accumulated map + frame_count tag)
    Note over MP: compute start = (x, y, z - robot_height)<br/>update_global_map → plan(start, goal)
    MP->>RR: "Observation[Path] with tags:<br/>voxel_map, surface_map, nodes,<br/>node_edges, start, planned"

Loading

_{Reviews (5): Last reviewed commit: "Merge branch 'main' into andrew/feat/ray..." | Re-trigger Greptile}

[paul-nechifor]

Noooooo, don't skip the tests 😭

[Dreamsorcerer]

Think this'll be necessary if it's an optional dependency (also see the other thread about merging into the regular test job).

[paul-nechifor]

But if the test is never executed then why have it at all?

[aclauer]

The tests do get ran in the rust job. First it builds the binaries, then does the pytest, so the importorskip imports properly.

https://github.com/dimensionalOS/dimos/pull/2368/changes#diff-b803fcb7f17ed9235f1e5cb1fcd2f5d3b2838429d4368ae4c57ce4436577f03fR127

[aclauer]

They just get skipped in all the other jobs that don't build the binaries

[paul-nechifor]

But they do get skipped locally by default, right? That is, if I run this locally, do they get executed:

git clone ...
CYCLONEDDS_HOME=/opt/cyclonedds uv sync --all-extras --all-groups
uv run pytest -m 'not (tool or mujoco)'

[Dreamsorcerer]

Because you need to build the dependency, yeah.

[aclauer]

Yeah they'll get skipped by default. But part of the dev process is building the binaries, so I think that's alright at least for now? Once we do what Sam mentioned, then I think we should be able to remove this

[Dreamsorcerer]

Approving the CI change per the discussion.