refactor(memory): accept generated poses in lossless pose-fill

.github/workflows/message-codegen.yml

@@ -64,6 +64,8 @@ on:
       - native/rust/dimos-module/**
       - native/rust/dimos-module-macros/**
       - native/message-source.nix
+      - dimos/mapping/cli/pose_fill.py
+      - dimos/mapping/cli/test_pose_fill.py
       - native/cpp/include/dimos/native/cdr_codec.hpp
       - docs/development/message-source-packages.md
       - docs/development/messages-external-project.md
@@ -366,3 +368,8 @@ jobs:
           export DIMOS_NATIVE_MLS_BINARY="$PWD/dimos/navigation/global_planner/mls_planner/rust/target/debug/mls_planner"
           .native-review/bin/python -I -c 'from dimos.navigation.global_planner.mls_planner import mls_planner_native,transformer,viz; import dimos_mls_planner; import pytest; import sys; assert all("site-packages" in str(value.__file__) for value in (mls_planner_native,transformer,viz,dimos_mls_planner)); raise SystemExit(pytest.main(sys.argv[1:]))' --noconftest --import-mode=importlib -o addopts='' -q "$PWD/dimos/navigation/global_planner/mls_planner/test_generated_native.py" "$PWD/dimos/navigation/global_planner/mls_planner/test_transformer.py" "$PWD/dimos/navigation/global_planner/mls_planner/test_viz.py"
           MYPYPATH="$PWD/build/message-source/python" .native-review/bin/python -m mypy --follow-imports=silent dimos/navigation/global_planner/mls_planner/{mls_planner_native,transformer,viz}.py dimos/navigation/spec.py
+      - name: Verify canonical observation pose payloads and lossless pose-fill
+        if: runner.os == 'Linux'
+        run: |
+          .native-review/bin/python -I -c 'from dimos.memory.type import observation; from dimos.mapping.cli import pose_fill; import pytest; import sys; assert all("site-packages" in str(value.__file__) for value in (observation,pose_fill)); raise SystemExit(pytest.main(sys.argv[1:]))' --noconftest --import-mode=importlib -o addopts='' -q "$PWD/dimos/memory/type/test_observation.py" "$PWD/dimos/mapping/cli/test_pose_fill.py" "$PWD/dimos/memory/test_stream.py::TestWithPose" "$PWD/dimos/memory/store/test_cdr_recording.py"
+          MYPYPATH="$PWD/build/message-source/python" .native-review/bin/python -m mypy --follow-imports=silent dimos/memory/type/observation.py dimos/mapping/cli/pose_fill.py

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.messages.geometry_msgs.msg import Quaternion, Transform, Vector3
+    from dimos.msgs.geometry import compose_transform_values
 
     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/test_pose_fill.py

@@ -20,11 +20,28 @@
 
 from __future__ import annotations
 
-from dimos.mapping.cli.pose_fill import pose_fill
+from copy import deepcopy
+import math
+
+from dimos_cdrgen.registry import encode
+import numpy as np
+
+from dimos.mapping.cli.pose_fill import pose_fill, pose_fill_db
 from dimos.memory.store.memory import MemoryStore
-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.memory.store.sqlite import SqliteStore
+from dimos.messages.geometry_msgs.msg import (
+    Point,
+    Pose,
+    PoseStamped,
+    Quaternion,
+    Transform,
+    Vector3,
+)
+from dimos.messages.sensor_msgs.msg import PointCloud2
+from dimos.messages.std_msgs.msg import Header
+from dimos.msgs.geometry import quaternion_from_euler
+from dimos.msgs.pointcloud import pointcloud_from_xyz
+from dimos.msgs.time import time_from_nanoseconds
 
 
 def test_pose_fill_attaches_nearest_pose() -> None:
@@ -33,7 +50,12 @@ def test_pose_fill_attaches_nearest_pose() -> None:
         target = store.stream("image", str)
         poses = store.stream("odom", Transform)
         target.append("img0", ts=0.0)
-        poses.append(Transform(translation=Vector3(1.0, 0.0, 0.0)), ts=0.001)
+        poses.append(
+            Transform(
+                translation=Vector3(x=1.0, y=0.0, z=0.0), rotation=Quaternion(x=0, y=0, z=0, w=1)
+            ),
+            ts=0.001,
+        )
 
         out = pose_fill(target, poses, tolerance=0.05).to_list()
 
@@ -50,12 +72,73 @@ def test_pose_fill_mount_composes_static_child_transform() -> None:
         poses = store.stream("odom", Transform)
         target.append("img0", ts=0.0)
         # Base pose 1m forward in x; mount offsets 1m in y (identity rotations).
-        poses.append(Transform(translation=Vector3(1.0, 0.0, 0.0)), ts=0.001)
-        mount = Transform(translation=Vector3(0.0, 1.0, 0.0), rotation=Quaternion())
+        poses.append(
+            Transform(
+                translation=Vector3(x=1.0, y=0.0, z=0.0), rotation=Quaternion(x=0, y=0, z=0, w=1)
+            ),
+            ts=0.001,
+        )
+        mount = Transform(
+            translation=Vector3(x=0, y=1, z=0), rotation=Quaternion(x=0, y=0, z=0, w=1)
+        )
 
         out = pose_fill(target, poses, tolerance=0.05, mount=mount).to_list()
 
     assert len(out) == 1
     assert out[0].pose_tuple is not None
     # Identity rotations → composed translation is the component-wise sum.
     assert out[0].pose_tuple[:3] == (1.0, 1.0, 0.0)
+
+
+def test_stamped_pose_mount_rotates_the_offset_and_keeps_payload():
+    header = Header(frame_id="world", stamp=time_from_nanoseconds(1700000000123456789))
+    pose = PoseStamped(
+        header=header,
+        pose=Pose(
+            position=Point(x=1, y=2, z=3), orientation=quaternion_from_euler(0, 0, math.pi / 2)
+        ),
+    )
+    mount = Transform(translation=Vector3(x=1, y=0, z=0), rotation=Quaternion(x=0, y=0, z=0, w=1))
+    with MemoryStore() as store:
+        target = store.stream("target", str)
+        source = store.stream("pose", PoseStamped)
+        target.append("frame", ts=10.0)
+        source.append(pose, ts=10.01)
+        before = encode(pose)
+        [result] = pose_fill(target, source, mount=mount).to_list()
+        assert result.data == "frame"
+        np.testing.assert_allclose(result.pose_tuple[:3], [1, 3, 3], atol=1e-12)
+        assert encode(pose) == before
+
+
+def test_sqlite_pose_fill_copies_original_cdr_bytes_and_exact_header(tmp_path):
+    source_path = tmp_path / "source.db"
+    destination = tmp_path / "posed.db"
+    header = Header(frame_id="lidar", stamp=time_from_nanoseconds(1700000000123456789))
+    cloud = pointcloud_from_xyz(np.array([[1, 2, 3]], dtype=np.float32), header=header)
+    pose_header = deepcopy(header)
+    pose_header.frame_id = "odom"
+    pose = PoseStamped(
+        header=pose_header,
+        pose=Pose(position=Point(x=4, y=5, z=6), orientation=Quaternion(x=0, y=0, z=0, w=1)),
+    )
+    with SqliteStore(path=str(source_path)) as store:
+        store.stream("lidar", PointCloud2).append(cloud, ts=10.0)
+        store.stream("odom", PoseStamped).append(pose, ts=10.001)
+    written = pose_fill_db(source_path, destination, target="lidar", pose_source="odom")
+    assert written == {"lidar": 1, "odom": 1}
+    with (
+        SqliteStore(path=str(source_path), must_exist=True) as original,
+        SqliteStore(path=str(destination), must_exist=True) as store,
+    ):
+        result = store.stream("lidar", PointCloud2).first()
+        assert result.pose_tuple == (4.0, 5.0, 6.0, 0.0, 0.0, 0.0, 1.0)
+        assert result.ts == 10.0
+        assert encode(result.data) == encode(cloud)
+        assert result.data.header.stamp.nanosec == 123456789
+        assert encode(store.stream("odom", PoseStamped).first().data) == encode(pose)
+        before = original.stream("lidar", PointCloud2)
+        after = store.stream("lidar", PointCloud2)
+        assert before._source.blob_store.get(
+            "lidar", before.first().id
+        ) == after._source.blob_store.get("lidar", result.id)

dimos/memory/type/observation.py

@@ -65,9 +65,8 @@ def _to_tuple(p: Any) -> PoseTuple | None:
 
     Accepts: `None`, a 3- or 7-tuple, or any object with
     `translation`+`rotation` (Transform) or `position`+`orientation`
-    (Pose / PoseStamped) attributes. 3-tuples are padded with the identity
-    quaternion. Duck-typed to avoid importing `dimos.msgs.geometry_msgs`
-    at module load.
+    (Pose) attributes, including the standard generated stamped/covariance/
+    odometry nesting. 3-tuples are padded with the identity quaternion.
     """
     if p is None:
         return None
@@ -78,6 +77,15 @@ def _to_tuple(p: Any) -> PoseTuple | None:
             x, y, z = p
             return (float(x), float(y), float(z), 0.0, 0.0, 0.0, 1.0)
         raise TypeError(f"Pose tuple must have length 3 or 7, got {len(p)}")
+    # Generated ROS messages retain their declared nesting; only unwrap the
+    # pose-bearing shapes, leaving the indexed storage tuple contract unchanged.
+    name = getattr(type(p), "__msgtype__", None)
+    if name in ("nav_msgs/msg/Odometry", "geometry_msgs/msg/PoseWithCovarianceStamped"):
+        p = p.pose.pose
+    elif name in ("geometry_msgs/msg/PoseStamped", "geometry_msgs/msg/PoseWithCovariance"):
+        p = p.pose
+    elif name == "geometry_msgs/msg/TransformStamped":
+        p = p.transform
     # Use ``is not None`` rather than ``or`` — a Vector3 at the origin is
     # falsy but valid, and falling through to ``position`` on a Transform
     # would crash.