Ivan/feat/markermerge

dimos/memory2/module.py

@@ -314,9 +314,8 @@ def on_msg(msg: Any) -> None:
                     "[%s] No tf available for frame '%s' at time %s (msg ts: %s), storing without pose",
                     name,
                     frame_id,
-                    ts,
                     getattr(msg, "ts", None),
                 )
-            stream.append(msg, ts=ts, pose=pose)
+            stream.append(msg, ts=time.time(), pose=pose)
 
         self.register_disposable(Disposable(input_topic.subscribe(on_msg)))

dimos/perception/fiducial/blueprints/desk_marker_tf.py

@@ -0,0 +1,145 @@
+# Copyright 2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from __future__ import annotations
+
+from pathlib import Path
+import threading
+import time
+
+from dimos.core.coordination.blueprints import autoconnect
+from dimos.core.core import rpc
+from dimos.core.module import Module, ModuleConfig
+from dimos.hardware.sensors.camera.module import CameraModule
+from dimos.hardware.sensors.camera.webcam import Webcam
+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.msgs.sensor_msgs.CameraInfo import CameraInfo
+from dimos.perception.fiducial.marker_tf_module import MarkerTfModule
+
+DESK_CAMERA_FRAME_ID = "camera_optical"
+DESK_MARKER_NAMESPACE_PREFIX = "marker_tf"
+DESK_MARKER_ARUCO_DICTIONARY = "DICT_APRILTAG_36h11"
+DESK_MARKER_LENGTH_M = 0.05
+DEFAULT_DESK_CAMERA_INFO_YAML = Path(__file__).resolve().parent / "fixtures" / "camera_info.yaml"
+
+
+def create_desk_webcam(
+    camera_info_yaml: str | Path = DEFAULT_DESK_CAMERA_INFO_YAML,
+    camera_index: int = 0,
+    fps: float = 15.0,
+) -> Webcam:
+    camera_info = CameraInfo.from_yaml(str(camera_info_yaml))
+    camera_info.frame_id = DESK_CAMERA_FRAME_ID
+    return Webcam(
+        camera_index=camera_index,
+        width=camera_info.width,
+        height=camera_info.height,
+        fps=fps,
+        camera_info=camera_info,
+    )
+
+
+class DeskStaticTfModuleConfig(ModuleConfig):
+    world_frame: str = "world"
+    base_frame: str = "base_link"
+    camera_optical_frame: str = "camera_optical"
+    camera_translation_m: tuple[float, float, float] = (
+        0.25,
+        0.0,
+        0.15,
+    )
+    camera_rotation_rpy_rad: tuple[float, float, float] = (0.0, 0.0, 0.0)
+    #: Republish fixed transforms at this rate so TF lookups at image timestamps stay
+    #: within MarkerTfModule tf_lookup_tolerance (single-shot stamps fall out of tolerance).
+    static_tf_republish_hz: float = 10.0
+
+
+class DeskStaticTfModule(Module):
+    """Publish the fixed desk TF chain needed by marker pose estimation."""
+
+    config: DeskStaticTfModuleConfig
+    _last_publish_ts: float | None = None
+    _republish_stop: threading.Event | None = None
+    _republish_thread: threading.Thread | None = None
+
+    @rpc
+    def start(self) -> None:
+        super().start()
+        self.publish_static_chain()
+        hz = self.config.static_tf_republish_hz
+        if hz > 0.0:
+            self._republish_stop = threading.Event()
+            period = 1.0 / hz
+
+            def _republish_loop() -> None:
+                assert self._republish_stop is not None
+                while not self._republish_stop.wait(period):
+                    self.publish_static_chain()
+
+            self._republish_thread = threading.Thread(
+                target=_republish_loop,
+                name="desk_static_tf_republish",
+                daemon=True,
+            )
+            self._republish_thread.start()
+
+    @rpc
+    def stop(self) -> None:
+        if self._republish_stop is not None:
+            self._republish_stop.set()
+        if self._republish_thread is not None:
+            self._republish_thread.join(timeout=2.0)
+            self._republish_thread = None
+        self._republish_stop = None
+        super().stop()
+
+    def publish_static_chain(self) -> None:
+        ts = time.time()
+        self._last_publish_ts = ts
+        roll, pitch, yaw = self.config.camera_rotation_rpy_rad
+        x, y, z = self.config.camera_translation_m
+
+        self.tf.publish(
+            Transform(
+                translation=Vector3(0.0, 0.0, 0.0),
+                rotation=Quaternion(0.0, 0.0, 0.0, 1.0),
+                frame_id=self.config.world_frame,
+                child_frame_id=self.config.base_frame,
+                ts=ts,
+            ),
+            Transform(
+                # Default desk camera pose: about 25 cm forward and 15 cm above base_link.
+                translation=Vector3(x, y, z),
+                rotation=Quaternion.from_euler(Vector3(roll, pitch, yaw)),
+                frame_id=self.config.base_frame,
+                child_frame_id=self.config.camera_optical_frame,
+                ts=ts,
+            ),
+        )
+
+
+desk_marker_tf = autoconnect(
+    DeskStaticTfModule.blueprint(),
+    CameraModule.blueprint(
+        hardware=create_desk_webcam,
+        transform=None,
+    ),
+    MarkerTfModule.blueprint(
+        marker_length_m=DESK_MARKER_LENGTH_M,
+        aruco_dictionary=DESK_MARKER_ARUCO_DICTIONARY,
+        marker_namespace_prefix=DESK_MARKER_NAMESPACE_PREFIX,
+    ),
+)

