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

dimos/mapping/cli/pose_fill.py

@@ -40,7 +40,7 @@
     from dimos.memory.backend import Backend
     from dimos.memory.stream import Stream
     from dimos.memory.type.observation import Observation
-    from dimos.msgs.geometry_msgs.Transform import Transform
+    from dimos.messages.geometry_msgs.msg import Transform
 
 
 def pose_fill(
@@ -54,20 +54,19 @@ def pose_fill(
 
     Pairs are formed by :meth:`Stream.align` (nearest ``|Δts| <= tolerance``);
     primaries with no match in tolerance are dropped. The pose is read from the
-    matched pose observation's *payload* (``PoseStamped`` / ``Odometry`` / any
-    object exposing ``.position`` + ``.orientation``) because pose-message
-    streams carry the pose in the value, not the indexed pose columns. The
-    target stream's payload stays lazy.
+    matched pose observation's payload: generated ``Pose``, ``Transform`` or
+    their standard stamped/covariance/odometry forms. Pose-message streams
+    carry the pose in the value, not the indexed pose columns. The target
+    stream's payload stays lazy.
 
     *mount* composes a static child transform onto each source pose
     (``world_base ∘ mount``) before attaching — e.g. ``base_link →
     camera_optical`` so a base-frame odometry source yields an optical-frame
     image pose.
     """
     from dimos.memory.type.observation import _to_tuple
-    from dimos.msgs.geometry_msgs.Quaternion import Quaternion
-    from dimos.msgs.geometry_msgs.Transform import Transform
-    from dimos.msgs.geometry_msgs.Vector3 import Vector3
+    from dimos.message_utils.geometry import compose_transform_values
+    from dimos.messages.geometry_msgs.msg import Quaternion, Transform, Vector3
 
     def _fill(pair_obs: Observation[Any]) -> Observation[Any]:
         primary, secondary = cast(
@@ -77,7 +76,7 @@ def _fill(pair_obs: Observation[Any]) -> Observation[Any]:
             return primary.with_pose(secondary.data)
         x, y, z, qx, qy, qz, qw = cast("tuple[float, ...]", _to_tuple(secondary.data))
         world_base = Transform(translation=Vector3(x, y, z), rotation=Quaternion(qx, qy, qz, qw))
-        return primary.with_pose(world_base + mount)
+        return primary.with_pose(compose_transform_values(world_base, mount))
 
     return stream.align(pose_stream, tolerance=tolerance).map(_fill)
 
@@ -112,48 +111,57 @@ def pose_fill_db(
             "collide on ts). Delete it (and any -wal/-shm sidecars) first."
         )
 
-    src = SqliteStore(path=str(src_path), must_exist=True)
-    dest = SqliteStore(path=str(dest_path))
-    names = streams if streams is not None else src.list_streams()
-    if target not in names:
-        raise ValueError(f"target stream {target!r} not in {names}")
-    if pose_source not in src.list_streams():
-        raise ValueError(f"pose_source stream {pose_source!r} not found in source dataset")
-
-    written: dict[str, int] = {}
-    for name in names:
-        src_b = cast("Backend[Any]", src.stream(name)._source)
-        dest_b = cast("Backend[Any]", dest.stream(name, src_b.data_type, codec=src_b.codec)._source)
-
-        # Re-posed observations for the target (ids preserved from source, so
-        # the source blob still resolves); plain metadata for everything else.
-        if name == target:
-            rows: Iterable[Observation[Any]] = pose_fill(
-                src.stream(name).from_time(seek or None).to_time(duration).order_by("ts"),
-                src.stream(pose_source).from_time(seek or None).to_time(duration).order_by("ts"),
-                tolerance=tolerance,
+    with (
+        SqliteStore(path=str(src_path), must_exist=True) as src,
+        SqliteStore(path=str(dest_path)) as dest,
+    ):
+        names = streams if streams is not None else src.list_streams()
+        if target not in names:
+            raise ValueError(f"target stream {target!r} not in {names}")
+        if pose_source not in src.list_streams():
+            raise ValueError(f"pose_source stream {pose_source!r} not found in source dataset")
+
+        written: dict[str, int] = {}
+        for name in names:
+            src_b = cast("Backend[Any]", src.stream(name)._source)
+            dest_b = cast(
+                "Backend[Any]", dest.stream(name, src_b.data_type, codec=src_b.codec)._source
             )
-        else:
-            rows = src.stream(name).from_time(seek or None).to_time(duration).order_by("ts")
-
-        scalar = src_b.data_type in (int, float)
-        n = 0
-        for obs in rows:
-            blob = (
-                None if scalar or src_b.blob_store is None else src_b.blob_store.get(name, obs.id)
-            )
-            row_id = dest_b.metadata_store.insert(obs)
-            if blob is not None:
-                assert dest_b.blob_store is not None
-                dest_b.blob_store.put(name, row_id, blob)
-            n += 1
-        commit = getattr(dest_b.metadata_store, "commit", None)
-        if commit is not None:
-            commit()
-        written[name] = n
-
-    src.stop()
-    dest.stop()
+
+            # Re-posed observations for the target (ids preserved from source, so
+            # the source blob still resolves); plain metadata for everything else.
+            if name == target:
+                rows: Iterable[Observation[Any]] = pose_fill(
+                    src.stream(name).from_time(seek or None).to_time(duration).order_by("ts"),
+                    src.stream(pose_source)
+                    .from_time(seek or None)
+                    .to_time(duration)
+                    .order_by("ts"),
+                    tolerance=tolerance,
+                )
+            else:
+                rows = src.stream(name).from_time(seek or None).to_time(duration).order_by("ts")
+
+            scalar = src_b.data_type in (int, float)
+            n = 0
+            for obs in rows:
+                blob = (
+                    None
+                    if scalar or src_b.blob_store is None
+                    else src_b.blob_store.get(name, obs.id)
+                )
+                row_id = dest_b.metadata_store.insert(obs)
+                if blob is not None:
+                    assert dest_b.blob_store is not None
+                    dest_b.blob_store.put(name, row_id, blob)
+                n += 1
+            commit = getattr(dest_b.metadata_store, "commit", None)
+            if commit is not None:
+                commit()
+            written[name] = n
+
+        src.stop()
+        dest.stop()
     return written
 
 

dimos/mapping/cli/replay.py

@@ -41,7 +41,7 @@
 
 if TYPE_CHECKING:
     from dimos.memory.stream import Stream
-    from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
+    from dimos.messages.sensor_msgs.msg import PointCloud2
 
 TIMELINE = "ts"
 
@@ -64,14 +64,22 @@ def _log_clouds(
     """
     import rerun as rr
 
+    from dimos.visualization.rerun.cdr import cloud_archetype
+
     n = total if total is not None else stream.count()
     with progress(n, label) as bar:
         for obs in stream:
             bar(obs)
             rr.set_time(TIMELINE, timestamp=obs.ts)
             rr.log(
                 entity,
-                obs.data.to_rerun(voxel_size=voxel, mode=point_mode, bottom_cutoff=bottom_cutoff),
+                cloud_archetype(
+                    obs.data,
+                    voxel_size=voxel,
+                    mode=point_mode,
+                    bottom_cutoff=bottom_cutoff,
+                    height_colors=True,
+                ),
             )
 
 
@@ -176,14 +184,14 @@ def main(
     """Dump a recording to .rrd (lidar clouds + camera frames) and open it in rerun."""
     import rerun as rr
 
+    from dimos.mapping.cli.map import _camera_info_static
     from dimos.mapping.voxels.module import VoxelMapTransformer
     from dimos.memory.cli.dataset import open_store, resolve_dataset, stream_payload_types
     from dimos.memory.transform import throttle
-    from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
-    from dimos.msgs.nav_msgs.Odometry import Odometry
-    from dimos.msgs.sensor_msgs.Image import Image
-    from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2, register_colormap_annotation
-    from dimos.robot.unitree.go2.connection import _camera_info_static
+    from dimos.messages.geometry_msgs.msg import PoseStamped
+    from dimos.messages.nav_msgs.msg import Odometry
+    from dimos.messages.sensor_msgs.msg import Image, PointCloud2
+    from dimos.visualization.rerun.cdr import camera_pinhole, cloud_archetype, image_archetype
 
     src_path = resolve_dataset(dataset)
     store = open_store(src_path)
@@ -201,11 +209,10 @@ def main(
 
         rr.init("dimos map_rrd", recording_id=src_path.stem)
         rr.save(str(out))
-        register_colormap_annotation("turbo")
 
         # Static pinhole on the camera entity; per-frame Transform3D goes on the
         # same entity. Image is the child so it projects through the pinhole.
-        pinhole = cam_info.to_rerun()
+        pinhole = camera_pinhole(cam_info)
         assert not isinstance(pinhole, list)
         rr.log("world/camera", pinhole, static=True)
 
@@ -268,8 +275,12 @@ def clipped(name: str, ptype: type[Any]) -> Stream[Any]:
                     ).last()
                     rr.log(
                         f"world/{name}_map",
-                        final.data.to_rerun(
-                            voxel_size=voxel / 4, mode=point_mode, bottom_cutoff=bottom_cutoff
+                        cloud_archetype(
+                            final.data,
+                            voxel_size=voxel / 4,
+                            mode=point_mode,
+                            bottom_cutoff=bottom_cutoff,
+                            height_colors=True,
                         ),
                         static=True,
                     )
@@ -339,7 +350,7 @@ def clipped(name: str, ptype: type[Any]) -> Stream[Any]:
                             translation=[x, y, z], quaternion=rr.Quaternion(xyzw=[qx, qy, qz, qw])
                         ),
                     )
-                rr.log("world/camera/image", img_obs.data.to_rerun())
+                rr.log("world/camera/image", image_archetype(img_obs.data))
 
     print(f"wrote {out}")
     if no_gui:

dimos/mapping/cli/replay_marker.py

@@ -64,13 +64,14 @@ def main(
     """Dump an AprilTag detection replay to .rrd and open it in rerun."""
     import rerun as rr
 
+    from dimos.mapping.cli.map import _camera_info_static
     from dimos.memory.cli.dataset import open_store, resolve_dataset
     from dimos.memory.transform import QualityWindow, SpeedLimit
     from dimos.memory.vis.color import Color
-    from dimos.msgs.sensor_msgs.Image import Image
-    from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
+    from dimos.message_utils.image import image_sharpness
+    from dimos.messages.sensor_msgs.msg import Image, PointCloud2
     from dimos.perception.fiducial.marker_transformer import DetectMarkers
-    from dimos.robot.unitree.go2.connection import _camera_info_static
+    from dimos.visualization.rerun.cdr import camera_pinhole, image_archetype
 
     db_path = resolve_dataset(dataset)
     store = open_store(db_path)
@@ -83,7 +84,7 @@ def main(
 
     # Static pinhole on the camera entity; per-frame Transform3D goes on the
     # same entity. Image is the child so it projects through the pinhole.
-    pinhole = cam_info.to_rerun()
+    pinhole = camera_pinhole(cam_info)
     assert not isinstance(pinhole, list)
     rr.log("world/camera", pinhole, static=True)
 
@@ -116,7 +117,7 @@ def main(
                         translation=[x, y, z], quaternion=rr.Quaternion(xyzw=[qx, qy, qz, qw])
                     ),
                 )
-            rr.log("world/camera/image", img_obs.data.to_rerun())
+            rr.log("world/camera/image", image_archetype(img_obs.data))
             # Clear any prior detection box at this ts; pass 3 will overwrite
             # this with the actual detection iff one fires for this frame.
             rr.log(
@@ -129,7 +130,7 @@ def main(
         # Pass 3: marker detections (filtered same way as `dimos map`).
         xf = DetectMarkers(camera_info=cam_info, marker_length_m=marker_size)
         pipeline: Stream[Any] = color_image.transform(
-            QualityWindow(lambda img: img.sharpness, window=quality_window)
+            QualityWindow(image_sharpness, window=quality_window)
         )
         if marker_max_speed > 0:
             pipeline = pipeline.transform(
@@ -195,7 +196,7 @@ def main(
                 smoothing_window=smoothing_window,
             )
             pipeline_tracked: Stream[Any] = color_image.transform(
-                QualityWindow(lambda img: img.sharpness, window=quality_window)
+                QualityWindow(image_sharpness, window=quality_window)
             )
             if marker_max_speed > 0:
                 pipeline_tracked = pipeline_tracked.transform(

dimos/mapping/cli/test_cli.py

@@ -29,9 +29,15 @@
 import traceback
 from types import SimpleNamespace
 
+from dimos_cdrgen.registry import encode
+import numpy as np
 import pytest
 from typer.testing import CliRunner
 
+from dimos.message_utils.pointcloud import pointcloud_from_xyz
+from dimos.message_utils.time import time_from_seconds
+from dimos.messages.std_msgs.msg import Header
+
 # These drive the real CLI against an LFS recording (CPU voxel accumulation,
 # multi-second runs) — self-hosted runner only.
 pytestmark = pytest.mark.self_hosted
@@ -152,7 +158,7 @@ def test_pose_fill_snippet(dataset: str, tmp_path: Path) -> None:
     assert out.exists()
 
     from dimos.memory.store.sqlite import SqliteStore
-    from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
+    from dimos.messages.sensor_msgs.msg import PointCloud2
 
     store = SqliteStore(path=str(out), must_exist=True)
     try:
@@ -226,15 +232,12 @@ def test_global_snippet(dataset: str, tmp_path: Path) -> None:
 
 
 def test_view_pc2(tmp_path: Path) -> None:
-    import numpy as np
-
-    from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
-
-    pc2 = tmp_path / "map.pc2.lcm"
-    cloud = PointCloud2.from_numpy(
-        np.random.default_rng(0).random((256, 3)), frame_id="world", timestamp=0.0
+    pc2 = tmp_path / "map.pc2.cdr"
+    cloud = pointcloud_from_xyz(
+        np.random.default_rng(0).random((256, 3)),
+        header=Header(stamp=time_from_seconds(0), frame_id="world"),
     )
-    pc2.write_bytes(cloud.lcm_encode())
+    pc2.write_bytes(encode(cloud))
 
     out = tmp_path / "view.rrd"
     res = _run("view", str(pc2), "--out", str(out))