dimos/perception/fiducial/blueprints/fixtures/camera_info.yaml

@@ -0,0 +1,55 @@
+image_width: 1920
+image_height: 1080
+camera_name: macbook_pro_14_2025_center_stage
+distortion_model: plumb_bob
+camera_matrix:
+  rows: 3
+  cols: 3
+  data:
+  - 2236.6990940785167
+  - 0.0
+  - 989.9782243556235
+  - 0.0
+  - 2378.17472170537
+  - 568.3408769482405
+  - 0.0
+  - 0.0
+  - 1.0
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data:
+  - 1.749205286914066
+  - -24.50117288997724
+  - -0.03351506321703973
+  - -0.10786678290448087
+  - 212.1609935197354
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data:
+  - 1.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 1.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 1.0
+projection_matrix:
+  rows: 3
+  cols: 4
+  data:
+  - 2236.6990940785167
+  - 0.0
+  - 989.9782243556235
+  - 0.0
+  - 0.0
+  - 2378.17472170537
+  - 568.3408769482405
+  - 0.0
+  - 0.0
+  - 0.0
+  - 1.0
+  - 0.0

dimos/perception/fiducial/blueprints/test_desk_marker_tf.py

@@ -0,0 +1,101 @@
+# Copyright 2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from pathlib import Path
+
+from dimos.core.coordination.blueprints import Blueprint
+from dimos.hardware.sensors.camera.module import CameraModule
+from dimos.hardware.sensors.camera.webcam import Webcam
+from dimos.perception.fiducial.blueprints.desk_marker_tf import (
+    DESK_CAMERA_FRAME_ID,
+    DESK_MARKER_ARUCO_DICTIONARY,
+    DESK_MARKER_LENGTH_M,
+    DESK_MARKER_NAMESPACE_PREFIX,
+    DeskStaticTfModule,
+    create_desk_webcam,
+    desk_marker_tf,
+)
+from dimos.perception.fiducial.marker_tf_module import MarkerTfModule
+
+
+def test_desk_marker_tf_blueprint_declares_static_tf_module() -> None:
+    assert isinstance(desk_marker_tf, Blueprint)
+    assert desk_marker_tf.blueprints[0].module is DeskStaticTfModule
+    assert desk_marker_tf.blueprints[1].module is CameraModule
+    assert desk_marker_tf.blueprints[1].kwargs["hardware"] is create_desk_webcam
+    assert desk_marker_tf.blueprints[1].kwargs["transform"] is None
+    assert desk_marker_tf.blueprints[2].module is MarkerTfModule
+    assert desk_marker_tf.blueprints[2].kwargs["marker_length_m"] == DESK_MARKER_LENGTH_M
+    assert desk_marker_tf.blueprints[2].kwargs["aruco_dictionary"] == DESK_MARKER_ARUCO_DICTIONARY
+    assert (
+        desk_marker_tf.blueprints[2].kwargs["marker_namespace_prefix"]
+        == DESK_MARKER_NAMESPACE_PREFIX
+    )
+
+
+def test_create_desk_webcam_loads_camera_info_yaml(tmp_path: Path) -> None:
+    camera_info_yaml = tmp_path / "camera_info.yaml"
+    camera_info_yaml.write_text(
+        """
+image_width: 1920
+image_height: 1080
+camera_name: macbook_pro_14_2025_center_stage
+distortion_model: plumb_bob
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [2236.0, 0.0, 990.0, 0.0, 2378.0, 568.0, 0.0, 0.0, 1.0]
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [1.7, -24.5, -0.03, -0.1, 212.1]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [2236.0, 0.0, 990.0, 0.0, 0.0, 2378.0, 568.0, 0.0, 0.0, 0.0, 1.0, 0.0]
+""".lstrip()
+    )
+
+    camera = create_desk_webcam(camera_info_yaml, camera_index=1, fps=7.5)
+
+    assert isinstance(camera, Webcam)
+    assert camera.config.camera_index == 1
+    assert camera.config.width == 1920
+    assert camera.config.height == 1080
+    assert camera.config.fps == 7.5
+    assert camera.config.camera_info.frame_id == DESK_CAMERA_FRAME_ID
+
+
+def test_desk_static_tf_module_publishes_world_to_camera_optical_chain() -> None:
+    mod = DeskStaticTfModule(
+        camera_translation_m=(0.3, 0.0, 0.2),
+        camera_rotation_rpy_rad=(0.0, 0.0, 0.0),
+    )
+    try:
+        mod.start()
+        assert mod._last_publish_ts is not None
+
+        world_camera = mod.tf.get("world", "camera_optical", mod._last_publish_ts, 1.0)
+        assert world_camera is not None
+        assert world_camera.frame_id == "world"
+        assert world_camera.child_frame_id == "camera_optical"
+        assert world_camera.translation.x == 0.3
+        assert world_camera.translation.y == 0.0
+        assert world_camera.translation.z == 0.2
+    finally:
+        mod.stop()