diff --git a/data/.lfs/unitree_go2_detection_cdr.tar.gz b/data/.lfs/unitree_go2_detection_cdr.tar.gz new file mode 100644 index 00000000000..cfb22141ba5 --- /dev/null +++ b/data/.lfs/unitree_go2_detection_cdr.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:966a7b4d542a58a7891b7c0fc4951e3ea3975c56f52cd99b0a5f0541198d2d40 +size 5823304 diff --git a/dimos/experimental/memory/rust/Cargo.lock b/dimos/experimental/memory/rust/Cargo.lock index e0158b7e713..785b380c864 100644 --- a/dimos/experimental/memory/rust/Cargo.lock +++ b/dimos/experimental/memory/rust/Cargo.lock @@ -657,7 +657,7 @@ dependencies = [ [[package]] name = "dimos-lcm" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", "socket2 0.5.10", @@ -672,7 +672,7 @@ dependencies = [ "crossbeam-channel", "crossbeam-utils", "dimos-module", - "lcm-msgs", + "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen)", "lz4_flex 0.14.0", "mcap", "rayon", @@ -689,11 +689,11 @@ dependencies = [ [[package]] name = "dimos-module" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "dimos-lcm", "dimos-module-macros", - "lcm-msgs", + "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549)", "nalgebra", "rayon", "serde", @@ -709,7 +709,7 @@ dependencies = [ [[package]] name = "dimos-module-macros" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "proc-macro2", "quote", @@ -1555,6 +1555,14 @@ dependencies = [ "byteorder", ] +[[package]] +name = "lcm-msgs" +version = "0.1.0" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" +dependencies = [ + "byteorder", +] + [[package]] name = "libc" version = "0.2.189" diff --git a/dimos/experimental/memory/rust/Cargo.toml b/dimos/experimental/memory/rust/Cargo.toml index 08964427d05..056ec059fb1 100644 --- a/dimos/experimental/memory/rust/Cargo.toml +++ b/dimos/experimental/memory/rust/Cargo.toml @@ -9,7 +9,7 @@ license = "Apache-2.0" anyhow = "1" crossbeam-channel = "0.5" crossbeam-utils = "0.8" -dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6", version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } lz4_flex = "0.14.0" mcap = { version = "0.25.0", default-features = false, features = ["zstd"] } diff --git a/dimos/experimental/memory/rust/crate-hashes.json b/dimos/experimental/memory/rust/crate-hashes.json index 4c5fde86d6c..187d28513c6 100644 --- a/dimos/experimental/memory/rust/crate-hashes.json +++ b/dimos/experimental/memory/rust/crate-hashes.json @@ -1,6 +1,7 @@ { - "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module-macros@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=" + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module-macros@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=" } \ No newline at end of file diff --git a/dimos/hardware/sensors/camera/gstreamer/gstreamer_camera.py b/dimos/hardware/sensors/camera/gstreamer/gstreamer_camera.py index 055d50f9602..113f561b56e 100644 --- a/dimos/hardware/sensors/camera/gstreamer/gstreamer_camera.py +++ b/dimos/hardware/sensors/camera/gstreamer/gstreamer_camera.py @@ -26,7 +26,10 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import Out -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.message_utils.image import image_from_array +from dimos.message_utils.time import duration_from_seconds, time_from_nanoseconds, to_nanoseconds +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.std_msgs.msg import Header from dimos.utils.logging_config import setup_logger # Add system path for gi module if needed @@ -37,7 +40,8 @@ gi.require_version("Gst", "1.0") gi.require_version("GstApp", "1.0") -from gi.repository import GLib, Gst # type: ignore[import-not-found,import-untyped] +gi.require_version("GstVideo", "1.0") +from gi.repository import GLib, Gst, GstVideo # type: ignore[import-not-found,import-untyped] logger = setup_logger(level=logging.INFO) @@ -101,6 +105,7 @@ def stop(self) -> None: self._cleanup_reconnect_timer() if not self.running: + super().stop() return self.running = False @@ -251,12 +256,15 @@ def _on_new_sample(self, appsink): # type: ignore[no-untyped-def] if buffer.pts != Gst.CLOCK_TIME_NONE: # Convert nanoseconds to seconds and add offset # This is the absolute time from when the frame was captured - timestamp = (buffer.pts / 1e9) + self.config.timestamp_offset + stamp_ns = int(buffer.pts) + to_nanoseconds( + duration_from_seconds(self.config.timestamp_offset) + ) + timestamp = stamp_ns / 1e9 # Skip frames with invalid timestamps (before year 2000) # This filters out initial gray frames with relative timestamps year_2000_timestamp = 946684800.0 # January 1, 2000 00:00:00 UTC - if timestamp < year_2000_timestamp: + if stamp_ns < int(year_2000_timestamp) * 1_000_000_000: logger.debug(f"Skipping frame with invalid timestamp: {timestamp:.6f}") return Gst.FlowReturn.OK @@ -272,18 +280,22 @@ def _on_new_sample(self, appsink): # type: ignore[no-untyped-def] try: # Convert buffer data to numpy array # The videoconvert element outputs BGR format - data = np.frombuffer(map_info.data, dtype=np.uint8) - - # Reshape to image dimensions - # For BGR format, we have 3 channels - image_array = data.reshape((height, width, 3)) + info = GstVideo.VideoInfo.new_from_caps(caps) + if info.finfo.name != "BGR": + raise ValueError("The camera pipeline must supply BGR pixels") + image_array = np.ndarray( + (height, width, 3), + dtype=np.uint8, + buffer=map_info.data, + offset=info.offset[0], + strides=(info.stride[0], 3, 1), + ) # Create an Image message with the absolute timestamp - image_msg = Image( - data=image_array.copy(), # Make a copy to ensure data persistence - format=ImageFormat.BGR, - frame_id=self.frame_id, - ts=timestamp, + image_msg = image_from_array( + image_array, + encoding="bgr8", + header=Header(frame_id=self.frame_id, stamp=time_from_nanoseconds(stamp_ns)), ) # Publish the image diff --git a/dimos/hardware/sensors/camera/gstreamer/test_generated_frames.py b/dimos/hardware/sensors/camera/gstreamer/test_generated_frames.py new file mode 100644 index 00000000000..0ebfeed75c5 --- /dev/null +++ b/dimos/hardware/sensors/camera/gstreamer/test_generated_frames.py @@ -0,0 +1,105 @@ +# 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. + +"""Actual in-memory GStreamer buffers/pipelines; no camera, TCP stream or fake SDK.""" + +import gc +import threading +from unittest.mock import Mock + +from dimos_cdrgen.registry import decode, encode +import numpy as np +import pytest + +from dimos.message_utils.image import image_view +from dimos.message_utils.time import to_nanoseconds +from dimos.messages.sensor_msgs.msg import Image +from dimos.protocol.rpc.zenohrpc import ZenohRPC + +gi = pytest.importorskip("gi", reason="GStreamer acceptance requires the optional GI binding") +gi.require_version("Gst", "1.0") +from gi.repository import Gst + +from dimos.hardware.sensors.camera.gstreamer.gstreamer_camera import GstreamerCameraModule + + +@pytest.fixture +def camera(mocker): + mocker.patch.object(ZenohRPC, "start") + mocker.patch.object(ZenohRPC, "serve_module_rpc") + instance = GstreamerCameraModule(frame_id="camera_test") + yield instance + instance.stop() + + +@pytest.mark.parametrize("offset,delta_ns", [(0.0, 0), (1e-9, 1), (-1e-9, -1)]) +def test_actual_pipeline_copies_padded_bgr_and_exact_stamp( + camera, mocker, offset, delta_ns +) -> None: + camera.config.timestamp_offset = offset + camera.running = True + pipeline = Gst.parse_launch( + "appsrc name=source format=time ! video/x-raw,format=BGR,width=3,height=2,framerate=30/1 " + "! appsink name=sink emit-signals=true sync=false" + ) + sink, source = pipeline.get_by_name("sink"), pipeline.get_by_name("source") + sink.connect("new-sample", camera._on_new_sample) + received = threading.Event() + published = [] + + def publish(value): + published.append(value) + received.set() + + mocker.patch.object(camera.video, "publish", side_effect=publish) + pixels = np.arange(18, dtype=np.uint8).reshape(2, 3, 3) + raw = bytearray([99] * 24) + np.ndarray((2, 3, 3), dtype=np.uint8, buffer=raw, strides=(12, 3, 1))[:] = pixels + buffer = Gst.Buffer.new_allocate(None, len(raw), None) + buffer.fill(0, raw) + buffer.pts = 1700000000123456789 + try: + pipeline.set_state(Gst.State.PLAYING) + assert source.emit("push-buffer", buffer) == Gst.FlowReturn.OK + source.emit("end-of-stream") + assert received.wait(timeout=5) + finally: + pipeline.set_state(Gst.State.NULL) + del buffer, raw, pipeline + gc.collect() + assert len(published) == 1 + assert type(published[0]) is Image + message = decode(encode(published[0]), Image) + assert message.encoding == "bgr8" and message.step == 9 + assert to_nanoseconds(message.header.stamp) == 1700000000123456789 + delta_ns + assert message.header.frame_id == "camera_test" + np.testing.assert_array_equal(image_view(message), pixels) + + +def test_invalid_source_stamp_keeps_existing_skip_behavior(camera, mocker) -> None: + buffer = Gst.Buffer.new_allocate(None, 24, None) + buffer.pts = 0 + caps = Gst.Caps.from_string("video/x-raw,format=BGR,width=3,height=2,framerate=30/1") + sample = Gst.Sample.new(buffer, caps, None, None) + sink = Mock() + sink.emit.return_value = sample + publish = mocker.patch.object(camera.video, "publish") + assert camera._on_new_sample(sink) == Gst.FlowReturn.OK + publish.assert_not_called() + + +def test_stop_closes_module_loop_when_pipeline_was_never_started(camera): + loop_thread = camera._loop_thread + camera.stop() + assert not loop_thread.is_alive() diff --git a/dimos/hardware/sensors/camera/module.py b/dimos/hardware/sensors/camera/module.py index 83020165da7..37a33781b4c 100644 --- a/dimos/hardware/sensors/camera/module.py +++ b/dimos/hardware/sensors/camera/module.py @@ -13,9 +13,10 @@ # limitations under the License. from collections.abc import Callable +from copy import deepcopy import time -from pydantic import Field +from pydantic import Field, InstanceOf import reactivex as rx from dimos.core.coordination.blueprints import autoconnect @@ -25,28 +26,32 @@ from dimos.core.stream import Out from dimos.hardware.sensors.camera.spec import CameraHardware 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.msgs.sensor_msgs.Image import Image, sharpness_barrier -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.message_utils.image import image_sharpness +from dimos.message_utils.time import time_from_nanoseconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.sensor_msgs.msg import CameraInfo, Image +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage from dimos.spec import perception +from dimos.utils.reactive import quality_barrier from dimos.visualization.vis_module import vis_module -def default_transform() -> Transform: - return Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", +def default_transform() -> TransformStamped: + return TransformStamped( + header=Header(frame_id="base_link", stamp=Time(sec=0, nanosec=0)), child_frame_id="camera_link", + transform=Transform( + rotation=Quaternion(w=1.0, x=0.0, y=0.0, z=0.0), + translation=Vector3(x=0.0, y=0.0, z=0.0), + ), ) class CameraModuleConfig(ModuleConfig): frame_id: str = "camera_link" - transform: Transform | None = Field(default_factory=default_transform) + transform: InstanceOf[TransformStamped] | None = Field(default_factory=default_transform) hardware: Callable[[], CameraHardware] | CameraHardware = Webcam frequency: float = 0.0 # Hz, 0 means no limit @@ -71,7 +76,7 @@ def start(self) -> None: stream = self.hardware.image_stream() if self.config.frequency > 0: - stream = stream.pipe(sharpness_barrier(self.config.frequency)) + stream = stream.pipe(quality_barrier(image_sharpness, self.config.frequency)) self.register_disposable( stream.subscribe(self.color_image.publish), @@ -82,24 +87,24 @@ def start(self) -> None: ) def publish_metadata(self) -> None: - camera_info = self.hardware.camera_info.with_ts(time.time()) + camera_info = deepcopy(self.hardware.camera_info) + camera_info.header.stamp = time_from_nanoseconds(time.time_ns()) self.camera_info.publish(camera_info) if not self.config.transform: return - camera_link = self.config.transform - camera_link.ts = camera_info.ts - - camera_optical = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), - frame_id="camera_link", - child_frame_id="camera_optical", - ts=camera_link.ts, + camera_link = deepcopy(self.config.transform) + camera_link.header.stamp = camera_info.header.stamp + camera_optical = TransformStamped( + header=Header(frame_id=camera_link.child_frame_id, stamp=camera_info.header.stamp), + child_frame_id=camera_info.header.frame_id, + transform=Transform( + rotation=Quaternion(x=-0.5, y=0.5, z=-0.5, w=0.5), + translation=Vector3(x=0.0, y=0.0, z=0.0), + ), ) - - self.tf.publish(TFMessage(camera_link, camera_optical)) + self.tf.publish(TFMessage(transforms=[camera_link, camera_optical])) @rpc def stop(self) -> None: diff --git a/dimos/hardware/sensors/camera/realsense/camera.py b/dimos/hardware/sensors/camera/realsense/camera.py index 4314fc51d8d..91cd5f5df16 100644 --- a/dimos/hardware/sensors/camera/realsense/camera.py +++ b/dimos/hardware/sensors/camera/realsense/camera.py @@ -19,17 +19,16 @@ from typing import TYPE_CHECKING, Any -from pydantic import Field +from pydantic import Field, InstanceOf from dimos.core.native_module import NativeModule, NativeModuleConfig -from dimos.core.stream import Out +from dimos.core.stream import In, Out from dimos.hardware.sensors.camera.spec import DepthCameraConfig -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.ImuInfo import ImuInfo -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.dimos_msgs.msg import ImuInfo +from dimos.messages.sensor_msgs.msg import CameraInfo, Image, Imu, PointCloud2 +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage from dimos.spec import perception @@ -60,13 +59,15 @@ class RealSenseCameraConfig(NativeModuleConfig, DepthCameraConfig): enable_imu: bool = False # Gyro rate, and so the Imu output rate. imu_hz: int = 400 - imu_info: ImuInfo | None = Field( + imu_info: InstanceOf[ImuInfo] | None = Field( # default value based on a d455, but this should be measured for every physical camera (every d455,d435,etc) default_factory=lambda: ImuInfo( gyro_noise_density=2.0e-4, gyro_random_walk=1.0e-5, accel_noise_density=1.8e-3, accel_random_walk=1.0e-4, + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + frequency=0.0, ) ) pointcloud_fps: float = 5.0 @@ -110,6 +111,7 @@ class RealSenseCamera(NativeModule, perception.DepthCamera): infrared_left_camera_info: Out[CameraInfo] infrared_right_camera_info: Out[CameraInfo] tf: Out[TFMessage] + tf_static: In[TFMessage] if TYPE_CHECKING: diff --git a/dimos/hardware/sensors/camera/realsense/rust/Cargo.lock b/dimos/hardware/sensors/camera/realsense/rust/Cargo.lock index 89422d755e8..9358764ea78 100644 --- a/dimos/hardware/sensors/camera/realsense/rust/Cargo.lock +++ b/dimos/hardware/sensors/camera/realsense/rust/Cargo.lock @@ -243,6 +243,15 @@ dependencies = [ "shlex", ] +[[package]] +name = "cdr" +version = "0.2.4" +source = "git+https://github.com/TomCC7/cdr-rs?rev=38a4368255d408faf374726653b39d4a4e9735cb#38a4368255d408faf374726653b39d4a4e9735cb" +dependencies = [ + "byteorder", + "serde", +] + [[package]] name = "cfg-if" version = "1.0.4" @@ -539,10 +548,21 @@ dependencies = [ "subtle", ] +[[package]] +name = "dimos-generated-messages" +version = "0.0.14" +dependencies = [ + "cdr", + "heapless", + "ros2msg", + "serde", + "serde-big-array", +] + [[package]] name = "dimos-lcm" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", "socket2 0.5.10", @@ -551,9 +571,10 @@ dependencies = [ [[package]] name = "dimos-module" -version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +version = "0.2.0" dependencies = [ + "cdr", + "dimos-generated-messages", "dimos-lcm", "dimos-module-macros", "lcm-msgs", @@ -564,6 +585,7 @@ dependencies = [ "tokio", "tracing", "tracing-subscriber", + "transforms", "url", "validator", "zenoh", @@ -572,7 +594,6 @@ dependencies = [ [[package]] name = "dimos-module-macros" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" dependencies = [ "proc-macro2", "quote", @@ -584,8 +605,8 @@ dependencies = [ name = "dimos-realsense" version = "0.1.0" dependencies = [ + "dimos-generated-messages", "dimos-module", - "lcm-msgs", "pkg-config", "realsense-rust", "realsense-sys", @@ -926,6 +947,15 @@ version = "0.33.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "c426daf70099baff33fac0ba85151c42424861c56828cef9ba02dacb78eb6b26" +[[package]] +name = "hash32" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "47d60b12902ba28e2730cd37e95b8c9223af2808df9e902d4df49588d1470606" +dependencies = [ + "byteorder", +] + [[package]] name = "hashbrown" version = "0.12.3" @@ -967,6 +997,28 @@ name = "hashbrown" version = "0.17.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ed5909b6e89a2db4456e54cd5f673791d7eca6732202bbf2a9cc504fe2f9b84a" +dependencies = [ + "allocator-api2", + "equivalent", + "foldhash 0.2.0", +] + +[[package]] +name = "heapless" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +dependencies = [ + "hash32", + "serde", + "stable_deref_trait", +] + +[[package]] +name = "heck" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" [[package]] name = "hermit-abi" @@ -1360,7 +1412,7 @@ checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" [[package]] name = "lcm-msgs" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", ] @@ -1918,6 +1970,16 @@ dependencies = [ "zerocopy", ] +[[package]] +name = "prettyplease" +version = "0.2.37" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "479ca8adacdd7ce8f1fb39ce9ecccbfe93a3f1344b3d0d97f20bc0196208f62b" +dependencies = [ + "proc-macro2", + "syn 2.0.119", +] + [[package]] name = "proc-macro-crate" version = "3.5.0" @@ -2294,6 +2356,23 @@ dependencies = [ "unicode-ident", ] +[[package]] +name = "ros2msg" +version = "0.5.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2fe291be5cb6d4b8e6bf9c9de80c40bd1cb09ecac50deafa2fdcae0027e04988" +dependencies = [ + "heck", + "pest", + "pest_derive", + "prettyplease", + "proc-macro2", + "quote", + "regex", + "syn 2.0.119", + "thiserror 2.0.20", +] + [[package]] name = "rustc-hash" version = "2.1.3" @@ -2534,6 +2613,15 @@ dependencies = [ "serde_derive", ] +[[package]] +name = "serde-big-array" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11fc7cc2c76d73e0f27ee52abbd64eec84d46f370c88371120433196934e4b7f" +dependencies = [ + "serde", +] + [[package]] name = "serde_core" version = "1.0.229" @@ -3227,6 +3315,18 @@ dependencies = [ "tracing-serde", ] +[[package]] +name = "transforms" +version = "2.2.0" +source = "git+https://github.com/TomCC7/transforms?rev=5ae9563de0c02638c04e393aadfad28444a4398e#5ae9563de0c02638c04e393aadfad28444a4398e" +dependencies = [ + "approx", + "hashbrown 0.17.1", + "libm", + "serde", + "thiserror 2.0.20", +] + [[package]] name = "twox-hash" version = "1.6.3" diff --git a/dimos/hardware/sensors/camera/realsense/rust/Cargo.toml b/dimos/hardware/sensors/camera/realsense/rust/Cargo.toml index 75aff44563c..70a886b5f9e 100644 --- a/dimos/hardware/sensors/camera/realsense/rust/Cargo.toml +++ b/dimos/hardware/sensors/camera/realsense/rust/Cargo.toml @@ -17,24 +17,28 @@ path = "dimos_realsense::RealSense" python = "dimos.hardware.sensors.camera.realsense.camera:RealSenseCamera" [package.metadata.dimos.module.realsense.inputs] -tf = "tf2_msgs.TFMessage" +tf = "tf2_msgs/msg/TFMessage" +tf_static = "tf2_msgs/msg/TFMessage" [package.metadata.dimos.module.realsense.outputs] -color_image = "sensor_msgs.Image" -depth_image = "sensor_msgs.Image" -infrared_left = "sensor_msgs.Image" -infrared_right = "sensor_msgs.Image" -imu = "sensor_msgs.Imu" -imu_info = "sensor_msgs.ImuInfo" -pointcloud = "sensor_msgs.PointCloud2" -camera_info = "sensor_msgs.CameraInfo" -depth_camera_info = "sensor_msgs.CameraInfo" -infrared_left_camera_info = "sensor_msgs.CameraInfo" -infrared_right_camera_info = "sensor_msgs.CameraInfo" +color_image = "sensor_msgs/msg/Image" +depth_image = "sensor_msgs/msg/Image" +infrared_left = "sensor_msgs/msg/Image" +infrared_right = "sensor_msgs/msg/Image" +imu = "sensor_msgs/msg/Imu" +imu_info = "dimos_msgs/msg/ImuInfo" +pointcloud = "sensor_msgs/msg/PointCloud2" +camera_info = "sensor_msgs/msg/CameraInfo" +depth_camera_info = "sensor_msgs/msg/CameraInfo" +infrared_left_camera_info = "sensor_msgs/msg/CameraInfo" +infrared_right_camera_info = "sensor_msgs/msg/CameraInfo" + +[features] +docs-only = ["realsense-rust/docs-only"] [dependencies] -dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } -lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } +dimos-module = { path = "../../../../../../native/rust/dimos-module" } +dimos-generated-messages = { path = "../../../../../../build/message-source/rust" } realsense-rust = "1.3" realsense-sys = "2.56" tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal"] } diff --git a/dimos/hardware/sensors/camera/realsense/rust/build.rs b/dimos/hardware/sensors/camera/realsense/rust/build.rs index d0deae698cd..934e8ab9e4e 100644 --- a/dimos/hardware/sensors/camera/realsense/rust/build.rs +++ b/dimos/hardware/sensors/camera/realsense/rust/build.rs @@ -17,6 +17,9 @@ // names lib/x86_64-linux-gnu while the .so sits in lib/, so walk up to it -- // for the linker too, or `-lrealsense2` is not found at all. fn main() { + if std::env::var_os("CARGO_FEATURE_DOCS_ONLY").is_some() { + return; + } let lib = pkg_config::probe_library("realsense2").expect("librealsense2 via pkg-config"); for path in lib.link_paths { let dir = path diff --git a/dimos/hardware/sensors/camera/realsense/rust/crate-hashes.json b/dimos/hardware/sensors/camera/realsense/rust/crate-hashes.json index 4c5fde86d6c..3ee4f926e23 100644 --- a/dimos/hardware/sensors/camera/realsense/rust/crate-hashes.json +++ b/dimos/hardware/sensors/camera/realsense/rust/crate-hashes.json @@ -1,6 +1,6 @@ { - "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", - "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module-macros@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=" + "git+https://github.com/TomCC7/cdr-rs?rev=38a4368255d408faf374726653b39d4a4e9735cb#cdr@0.2.4": "sha256-akg/bLBY3wQ1YRr2bijE5ZqB211Cst5lKjELLrgiG6o=", + "git+https://github.com/TomCC7/transforms?rev=5ae9563de0c02638c04e393aadfad28444a4398e#transforms@2.2.0": "sha256-Kh8aVjj7zQ+Xd1QHOcAhSJbubg7azaWFSMpaI/ZAgM4=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=" } \ No newline at end of file diff --git a/dimos/hardware/sensors/camera/realsense/rust/flake.nix b/dimos/hardware/sensors/camera/realsense/rust/flake.nix index a2a96eaefaf..ed2baaa98d8 100644 --- a/dimos/hardware/sensors/camera/realsense/rust/flake.nix +++ b/dimos/hardware/sensors/camera/realsense/rust/flake.nix @@ -15,9 +15,31 @@ pkgs = nixpkgs.legacyPackages.${system}; name = "dimos-realsense"; - src = nix-filter.lib { root = ./.; exclude = [ "target" "build" "result" "__pycache__" ]; }; - - generated = crate2nix.tools.${system}.generatedCargoNix { inherit name src; }; + # Keep this flake self-contained: shared SDK/configuration arrive through + # one fixed-hash project source, rather than paths outside this directory. + sharedArchive = pkgs.fetchurl { + url = "https://codeload.github.com/dimensionalOS/dimos/tar.gz/a9365487b4323151ea85041c6a944b509c0c3268"; + sha256 = "e339487dd2281980fcde7ab03c46dfd2e274491e0f73e66d9c6cbcaeea8902b2"; + }; + sharedSource = pkgs.runCommand "dimos-cdr-sdk-source" { + nativeBuildInputs = [ pkgs.gnutar pkgs.gzip ]; + } '' + mkdir -p $out + tar -xzf ${sharedArchive} --strip-components=1 -C $out + ''; + messages = import "${sharedSource}/native/message-source.nix" { inherit pkgs; }; + cameraSource = nix-filter.lib { root = ./.; exclude = [ "target" "build" "result" "__pycache__" ]; }; + src = pkgs.runCommand "dimos-realsense-cdr-source" { } '' + mkdir -p $out/native/rust $out/build/message-source $out/dimos/hardware/sensors/camera/realsense/rust + cp -r ${sharedSource}/native/rust/dimos-module $out/native/rust/ + cp -r ${sharedSource}/native/rust/dimos-module-macros $out/native/rust/ + cp -r ${messages.source}/rust $out/build/message-source/rust + cp -r ${cameraSource}/. $out/dimos/hardware/sensors/camera/realsense/rust/ + ''; + generated = crate2nix.tools.${system}.generatedCargoNix { + inherit name src; + cargoToml = "dimos/hardware/sensors/camera/realsense/rust/Cargo.toml"; + }; needsLibrealsense = _: { buildInputs = [ pkgs.librealsense ]; diff --git a/dimos/hardware/sensors/camera/realsense/rust/src/main.rs b/dimos/hardware/sensors/camera/realsense/rust/src/main.rs index 36de88262f3..be469686337 100644 --- a/dimos/hardware/sensors/camera/realsense/rust/src/main.rs +++ b/dimos/hardware/sensors/camera/realsense/rust/src/main.rs @@ -18,20 +18,28 @@ use std::collections::{BTreeSet, HashMap, HashSet, VecDeque}; use std::ffi::{CStr, CString}; -use std::io::Write; use std::os::raw::c_void; use std::ptr::{self, NonNull}; use std::sync::atomic::{AtomicBool, Ordering}; use std::sync::{mpsc, Arc, Mutex}; use std::time::{Duration, SystemTime, UNIX_EPOCH}; +use dimos_generated_messages::builtin_interfaces::msg::time::Time; +use dimos_generated_messages::dimos_msgs::msg::imu_info::ImuInfo; +use dimos_generated_messages::geometry_msgs::msg::{ + quaternion::Quaternion as QuaternionMsg, vector3::Vector3 as Vector3Msg, +}; +use dimos_generated_messages::sensor_msgs::msg::{ + camera_info::CameraInfo, image::Image, imu::Imu, point_cloud2::PointCloud2, + point_field::PointField, region_of_interest::RegionOfInterest, +}; +use dimos_generated_messages::std_msgs::msg::header::Header; + +use dimos_module::cdr; use dimos_module::nalgebra::{ Isometry3, Matrix3, Quaternion, Rotation3, Translation3, UnitQuaternion, Vector3, }; -use dimos_module::{native_config, run_with_transport, Module, Output, Tf, Transform}; -use lcm_msgs::geometry_msgs::{Quaternion as QuaternionMsg, Vector3 as Vector3Msg}; -use lcm_msgs::sensor_msgs::{CameraInfo, Image, Imu, PointCloud2, PointField, RegionOfInterest}; -use lcm_msgs::std_msgs::{Header, Time}; +use dimos_module::{native_config, run_with_transport, Module, Output, Tf}; use realsense_rust::base::{Rs2Extrinsics, Rs2Intrinsics}; use realsense_rust::config::Config as RsConfig; use realsense_rust::context::Context; @@ -56,9 +64,6 @@ const IMU_CLOCK_WINDOW_SECONDS: i64 = 30; const FRAME_TIMEOUT: Duration = Duration::from_secs(1); // Body (REP-103) from optical axes, x y z w. const OPTICAL_ROTATION: [f64; 4] = [-0.5, 0.5, -0.5, 0.5]; -// lcm-gen fingerprint of sensor_msgs.ImuInfo; it has no generated bindings. -const IMU_INFO_FINGERPRINT: u64 = 0xe6aa5563f2a33280; - // ---- config, field for field with RealSenseCameraConfig ---- #[derive(Debug, Clone, Deserialize, Serialize)] @@ -170,36 +175,6 @@ impl Config { } } -// ---- sensor_msgs.ImuInfo, hand-encoded ---- - -#[derive(Clone)] -struct ImuInfo { - header: Header, - gyro_noise_density: f64, - gyro_random_walk: f64, - accel_noise_density: f64, - accel_random_walk: f64, - frequency: f64, -} - -impl ImuInfo { - fn encode(&self) -> Vec { - let mut buf = Vec::with_capacity(8 + self.header.encoded_size() + 40); - buf.write_all(&IMU_INFO_FINGERPRINT.to_be_bytes()).unwrap(); - self.header.encode_one(&mut buf).unwrap(); - for v in [ - self.gyro_noise_density, - self.gyro_random_walk, - self.accel_noise_density, - self.accel_random_walk, - self.frequency, - ] { - buf.write_all(&v.to_be_bytes()).unwrap(); - } - buf - } -} - // ---- module ---- /// What the capture thread learns at start and the tickers publish from. @@ -250,27 +225,27 @@ struct Outs { #[derive(Module)] #[module(name = "realsense", setup = start, teardown = stop)] struct RealSense { - #[output(encode = Image::encode)] + #[output(encode = cdr::encode)] color_image: Output, - #[output(encode = Image::encode)] + #[output(encode = cdr::encode)] depth_image: Output, - #[output(encode = Image::encode)] + #[output(encode = cdr::encode)] infrared_left: Output, - #[output(encode = Image::encode)] + #[output(encode = cdr::encode)] infrared_right: Output, - #[output(encode = Imu::encode)] + #[output(encode = cdr::encode)] imu: Output, - #[output(encode = ImuInfo::encode)] + #[output(encode = cdr::encode)] imu_info: Output, - #[output(encode = PointCloud2::encode)] + #[output(encode = cdr::encode)] pointcloud: Output, - #[output(encode = CameraInfo::encode)] + #[output(encode = cdr::encode)] camera_info: Output, - #[output(encode = CameraInfo::encode)] + #[output(encode = cdr::encode)] depth_camera_info: Output, - #[output(encode = CameraInfo::encode)] + #[output(encode = cdr::encode)] infrared_left_camera_info: Output, - #[output(encode = CameraInfo::encode)] + #[output(encode = cdr::encode)] infrared_right_camera_info: Output, #[tf] tf: Tf, @@ -358,12 +333,16 @@ fn unix_now() -> f64 { } fn header(frame_id: &str, ts: f64) -> Header { - let sec = ts.trunc(); + let mut sec = ts.floor(); + let mut nanosec = ((ts - sec) * 1e9).round() as u32; + if nanosec >= 1_000_000_000 { + sec += 1.0; + nanosec -= 1_000_000_000; + } Header { - seq: 0, stamp: Time { sec: sec as i32, - nsec: ((ts - sec) * 1e9) as i32, + nanosec, }, frame_id: frame_id.to_string(), } @@ -390,11 +369,11 @@ fn image( }; Image { header: header(frame_id, ts), - height: frame.height() as i32, - width: frame.width() as i32, + height: frame.height() as u32, + width: frame.width() as u32, encoding: encoding.to_string(), is_bigendian: 0, - step: (frame.width() * bytes_per_pixel) as i32, + step: (frame.width() * bytes_per_pixel) as u32, data: data.to_vec(), } } @@ -411,13 +390,13 @@ fn camera_info(intrinsics: &Rs2Intrinsics, frame_id: &str) -> CameraInfo { }; CameraInfo { header: header(frame_id, 0.0), - height: intrinsics.height() as i32, - width: intrinsics.width() as i32, + height: intrinsics.height() as u32, + width: intrinsics.width() as u32, distortion_model: model.to_string(), - D: intrinsics.0.coeffs.iter().map(|&c| c as f64).collect(), - K: [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0], - R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], - P: [fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0], + d: intrinsics.0.coeffs.iter().map(|&c| c as f64).collect(), + k: [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0], + r: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], + p: [fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0], binning_x: 0, binning_y: 0, roi: RegionOfInterest { @@ -677,7 +656,7 @@ fn capture_thread(cfg: &Config, shared: &Shared, handle: &Handle, outs: &Outs) { // The pair is rectified, so the whole of their offset is along x. if let (Some(info), Ok(ext)) = (infra2_info.as_mut(), p2.extrinsics(p1)) { let baseline = (ext.translation()[0] as f64).abs(); - info.P[3] = -info.P[0] * baseline; + info.p[3] = -info.p[0] * baseline; } } } @@ -842,7 +821,13 @@ fn capture_thread(cfg: &Config, shared: &Shared, handle: &Handle, outs: &Outs) { if let Some(ts) = latest { let mut transforms = Vec::with_capacity(mount_edges.len()); for (parent, child, iso) in &mount_edges { - transforms.push(Transform::new(parent.clone(), child.clone(), ts, *iso)); + let stamp = header(parent, ts).stamp; + transforms.push(dimos_module::tf::from_isometry( + parent.clone(), + child.clone(), + stamp.clone(), + *iso, + )); } let _ = handle.block_on(outs.tf.publish(&transforms)); } @@ -937,10 +922,10 @@ fn pointcloud_thread(cfg: &Config, shared: &Shared, handle: &Handle, outs: &Outs } fn empty_cloud(header: &Header) -> PointCloud2 { - let field = |name: &str, offset: i32| PointField { + let field = |name: &str, offset: u32| PointField { name: name.to_string(), offset, - datatype: PointField::FLOAT32 as u8, + datatype: PointField::FLOAT32, count: 1, }; PointCloud2 { @@ -968,8 +953,8 @@ fn rgbd_to_cloud( stride: usize, depth_trunc_m: f32, ) -> PointCloud2 { - let (fx, fy) = (info.K[0] as f32, info.K[4] as f32); - let (cx, cy) = (info.K[2] as f32, info.K[5] as f32); + let (fx, fy) = (info.k[0] as f32, info.k[4] as f32); + let (cx, cy) = (info.k[2] as f32, info.k[5] as f32); let (w, h) = (rgbd.depth.width as usize, rgbd.depth.height as usize); let rgb = &rgbd.color.data; if rgb.len() != w * h * 3 || rgbd.depth.data.len() != w * h * 2 { @@ -1020,13 +1005,13 @@ fn rgbd_to_cloud( ((c[0] * 255.0) as u32) << 16 | ((c[1] * 255.0) as u32) << 8 | (c[2] * 255.0) as u32; data.extend_from_slice(&packed.to_le_bytes()); } - let field = |name: &str, offset: i32| PointField { + let field = |name: &str, offset: u32| PointField { name: name.to_string(), offset, - datatype: PointField::FLOAT32 as u8, + datatype: PointField::FLOAT32, count: 1, }; - let n = points.len() as i32; + let n = points.len() as u32; PointCloud2 { header: rgbd.depth.header.clone(), height: if n == 0 { 0 } else { 1 }, @@ -1374,6 +1359,91 @@ async fn main() { mod tests { use super::*; + fn export_fixture(name: &str, value: &T) { + use std::io::Write; + if let Some(directory) = std::env::var_os("DIMOS_REALSENSE_CDR_FIXTURES") { + let path = std::path::Path::new(&directory).join(name); + std::fs::OpenOptions::new() + .write(true) + .create_new(true) + .open(path) + .expect("create a new requested fixture") + .write_all(&cdr::encode(value).unwrap()) + .unwrap(); + } + } + + #[test] + fn generated_calibration_and_rgbd_cloud_keep_source_stamp_and_matching_colors() { + let intrinsics = Rs2Intrinsics(sys::rs2_intrinsics { + width: 2, + height: 1, + ppx: 0.0, + ppy: 0.0, + fx: 2.0, + fy: 2.0, + model: Rs2DistortionModel::None as sys::rs2_distortion, + coeffs: [0.0; 5], + }); + let info = camera_info(&intrinsics, "camera_optical"); + assert_eq!(info.k, [2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 1.0]); + let source = header("camera_optical", 3.25); + let rgbd = Rgbd { + color: Image { + header: source.clone(), + width: 2, + height: 1, + encoding: "rgb8".into(), + is_bigendian: 0, + step: 6, + data: vec![255, 0, 0, 0, 0, 255], + }, + depth: Image { + header: source.clone(), + width: 2, + height: 1, + encoding: "16UC1".into(), + is_bigendian: 0, + step: 4, + data: [1000u16.to_le_bytes(), 2000u16.to_le_bytes()].concat(), + }, + }; + let cloud = rgbd_to_cloud(&rgbd, &info, 0.001, 1, 3.0); + let decoded: PointCloud2 = cdr::decode(&cdr::encode(&cloud).unwrap()).unwrap(); + assert_eq!(decoded.header, source); + assert_eq!( + ( + decoded.width, + decoded.height, + decoded.point_step, + decoded.row_step + ), + (2, 1, 16, 32) + ); + for (chunk, point) in decoded + .data + .as_chunks::<16>() + .0 + .iter() + .zip([[0.0f32, 0.0, 1.0], [1.0, 0.0, 2.0]]) + { + for (value, expected) in chunk[..12].as_chunks::<4>().0.iter().zip(point) { + assert!((f32::from_le_bytes(*value) - expected).abs() < 1e-6); + } + } + assert_eq!( + u32::from_le_bytes(decoded.data[12..16].try_into().unwrap()), + 0x00ff0000 + ); + assert_eq!( + u32::from_le_bytes(decoded.data[28..32].try_into().unwrap()), + 0x000000ff + ); + export_fixture("cloud.cdr", &cloud); + export_fixture("image.cdr", &rgbd.color); + export_fixture("calibration.cdr", &info); + } + #[test] fn pairer_interpolates_accel_at_gyro_time() { let mut p = ImuPairer::default(); @@ -1406,10 +1476,10 @@ mod tests { accel_random_walk: 4.0, frequency: 5.0, }; - let bytes = msg.encode(); - assert_eq!(&bytes[..8], &IMU_INFO_FINGERPRINT.to_be_bytes()); - assert_eq!(bytes.len(), 8 + msg.header.encoded_size() + 40); - assert_eq!(&bytes[bytes.len() - 8..], &5.0f64.to_be_bytes()); + let bytes = cdr::encode(&msg).unwrap(); + assert_eq!(&bytes[..4], &[0, 1, 0, 0]); + assert_eq!(cdr::decode::(&bytes).unwrap(), msg); + export_fixture("imu-info.cdr", &msg); } } @@ -1427,20 +1497,20 @@ mod bench { let rgbd = Rgbd { color: Image { header: header("f", 0.0), - height: h as i32, - width: w as i32, + height: h as u32, + width: w as u32, encoding: "rgb8".into(), is_bigendian: 0, - step: (w * 3) as i32, + step: (w * 3) as u32, data: vec![128; w * h * 3], }, depth: Image { header: header("f", 0.0), - height: h as i32, - width: w as i32, + height: h as u32, + width: w as u32, encoding: "16UC1".into(), is_bigendian: 0, - step: (w * 2) as i32, + step: (w * 2) as u32, data: depth, }, }; @@ -1463,7 +1533,7 @@ mod bench { let cloud = rgbd_to_cloud(&rgbd, &info, 0.001, stride, 5.0); let build = t.elapsed(); let t = std::time::Instant::now(); - let bytes = cloud.encode(); + let bytes = cdr::encode(&cloud).unwrap(); eprintln!( "stride={stride} points={} build={:?} encode={:?} bytes={}", cloud.width, diff --git a/dimos/hardware/sensors/camera/realsense/test_camera.py b/dimos/hardware/sensors/camera/realsense/test_camera.py index f05616c758f..dd71d4ecc48 100644 --- a/dimos/hardware/sensors/camera/realsense/test_camera.py +++ b/dimos/hardware/sensors/camera/realsense/test_camera.py @@ -17,6 +17,7 @@ import json from pathlib import Path import sys +from typing import get_args, get_origin, get_type_hints if sys.version_info >= (3, 11): import tomllib @@ -24,6 +25,7 @@ import tomli as tomllib from dimos.core.native_module import NativeModuleConfig +from dimos.core.stream import IO, In, Out from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera, RealSenseCameraConfig _RUST_MANIFEST = Path(__file__).parent / "rust" / "Cargo.toml" @@ -55,12 +57,14 @@ def test_rust_struct_has_every_config_key() -> None: def test_ports_match_registry() -> None: def ports(cls: type) -> set[str]: return { - name - for name, hint in cls.__annotations__.items() - if getattr(hint, "__origin__", None) is not None or "Out[" in str(hint) + name for name, hint in get_type_hints(cls).items() if get_origin(hint) in (In, Out, IO) } - {"config"} manifest = tomllib.loads(_RUST_MANIFEST.read_text()) registry = manifest["package"]["metadata"]["dimos"]["module"]["realsense"] # The registry files a #[tf] port under inputs, as it subscribes too. assert set(registry["outputs"]) | set(registry["inputs"]) == ports(RealSenseCamera) + hints = get_type_hints(RealSenseCamera) + for name, expected in {**registry["outputs"], **registry["inputs"]}.items(): + value_type = get_args(hints[name])[0] + assert value_type.__msgtype__ == expected diff --git a/dimos/hardware/sensors/camera/spec.py b/dimos/hardware/sensors/camera/spec.py index ee6d2c9ee3b..4a90f666f4e 100644 --- a/dimos/hardware/sensors/camera/spec.py +++ b/dimos/hardware/sensors/camera/spec.py @@ -16,12 +16,11 @@ from reactivex.observable import Observable -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image +from dimos.messages.geometry_msgs.msg import Quaternion +from dimos.messages.sensor_msgs.msg import CameraInfo, Image from dimos.protocol.service.spec import BaseConfig, Configurable -OPTICAL_ROTATION = Quaternion(-0.5, 0.5, -0.5, 0.5) +OPTICAL_ROTATION = Quaternion(x=-0.5, y=0.5, z=-0.5, w=0.5) class CameraConfig(BaseConfig): diff --git a/dimos/hardware/sensors/camera/test_module.py b/dimos/hardware/sensors/camera/test_module.py new file mode 100644 index 00000000000..3d7bc41eab0 --- /dev/null +++ b/dimos/hardware/sensors/camera/test_module.py @@ -0,0 +1,73 @@ +# 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. + +"""Camera metadata uses the same generated header and frame chain as its images.""" + +from dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode +import numpy as np +import pytest + +from dimos.hardware.sensors.camera.module import CameraModule +from dimos.hardware.sensors.camera.webcam import Webcam +from dimos.message_utils.geometry import transform_matrix +from dimos.messages.sensor_msgs.msg import CameraInfo +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.protocol.rpc.zenohrpc import ZenohRPC + + +@pytest.fixture +def module(mocker): + mocker.patch.object(ZenohRPC, "start") + mocker.patch.object(ZenohRPC, "serve_module_rpc") + camera = CameraModule() + camera.hardware = Webcam(frame_id_prefix="robot") + try: + yield camera + finally: + camera.stop() + + +def test_metadata_preserves_calibration_and_does_not_mutate_mount_template(module, mocker): + info_publish = mocker.patch.object(module.camera_info, "publish") + tf_publish = mocker.patch.object(module.tf, "publish") + mocker.patch( + "dimos.hardware.sensors.camera.module.time.time_ns", return_value=1700000000123456789 + ) + original = cdr_encode(module.config.transform) + + module.publish_metadata() + + info_publish.assert_called_once() + tf_publish.assert_called_once() + info = cdr_decode(cdr_encode(info_publish.call_args.args[0]), CameraInfo) + transforms = cdr_decode(cdr_encode(tf_publish.call_args.args[0]), TFMessage).transforms + assert (info.header.stamp.sec, info.header.stamp.nanosec) == (1700000000, 123456789) + assert len(transforms) == 2 + assert [(edge.header.frame_id, edge.child_frame_id) for edge in transforms] == [ + ("base_link", "camera_link"), + ("camera_link", "robot/camera_optical"), + ] + assert all(edge.header.stamp == info.header.stamp for edge in transforms) + assert cdr_encode(module.config.transform) == original + np.testing.assert_allclose(transform_matrix(transforms[0].transform), np.eye(4)) + + +def test_disabled_mount_publishes_calibration_without_tf(module, mocker): + module.config.transform = None + info_publish = mocker.patch.object(module.camera_info, "publish") + tf_publish = mocker.patch.object(module.tf, "publish") + module.publish_metadata() + info_publish.assert_called_once() + tf_publish.assert_not_called() + assert type(info_publish.call_args.args[0]) is CameraInfo diff --git a/dimos/hardware/sensors/camera/test_webcam.py b/dimos/hardware/sensors/camera/test_webcam.py index 6e390d078c7..48cd1d53256 100644 --- a/dimos/hardware/sensors/camera/test_webcam.py +++ b/dimos/hardware/sensors/camera/test_webcam.py @@ -12,27 +12,71 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode +import numpy as np +import pytest + from dimos.hardware.sensors.camera.webcam import Webcam -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo +from dimos.message_utils.camera_info import camera_info_from_fov +from dimos.message_utils.image import image_view +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.sensor_msgs.msg import CameraInfo, Image, RegionOfInterest +from dimos.messages.std_msgs.msg import Header def test_webcam_without_intrinsics_reports_a_nominal_pinhole() -> None: info = Webcam(width=1280, height=720).camera_info assert (info.width, info.height) == (1280, 720) - assert info.K[0] > 0 - assert info.frame_id == "camera_optical" + assert info.k[0] > 0 + assert info.header.frame_id == "camera_optical" def test_webcam_keeps_configured_intrinsics() -> None: - configured = CameraInfo.from_fov(90.0, 640, 480) + configured = camera_info_from_fov( + 90.0, 640, 480, header=Header(stamp=Time(sec=0, nanosec=0), frame_id="") + ) assert Webcam(camera_info=configured).camera_info is configured def test_webcam_with_size_but_no_focal_length_gets_a_nominal_pinhole() -> None: - info = Webcam(camera_info=CameraInfo(width=1280, height=720)).camera_info - assert info.K[0] > 0 + info = Webcam( + camera_info=CameraInfo( + width=1280, + height=720, + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + distortion_model="", + d=np.array([], dtype=np.float64), + k=np.zeros(9, dtype=np.float64), + r=np.zeros(9, dtype=np.float64), + p=np.zeros(12, dtype=np.float64), + binning_x=0, + binning_y=0, + roi=RegionOfInterest(x_offset=0, y_offset=0, height=0, width=0, do_rectify=False), + ) + ).camera_info + assert info.k[0] > 0 def test_webcam_stereo_slice_halves_the_nominal_pinhole_width() -> None: info = Webcam(width=1280, height=720, stereo_slice="left").camera_info assert (info.width, info.height) == (640, 720) + + +@pytest.mark.parametrize("side,start", [("left", 0), ("right", 2)]) +def test_capture_converts_bgr_and_crops_before_cdr_encoding(side, start, mocker): + camera = Webcam(stereo_slice=side, frame_id_prefix="robot") + pixels = np.arange(2 * 4 * 3, dtype=np.uint8).reshape(2, 4, 3) + capture = mocker.Mock() + capture.read.return_value = (True, pixels) + mocker.patch.object(camera, "_capture", capture) + mocker.patch("dimos.message_utils.time.time.time_ns", return_value=1700000000123456789) + try: + message = cdr_decode(cdr_encode(camera.capture_frame()), Image) + finally: + camera.stop() + assert message.encoding == "rgb8" + assert (message.width, message.height, message.step) == (2, 2, 6) + assert message.header.frame_id == "robot/camera_optical" + assert (message.header.stamp.sec, message.header.stamp.nanosec) == (1700000000, 123456789) + np.testing.assert_array_equal(image_view(message), pixels[:, start : start + 2, ::-1]) + capture.release.assert_called_once() diff --git a/dimos/hardware/sensors/camera/v4l2/module.py b/dimos/hardware/sensors/camera/v4l2/module.py index df887eb525a..285391018cd 100644 --- a/dimos/hardware/sensors/camera/v4l2/module.py +++ b/dimos/hardware/sensors/camera/v4l2/module.py @@ -29,7 +29,7 @@ from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import Out -from dimos.msgs.sensor_msgs.CompressedImage import CompressedImage +from dimos.messages.sensor_msgs.msg import CompressedImage class V4L2CameraConfig(NativeModuleConfig): diff --git a/dimos/hardware/sensors/camera/v4l2/rust/Cargo.lock b/dimos/hardware/sensors/camera/v4l2/rust/Cargo.lock index eee543479d3..a4f7c38d602 100644 --- a/dimos/hardware/sensors/camera/v4l2/rust/Cargo.lock +++ b/dimos/hardware/sensors/camera/v4l2/rust/Cargo.lock @@ -249,6 +249,15 @@ dependencies = [ "shlex", ] +[[package]] +name = "cdr" +version = "0.2.4" +source = "git+https://github.com/TomCC7/cdr-rs?rev=38a4368255d408faf374726653b39d4a4e9735cb#38a4368255d408faf374726653b39d4a4e9735cb" +dependencies = [ + "byteorder", + "serde", +] + [[package]] name = "cfg-if" version = "1.0.4" @@ -554,10 +563,21 @@ dependencies = [ "subtle", ] +[[package]] +name = "dimos-generated-messages" +version = "0.0.14" +dependencies = [ + "cdr", + "heapless", + "ros2msg", + "serde", + "serde-big-array", +] + [[package]] name = "dimos-lcm" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", "socket2 0.5.10", @@ -566,9 +586,10 @@ dependencies = [ [[package]] name = "dimos-module" -version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +version = "0.2.0" dependencies = [ + "cdr", + "dimos-generated-messages", "dimos-lcm", "dimos-module-macros", "lcm-msgs", @@ -579,6 +600,7 @@ dependencies = [ "tokio", "tracing", "tracing-subscriber", + "transforms", "url", "validator", "zenoh", @@ -587,7 +609,6 @@ dependencies = [ [[package]] name = "dimos-module-macros" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" dependencies = [ "proc-macro2", "quote", @@ -600,8 +621,8 @@ name = "dimos-v4l2-camera" version = "0.1.0" dependencies = [ "cc", + "dimos-generated-messages", "dimos-module", - "lcm-msgs", "libc", "serde", "tokio", @@ -947,6 +968,15 @@ version = "0.33.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "21fef0953c54fd3de2f44b743fbf77e044c81a25faee03636dfccc0d35135e23" +[[package]] +name = "hash32" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "47d60b12902ba28e2730cd37e95b8c9223af2808df9e902d4df49588d1470606" +dependencies = [ + "byteorder", +] + [[package]] name = "hashbrown" version = "0.12.3" @@ -988,6 +1018,28 @@ name = "hashbrown" version = "0.17.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ed5909b6e89a2db4456e54cd5f673791d7eca6732202bbf2a9cc504fe2f9b84a" +dependencies = [ + "allocator-api2", + "equivalent", + "foldhash 0.2.0", +] + +[[package]] +name = "heapless" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +dependencies = [ + "hash32", + "serde", + "stable_deref_trait", +] + +[[package]] +name = "heck" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" [[package]] name = "hermit-abi" @@ -1381,7 +1433,7 @@ checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" [[package]] name = "lcm-msgs" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", ] @@ -1922,6 +1974,16 @@ dependencies = [ "zerocopy", ] +[[package]] +name = "prettyplease" +version = "0.2.37" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "479ca8adacdd7ce8f1fb39ce9ecccbfe93a3f1344b3d0d97f20bc0196208f62b" +dependencies = [ + "proc-macro2", + "syn 2.0.119", +] + [[package]] name = "proc-macro-crate" version = "3.5.0" @@ -2276,6 +2338,23 @@ dependencies = [ "unicode-ident", ] +[[package]] +name = "ros2msg" +version = "0.5.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2fe291be5cb6d4b8e6bf9c9de80c40bd1cb09ecac50deafa2fdcae0027e04988" +dependencies = [ + "heck", + "pest", + "pest_derive", + "prettyplease", + "proc-macro2", + "quote", + "regex", + "syn 2.0.119", + "thiserror 2.0.20", +] + [[package]] name = "rustc-hash" version = "2.1.3" @@ -2516,6 +2595,15 @@ dependencies = [ "serde_derive", ] +[[package]] +name = "serde-big-array" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11fc7cc2c76d73e0f27ee52abbd64eec84d46f370c88371120433196934e4b7f" +dependencies = [ + "serde", +] + [[package]] name = "serde_core" version = "1.0.229" @@ -3209,6 +3297,18 @@ dependencies = [ "tracing-serde", ] +[[package]] +name = "transforms" +version = "2.2.0" +source = "git+https://github.com/TomCC7/transforms?rev=5ae9563de0c02638c04e393aadfad28444a4398e#5ae9563de0c02638c04e393aadfad28444a4398e" +dependencies = [ + "approx", + "hashbrown 0.17.1", + "libm", + "serde", + "thiserror 2.0.20", +] + [[package]] name = "turbojpeg" version = "1.5.1" diff --git a/dimos/hardware/sensors/camera/v4l2/rust/Cargo.toml b/dimos/hardware/sensors/camera/v4l2/rust/Cargo.toml index 4981ac593d8..ef5a92263f4 100644 --- a/dimos/hardware/sensors/camera/v4l2/rust/Cargo.toml +++ b/dimos/hardware/sensors/camera/v4l2/rust/Cargo.toml @@ -18,11 +18,11 @@ path = "dimos_v4l2_camera::module::V4L2Camera" python = "dimos.hardware.sensors.camera.v4l2.module:V4L2Camera" [package.metadata.dimos.module.v4l2_camera.outputs] -jpeg_out = "sensor_msgs.CompressedImage" +jpeg_out = "sensor_msgs/msg/CompressedImage" [dependencies] -dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } -lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } +dimos-module = { path = "../../../../../../native/rust/dimos-module" } +dimos-generated-messages = { path = "../../../../../../build/message-source/rust" } tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal"] } serde = { version = "1", features = ["derive"] } tracing = "0.1" diff --git a/dimos/hardware/sensors/camera/v4l2/rust/crate-hashes.json b/dimos/hardware/sensors/camera/v4l2/rust/crate-hashes.json index 4c5fde86d6c..3ee4f926e23 100644 --- a/dimos/hardware/sensors/camera/v4l2/rust/crate-hashes.json +++ b/dimos/hardware/sensors/camera/v4l2/rust/crate-hashes.json @@ -1,6 +1,6 @@ { - "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", - "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module-macros@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=" + "git+https://github.com/TomCC7/cdr-rs?rev=38a4368255d408faf374726653b39d4a4e9735cb#cdr@0.2.4": "sha256-akg/bLBY3wQ1YRr2bijE5ZqB211Cst5lKjELLrgiG6o=", + "git+https://github.com/TomCC7/transforms?rev=5ae9563de0c02638c04e393aadfad28444a4398e#transforms@2.2.0": "sha256-Kh8aVjj7zQ+Xd1QHOcAhSJbubg7azaWFSMpaI/ZAgM4=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=" } \ No newline at end of file diff --git a/dimos/hardware/sensors/camera/v4l2/rust/flake.nix b/dimos/hardware/sensors/camera/v4l2/rust/flake.nix index f7237666148..00ac187a0f6 100644 --- a/dimos/hardware/sensors/camera/v4l2/rust/flake.nix +++ b/dimos/hardware/sensors/camera/v4l2/rust/flake.nix @@ -17,8 +17,31 @@ pkgs = nixpkgs.legacyPackages.${system}; name = "dimos-v4l2-camera"; - src = nix-filter.lib { root = ./.; exclude = [ "target" "build" "result" "__pycache__" ]; }; - generated = crate2nix.tools.${system}.generatedCargoNix { inherit name src; }; + # Keep this flake self-contained: shared SDK/configuration arrive through + # one fixed-hash project source, rather than paths outside this directory. + sharedArchive = pkgs.fetchurl { + url = "https://codeload.github.com/dimensionalOS/dimos/tar.gz/a9365487b4323151ea85041c6a944b509c0c3268"; + sha256 = "e339487dd2281980fcde7ab03c46dfd2e274491e0f73e66d9c6cbcaeea8902b2"; + }; + sharedSource = pkgs.runCommand "dimos-cdr-sdk-source" { + nativeBuildInputs = [ pkgs.gnutar pkgs.gzip ]; + } '' + mkdir -p $out + tar -xzf ${sharedArchive} --strip-components=1 -C $out + ''; + messages = import "${sharedSource}/native/message-source.nix" { inherit pkgs; }; + cameraSource = nix-filter.lib { root = ./.; exclude = [ "target" "build" "result" "__pycache__" ]; }; + src = pkgs.runCommand "dimos-v4l2-cdr-source" { } '' + mkdir -p $out/native/rust $out/build/message-source $out/dimos/hardware/sensors/camera/v4l2/rust + cp -r ${sharedSource}/native/rust/dimos-module $out/native/rust/ + cp -r ${sharedSource}/native/rust/dimos-module-macros $out/native/rust/ + cp -r ${messages.source}/rust $out/build/message-source/rust + cp -r ${cameraSource}/. $out/dimos/hardware/sensors/camera/v4l2/rust/ + ''; + generated = crate2nix.tools.${system}.generatedCargoNix { + inherit name src; + cargoToml = "dimos/hardware/sensors/camera/v4l2/rust/Cargo.toml"; + }; nvidiaDeb = repo: pname: hash: pkgs.fetchurl { url = "https://repo.download.nvidia.com/jetson/${repo}/pool/main/n/${pname}/${pname}_36.4.7-20250918154033_arm64.deb"; diff --git a/dimos/hardware/sensors/camera/v4l2/rust/src/module.rs b/dimos/hardware/sensors/camera/v4l2/rust/src/module.rs index 9bf25ae712a..c0cc10d23ea 100644 --- a/dimos/hardware/sensors/camera/v4l2/rust/src/module.rs +++ b/dimos/hardware/sensors/camera/v4l2/rust/src/module.rs @@ -17,15 +17,50 @@ use std::time::{Duration, Instant}; -use dimos_module::{native_config, Module, Output}; -use lcm_msgs::sensor_msgs::CompressedImage; -use lcm_msgs::std_msgs::{Header, Time}; +use dimos_generated_messages::{ + builtin_interfaces::msg::time::Time, sensor_msgs::msg::compressed_image::CompressedImage, + std_msgs::msg::header::Header, +}; +use dimos_module::{cdr, native_config, Module, Output}; use tracing::{info, warn}; use crate::capture::{fourcc, Capture}; use crate::clock::{driver_clocks, wall_s, CaptureClock}; use crate::jpeg::encode_packed_422; +#[cfg(test)] +mod cdr_tests { + use super::*; + use std::io::Write; + + #[test] + fn compressed_frame_uses_canonical_cdr_header_and_payload() { + let message = CompressedImage { + header: Header { + stamp: Time { + sec: 1_700_000_000, + nanosec: 123_456_789, + }, + frame_id: "camera_optical".into(), + }, + format: "jpeg".into(), + data: vec![0xff, 0xd8, 0xff, 0xd9], + }; + let payload = cdr::encode(&message).unwrap(); + assert_eq!(&payload[..4], &[0, 1, 0, 0]); + assert_eq!(cdr::decode::(&payload).unwrap(), message); + if let Some(path) = std::env::var_os("DIMOS_CAMERA_CDR_FIXTURE") { + std::fs::OpenOptions::new() + .write(true) + .create_new(true) + .open(path) + .expect("create a new interop fixture") + .write_all(&payload) + .expect("write the requested interop fixture"); + } + } +} + const STATS_EVERY: Duration = Duration::from_secs(10); const FRAME_TIMEOUT_MS: i32 = 1000; /// Consecutive frame timeouts before the device is closed and reopened. @@ -56,7 +91,7 @@ pub struct Config { #[derive(Module)] #[module(name = "v4l2_camera", setup = start)] pub struct V4L2Camera { - #[output(encode = CompressedImage::encode)] + #[output(encode = cdr::encode)] jpeg_out: Output, #[config] @@ -115,7 +150,7 @@ fn pump( ) { let mut clock = CaptureClock::new(driver_clocks(), wall_s); let (width, height) = (config.width as usize, config.height as usize); - let (mut timeouts, mut seq) = (0, 0i32); + let mut timeouts = 0; let (mut frames, mut encode_s, mut window) = (0u32, 0.0, Instant::now()); loop { let frame = match capture.next(FRAME_TIMEOUT_MS) { @@ -154,17 +189,15 @@ fn pump( }; let message = CompressedImage { header: Header { - seq, stamp: Time { sec: stamp.trunc() as i32, - nsec: (stamp.fract() * 1e9) as i32, + nanosec: (stamp.fract() * 1e9) as u32, }, frame_id: config.frame_id.clone(), }, format: "jpeg".into(), data, }; - seq = seq.wrapping_add(1); runtime.block_on(output.publish(&message)).ok(); frames += 1; if window.elapsed() >= STATS_EVERY { diff --git a/dimos/hardware/sensors/camera/webcam.py b/dimos/hardware/sensors/camera/webcam.py index 8523f356b6d..b4463fc33ea 100644 --- a/dimos/hardware/sensors/camera/webcam.py +++ b/dimos/hardware/sensors/camera/webcam.py @@ -18,13 +18,18 @@ import time from typing import Annotated, Any, Literal -from pydantic import BeforeValidator, Field +import numpy as np +from pydantic import BeforeValidator, Field, InstanceOf from reactivex import create from reactivex.observable import Observable from dimos.hardware.sensors.camera.spec import CameraConfig, CameraHardware -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.message_utils.camera_info import camera_info_from_fov +from dimos.message_utils.image import image_from_array +from dimos.message_utils.time import header_now +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.sensor_msgs.msg import CameraInfo, Image, RegionOfInterest +from dimos.messages.std_msgs.msg import Header from dimos.utils.reactive import backpressure @@ -45,7 +50,21 @@ class WebcamConfig(CameraConfig): width: int = 640 height: int = 480 fps: float = 15.0 - camera_info: CameraInfo = Field(default_factory=CameraInfo) + camera_info: InstanceOf[CameraInfo] = Field( + default_factory=lambda: CameraInfo( + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + height=0, + width=0, + distortion_model="", + d=np.array([], dtype=np.float64), + k=np.zeros(9, dtype=np.float64), + r=np.zeros(9, dtype=np.float64), + p=np.zeros(12, dtype=np.float64), + binning_x=0, + binning_y=0, + roi=RegionOfInterest(x_offset=0, y_offset=0, height=0, width=0, do_rectify=False), + ) + ) frame_id_prefix: str | None = None stereo_slice: Literal["left", "right"] | None = None # For stereo cameras @@ -142,22 +161,13 @@ def capture_frame(self) -> Image: # Convert BGR to RGB (OpenCV uses BGR by default) frame_rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) - # Create Image message - # Using Image.from_numpy() since it's designed for numpy arrays - # Setting format to RGB since we converted from BGR->RGB above - image = Image.from_numpy( - frame_rgb, - format=ImageFormat.RGB, # We converted to RGB above - frame_id=self._frame("camera_optical"), # Standard frame ID for camera images - ts=time.time(), # Current timestamp - ) - if self.config.stereo_slice in ("left", "right"): - half_width = image.width // 2 - if self.config.stereo_slice == "left": - image = image.crop(0, 0, half_width, image.height) - else: - image = image.crop(half_width, 0, half_width, image.height) + half_width = frame_rgb.shape[1] // 2 + start = 0 if self.config.stereo_slice == "left" else half_width + frame_rgb = frame_rgb[:, start : start + half_width] + image = image_from_array( + frame_rgb, encoding="rgb8", header=header_now(self._frame("camera_optical")) + ) self._emitted_size = (image.width, image.height) return image @@ -190,7 +200,7 @@ def _capture_loop(self) -> None: @property def camera_info(self) -> CameraInfo: info = self.config.camera_info - if info.width and info.height and info.K[0] > 0 and info.K[4] > 0: + if info.width and info.height and info.k[0] > 0 and info.k[4] > 0: return info # No intrinsics configured: a nominal pinhole so the image still renders. # Sized from the frames actually emitted (the stereo slice halves them). @@ -199,8 +209,12 @@ def camera_info(self) -> CameraInfo: else: width = self.config.width // 2 if self.config.stereo_slice else self.config.width height = self.config.height - return CameraInfo.from_fov( - 60.0, width, height, axis="horizontal", frame_id=self._frame("camera_optical") + return camera_info_from_fov( + 60.0, + width, + height, + axis="horizontal", + header=Header(frame_id=self._frame("camera_optical"), stamp=Time(sec=0, nanosec=0)), ) def emit(self, image: Image) -> None: ... diff --git a/dimos/hardware/sensors/camera/zed/camera.py b/dimos/hardware/sensors/camera/zed/camera.py index fd777339dfc..0f483a1934a 100644 --- a/dimos/hardware/sensors/camera/zed/camera.py +++ b/dimos/hardware/sensors/camera/zed/camera.py @@ -18,8 +18,9 @@ import threading import time -from pydantic import Field -import pyzed.sl as sl +import numpy as np +from pydantic import Field, InstanceOf +from pyzed import sl import reactivex as rx from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT @@ -29,17 +30,23 @@ from dimos.core.stream import Out from dimos.core.transport_factory import make_transport from dimos.hardware.sensors.camera.spec import ( - OPTICAL_ROTATION, DepthCameraConfig, DepthCameraHardware, ) -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.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.hardware.sensors.camera.zed.messages import ( + frame_message, + tracking_to_base, + transform_from_arrays, +) +from dimos.message_utils.camera_info import camera_info_with_stamp +from dimos.message_utils.image import image_from_array +from dimos.message_utils.pointcloud import pointcloud_from_rgbd, voxel_downsample_cloud +from dimos.message_utils.time import time_from_nanoseconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.sensor_msgs.msg import CameraInfo, Image, PointCloud2, RegionOfInterest +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage from dimos.spec import perception from dimos.utils.reactive import backpressure @@ -47,8 +54,8 @@ def default_base_transform() -> Transform: """Default identity transform for camera mounting.""" return Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + translation=Vector3(x=0.0, y=0.0, z=0.0), + rotation=Quaternion(w=1.0, x=0.0, y=0.0, z=0.0), ) @@ -58,7 +65,7 @@ class ZEDCameraConfig(ModuleConfig, DepthCameraConfig): fps: int = 15 camera_name: str = "camera" base_frame_id: str = "base_link" - base_transform: Transform | None = Field(default_factory=default_base_transform) + base_transform: InstanceOf[Transform] | None = Field(default_factory=default_base_transform) align_depth_to_color: bool = True enable_depth: bool = True enable_pointcloud: bool = False @@ -129,13 +136,11 @@ def __init__(self, *args, **kwargs) -> None: # type: ignore[no-untyped-def] self._sl_camera_info: sl.CameraInformation | None = None def _publish_camera_info(self) -> None: - ts = time.time() + stamp = time_from_nanoseconds(time.time_ns()) if self._color_camera_info: - self._color_camera_info.ts = ts - self.camera_info.publish(self._color_camera_info) + self.camera_info.publish(camera_info_with_stamp(self._color_camera_info, stamp)) if self._depth_camera_info: - self._depth_camera_info.ts = ts - self.depth_camera_info.publish(self._depth_camera_info) + self.depth_camera_info.publish(camera_info_with_stamp(self._depth_camera_info, stamp)) @rpc def start(self) -> None: @@ -232,10 +237,14 @@ def _intrinsics_to_camera_info( height=self._stream_height, width=self._stream_width, distortion_model="plumb_bob", - D=D, - K=K, - P=P, - frame_id=frame_id, + d=np.asarray(D, dtype=np.float64), + k=np.asarray(K, dtype=np.float64), + r=np.array([1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], dtype=np.float64), + p=np.asarray(P, dtype=np.float64), + header=Header(frame_id=frame_id, stamp=Time(sec=0, nanosec=0)), + binning_x=0, + binning_y=0, + roi=RegionOfInterest(x_offset=0, y_offset=0, height=0, width=0, do_rectify=False), ) def _get_extrinsics(self) -> None: @@ -250,16 +259,14 @@ def _extrinsics_to_transform( extrinsics: sl.Transform, frame_id: str, child_frame_id: str, - ts: float, - ) -> Transform: - translation = extrinsics.get_translation().get() - quat = extrinsics.get_orientation().get() # [x, y, z, w] - return Transform( - translation=Vector3(*translation), - rotation=Quaternion(quat[0], quat[1], quat[2], quat[3]), - frame_id=frame_id, - child_frame_id=child_frame_id, - ts=ts, + stamp: Time, + ) -> TransformStamped: + return transform_from_arrays( + extrinsics.get_translation().get(), + extrinsics.get_orientation().get(), + parent=frame_id, + child=child_frame_id, + stamp=stamp, ) def _enable_tracking(self) -> None: @@ -292,7 +299,7 @@ def _capture_loop(self) -> None: time.sleep(0.001) continue - ts = time.time() + stamp = time_from_nanoseconds(time.time_ns()) color_img = None if self._image_left is not None: @@ -301,11 +308,10 @@ def _capture_loop(self) -> None: if color_data.ndim == 3 and color_data.shape[2] == 4: color_data = color_data[:, :, :3] color_data = cv2.cvtColor(color_data, cv2.COLOR_BGR2RGB) - color_img = Image( - data=color_data, - format=ImageFormat.RGB, - frame_id=self._color_optical_frame, - ts=ts, + color_img = image_from_array( + color_data, + encoding="rgb8", + header=Header(frame_id=self._color_optical_frame, stamp=stamp), ) self.color_image.publish(color_img) @@ -320,11 +326,10 @@ def _capture_loop(self) -> None: if self.config.align_depth_to_color else self._depth_optical_frame ) - depth_img = Image( - data=depth_data, - format=ImageFormat.DEPTH, - frame_id=depth_frame_id, - ts=ts, + depth_img = image_from_array( + depth_data, + encoding="32FC1", + header=Header(frame_id=depth_frame_id, stamp=stamp), ) self.depth_image.publish(depth_img) @@ -333,99 +338,39 @@ def _capture_loop(self) -> None: self._latest_color_img = color_img self._latest_depth_img = depth_img - self._publish_tf(ts) + self._publish_tf(stamp) - def _tracking_transform(self, ts: float) -> Transform | None: + def _tracking_transform(self, stamp: Time) -> TransformStamped | None: if not self._tracking_enabled or self._zed is None or self._pose is None: return None state = self._zed.get_position(self._pose, sl.REFERENCE_FRAME.WORLD) if state != sl.POSITIONAL_TRACKING_STATE.OK: return None - - translation = self._pose.get_translation().get().tolist() - rotation = self._pose.get_orientation().get().tolist() - world_to_camera = Transform( - translation=Vector3(*translation), - rotation=Quaternion(*rotation), - frame_id=self.config.world_frame, - child_frame_id=self._camera_link, - ts=ts, - ) - if self.config.base_transform is None: - return world_to_camera - - base_to_camera = Transform( - translation=self.config.base_transform.translation, - rotation=self.config.base_transform.rotation, - frame_id=self.config.base_frame_id, - child_frame_id=self._camera_link, - ts=ts, - ) - camera_to_base = base_to_camera.inverse() - world_to_base = world_to_camera + camera_to_base - world_to_base.frame_id = self.config.world_frame - world_to_base.child_frame_id = self.config.base_frame_id - world_to_base.ts = ts - return world_to_base - - def _publish_tf(self, ts: float) -> None: - transforms = [] - - if self.config.base_transform is not None: - base_to_camera = Transform( - translation=self.config.base_transform.translation, - rotation=self.config.base_transform.rotation, - frame_id=self.config.base_frame_id, - child_frame_id=self._camera_link, - ts=ts, - ) - transforms.append(base_to_camera) - - # camera_imu_transform is IMU -> left_camera (coordinate transform), - # we need to invert to get the pose of left camera in camera_link frame - camera_link_to_depth = self._extrinsics_to_transform( - self._camera_link_to_color_extrinsics, + pose = self._extrinsics_to_transform( + self._pose, + self.config.world_frame, self._camera_link, - self._depth_frame, - ts, - ).inverse() - camera_link_to_depth.frame_id = self._camera_link - camera_link_to_depth.child_frame_id = self._depth_frame - transforms.append(camera_link_to_depth) - - depth_to_depth_optical = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=OPTICAL_ROTATION, - frame_id=self._depth_frame, - child_frame_id=self._depth_optical_frame, - ts=ts, + stamp, ) - transforms.append(depth_to_depth_optical) + return tracking_to_base(pose, self.config.base_transform, self.config.base_frame_id) - color_tf = self._extrinsics_to_transform( + def _publish_tf(self, stamp: Time) -> None: + extrinsics = self._extrinsics_to_transform( self._camera_link_to_color_extrinsics, self._camera_link, self._color_frame, - ts, - ).inverse() - color_tf.frame_id = self._camera_link - color_tf.child_frame_id = self._color_frame - transforms.append(color_tf) - - color_to_color_optical = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=OPTICAL_ROTATION, - frame_id=self._color_frame, - child_frame_id=self._color_optical_frame, - ts=ts, + stamp, + ) + self.tf.publish( + frame_message( + camera_name=self.config.camera_name, + base_frame=self.config.base_frame_id, + base_transform=self.config.base_transform, + sdk_extrinsics=extrinsics.transform, + stamp=stamp, + tracking=self._tracking_transform(stamp), + ) ) - transforms.append(color_to_color_optical) - - tracking_tf = self._tracking_transform(ts) - if tracking_tf is not None: - transforms.append(tracking_tf) - - self.tf.publish(TFMessage(*transforms)) def _generate_pointcloud(self) -> None: with self._pointcloud_lock: @@ -436,13 +381,13 @@ def _generate_pointcloud(self) -> None: return try: - pcd = PointCloud2.from_rgbd( - color_image=color_img, - depth_image=depth_img, - camera_info=self._color_camera_info, + pcd = pointcloud_from_rgbd( + color=color_img, + depth=depth_img, + calibration=self._color_camera_info, depth_scale=self._depth_scale, ) - pcd = pcd.voxel_downsample(0.005) + pcd = voxel_downsample_cloud(pcd, 0.005) self.pointcloud.publish(pcd) except Exception as e: print(f"Pointcloud generation error: {e}") diff --git a/dimos/hardware/sensors/camera/zed/compat.py b/dimos/hardware/sensors/camera/zed/compat.py index f7e78fc5d62..e2e01022f01 100644 --- a/dimos/hardware/sensors/camera/zed/compat.py +++ b/dimos/hardware/sensors/camera/zed/compat.py @@ -16,7 +16,7 @@ from pathlib import Path -from dimos.msgs.sensor_msgs.CameraInfo import CalibrationProvider +from dimos.message_utils.camera_info import CalibrationProvider try: import pyzed.sl # noqa: F401 diff --git a/dimos/hardware/sensors/camera/zed/messages.py b/dimos/hardware/sensors/camera/zed/messages.py new file mode 100644 index 00000000000..a23fccd222f --- /dev/null +++ b/dimos/hardware/sensors/camera/zed/messages.py @@ -0,0 +1,112 @@ +# Copyright 2025-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. + +"""SDK-independent CDR frame conversions used by the ZED producer.""" + +from __future__ import annotations + +from collections.abc import Sequence + +from dimos.hardware.sensors.camera.spec import OPTICAL_ROTATION +from dimos.message_utils.geometry import compose_transforms, inverse_transform +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage + + +def transform_from_arrays( + translation: Sequence[float], + rotation_xyzw: Sequence[float], + *, + parent: str, + child: str, + stamp: Time, +) -> TransformStamped: + if len(translation) != 3 or len(rotation_xyzw) != 4: + raise ValueError("ZED pose requires three translation and four quaternion components") + return TransformStamped( + header=Header(frame_id=parent, stamp=stamp), + child_frame_id=child, + transform=Transform( + translation=Vector3( + x=float(translation[0]), y=float(translation[1]), z=float(translation[2]) + ), + rotation=Quaternion( + x=float(rotation_xyzw[0]), + y=float(rotation_xyzw[1]), + z=float(rotation_xyzw[2]), + w=float(rotation_xyzw[3]), + ), + ), + ) + + +def tracking_to_base( + world_to_camera: TransformStamped, + base_transform: Transform | None, + base_frame: str, +) -> TransformStamped: + if base_transform is None: + return world_to_camera + mount = TransformStamped( + header=Header(frame_id=base_frame, stamp=world_to_camera.header.stamp), + child_frame_id=world_to_camera.child_frame_id, + transform=base_transform, + ) + return compose_transforms(world_to_camera, inverse_transform(mount)) + + +def frame_message( + *, + camera_name: str, + base_frame: str, + base_transform: Transform | None, + sdk_extrinsics: Transform, + stamp: Time, + tracking: TransformStamped | None = None, +) -> TFMessage: + camera_link = camera_name + "_link" + transforms = [] + if base_transform is not None: + transforms.append( + TransformStamped( + header=Header(frame_id=base_frame, stamp=stamp), + child_frame_id=camera_link, + transform=base_transform, + ) + ) + for kind in ["depth", "color"]: + frame = camera_name + "_" + kind + "_frame" + optical = camera_name + "_" + kind + "_optical_frame" + # Preserve the producer's existing inverse SDK extrinsics convention. + edge = inverse_transform( + TransformStamped( + header=Header(frame_id=camera_link, stamp=stamp), + child_frame_id=frame, + transform=sdk_extrinsics, + ) + ) + edge.header.frame_id, edge.child_frame_id = camera_link, frame + transforms.append(edge) + transforms.append( + TransformStamped( + header=Header(frame_id=frame, stamp=stamp), + child_frame_id=optical, + transform=Transform(rotation=OPTICAL_ROTATION, translation=Vector3(x=0, y=0, z=0)), + ) + ) + if tracking is not None: + transforms.append(tracking) + return TFMessage(transforms=transforms) diff --git a/dimos/hardware/sensors/camera/zed/test_messages.py b/dimos/hardware/sensors/camera/zed/test_messages.py new file mode 100644 index 00000000000..117b4d25f95 --- /dev/null +++ b/dimos/hardware/sensors/camera/zed/test_messages.py @@ -0,0 +1,128 @@ +# Copyright 2025-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. + +import math + +from dimos_cdrgen.registry import decode, encode +import numpy as np +import pytest + +from dimos.hardware.sensors.camera.zed.compat import CameraInfo as ZedCalibration +from dimos.hardware.sensors.camera.zed.messages import ( + frame_message, + tracking_to_base, + transform_from_arrays, +) +from dimos.message_utils.geometry import quaternion_from_euler, transform_matrix +from dimos.message_utils.time import time_from_nanoseconds, to_nanoseconds +from dimos.messages.geometry_msgs.msg import Transform, Vector3 +from dimos.messages.tf2_msgs.msg import TFMessage + + +@pytest.mark.parametrize("ns", [0, -1, 1700000000123456789]) +def test_frames_roundtrip_cdr_preserves_exact_stamp_and_extrinsics(ns): + stamp = time_from_nanoseconds(ns) + extrinsics = Transform( + translation=Vector3(x=1, y=2, z=3), rotation=quaternion_from_euler(0, 0, math.pi / 2) + ) + mount = Transform(translation=Vector3(x=4, y=5, z=6), rotation=quaternion_from_euler(0.2, 0, 0)) + + message = frame_message( + camera_name="zed", + base_frame="base", + base_transform=mount, + sdk_extrinsics=extrinsics, + stamp=stamp, + ) + actual = decode(encode(message), TFMessage) + + assert [(edge.header.frame_id, edge.child_frame_id) for edge in actual.transforms] == [ + ("base", "zed_link"), + ("zed_link", "zed_depth_frame"), + ("zed_depth_frame", "zed_depth_optical_frame"), + ("zed_link", "zed_color_frame"), + ("zed_color_frame", "zed_color_optical_frame"), + ] + assert [to_nanoseconds(edge.header.stamp) for edge in actual.transforms] == [ns] * 5 + np.testing.assert_allclose( + transform_matrix(actual.transforms[0].transform), transform_matrix(mount) + ) + for edge in [actual.transforms[1], actual.transforms[3]]: + np.testing.assert_allclose( + transform_matrix(edge.transform), + np.linalg.inv(transform_matrix(extrinsics)), + atol=1e-12, + ) + for edge in [actual.transforms[2], actual.transforms[4]]: + optical_to_frame = transform_matrix(edge.transform) + # Optical right/down/forward maps to camera forward/left/up. + np.testing.assert_allclose(optical_to_frame @ [1, 2, 3, 1], [3, -1, -2, 1], atol=1e-12) + + +def test_tracking_removes_rotated_mount_and_is_included_without_mount_edge(): + stamp = time_from_nanoseconds(1700000000123456789) + world_camera = transform_from_arrays( + [5, 6, 7], + [0, 0, math.sqrt(0.5), math.sqrt(0.5)], + parent="world", + child="zed_link", + stamp=stamp, + ) + mount = Transform( + translation=Vector3(x=1, y=2, z=3), rotation=quaternion_from_euler(0, 0, -math.pi / 2) + ) + + tracking = tracking_to_base(world_camera, mount, "base") + message = frame_message( + camera_name="zed", + base_frame="base", + base_transform=None, + sdk_extrinsics=mount, + stamp=stamp, + tracking=tracking, + ) + actual = decode(encode(message), TFMessage) + + assert tracking.header.frame_id == "world" + assert tracking.child_frame_id == "base" + assert to_nanoseconds(tracking.header.stamp) == 1700000000123456789 + np.testing.assert_allclose( + transform_matrix(tracking.transform), + transform_matrix(world_camera.transform) @ np.linalg.inv(transform_matrix(mount)), + atol=1e-12, + ) + assert len(actual.transforms) == 5 + assert actual.transforms[-1] == tracking + assert actual.transforms[0].header.frame_id == "zed_link" + assert tracking_to_base(world_camera, None, "base") == world_camera + + +@pytest.mark.parametrize("translation,rotation", [([1, 2], [0, 0, 0, 1]), ([1, 2, 3], [0, 0, 1])]) +def test_incomplete_sdk_pose_is_rejected(translation, rotation): + with pytest.raises(ValueError, match="three translation and four quaternion"): + transform_from_arrays( + translation, rotation, parent="world", child="camera", stamp=time_from_nanoseconds(0) + ) + + +def test_shipped_zed_calibration_is_generated_and_roundtrips_cdr(): + calibration = ZedCalibration.SingleWebcam + + actual = decode(encode(calibration), type(calibration)) + + assert (actual.width, actual.height) == (640, 376) + assert actual.header.frame_id == "camera_optical" + assert to_nanoseconds(actual.header.stamp) == 0 + assert actual.k[0] == pytest.approx(379.45267) + assert actual.d[0] == pytest.approx(-0.309435) diff --git a/dimos/hardware/sensors/lidar/livox/rust/Cargo.lock b/dimos/hardware/sensors/lidar/livox/rust/Cargo.lock index d4f9a187f15..6dcf91e268c 100644 --- a/dimos/hardware/sensors/lidar/livox/rust/Cargo.lock +++ b/dimos/hardware/sensors/lidar/livox/rust/Cargo.lock @@ -609,7 +609,7 @@ dependencies = [ [[package]] name = "dimos-lcm" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", "socket2 0.5.10", @@ -621,7 +621,7 @@ name = "dimos-livox" version = "0.1.0" dependencies = [ "dimos-module", - "lcm-msgs", + "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen)", "livox-wire", "serde", "serde_json", @@ -636,11 +636,11 @@ dependencies = [ [[package]] name = "dimos-module" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "dimos-lcm", "dimos-module-macros", - "lcm-msgs", + "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549)", "nalgebra", "rayon", "serde", @@ -656,7 +656,7 @@ dependencies = [ [[package]] name = "dimos-module-macros" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "proc-macro2", "quote", @@ -1454,6 +1454,14 @@ dependencies = [ "byteorder", ] +[[package]] +name = "lcm-msgs" +version = "0.1.0" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" +dependencies = [ + "byteorder", +] + [[package]] name = "libc" version = "0.2.189" diff --git a/dimos/hardware/sensors/lidar/livox/rust/Cargo.toml b/dimos/hardware/sensors/lidar/livox/rust/Cargo.toml index 3a787626f00..807456d2f88 100644 --- a/dimos/hardware/sensors/lidar/livox/rust/Cargo.toml +++ b/dimos/hardware/sensors/lidar/livox/rust/Cargo.toml @@ -22,7 +22,7 @@ imu = "sensor_msgs.Imu" [dependencies] livox-wire = { git = "https://github.com/dimensionalOS/livox-wire", rev = "441c4a16dbe2adb0683ece976cc800c6d7fbc790" } -dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6", version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } serde = { version = "1", features = ["derive"] } serde_json = "1" diff --git a/dimos/hardware/sensors/lidar/livox/rust/crate-hashes.json b/dimos/hardware/sensors/lidar/livox/rust/crate-hashes.json index 25a968ff3d3..a20e1813041 100644 --- a/dimos/hardware/sensors/lidar/livox/rust/crate-hashes.json +++ b/dimos/hardware/sensors/lidar/livox/rust/crate-hashes.json @@ -1,7 +1,8 @@ { - "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module-macros@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module-macros@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=", "git+https://github.com/dimensionalOS/livox-wire?rev=441c4a16dbe2adb0683ece976cc800c6d7fbc790#0.1.0": "sha256-wDNVchRiqm4N9wPGwvYJ9JssmPOC33wBTE4QT6vZkGA=" } \ No newline at end of file diff --git a/dimos/hardware/sensors/lidar/pointlio/rust/Cargo.lock b/dimos/hardware/sensors/lidar/pointlio/rust/Cargo.lock index cedde0f558f..ac1eebc0ae2 100644 --- a/dimos/hardware/sensors/lidar/pointlio/rust/Cargo.lock +++ b/dimos/hardware/sensors/lidar/pointlio/rust/Cargo.lock @@ -609,7 +609,7 @@ dependencies = [ [[package]] name = "dimos-lcm" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", "socket2 0.5.10", @@ -619,11 +619,11 @@ dependencies = [ [[package]] name = "dimos-module" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "dimos-lcm", "dimos-module-macros", - "lcm-msgs", + "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549)", "nalgebra", "rayon", "serde", @@ -639,7 +639,7 @@ dependencies = [ [[package]] name = "dimos-module-macros" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "proc-macro2", "quote", @@ -652,7 +652,7 @@ name = "dimos-pointlio" version = "0.1.0" dependencies = [ "dimos-module", - "lcm-msgs", + "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen)", "livox-wire", "pointlio-core", "serde", @@ -1443,6 +1443,14 @@ dependencies = [ "byteorder", ] +[[package]] +name = "lcm-msgs" +version = "0.1.0" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" +dependencies = [ + "byteorder", +] + [[package]] name = "libc" version = "0.2.189" diff --git a/dimos/hardware/sensors/lidar/pointlio/rust/Cargo.toml b/dimos/hardware/sensors/lidar/pointlio/rust/Cargo.toml index e0837fc7160..208da0cd6da 100644 --- a/dimos/hardware/sensors/lidar/pointlio/rust/Cargo.toml +++ b/dimos/hardware/sensors/lidar/pointlio/rust/Cargo.toml @@ -33,7 +33,7 @@ odometry = "nav_msgs.Odometry" livox-wire = { git = "https://github.com/dimensionalOS/livox-wire", rev = "441c4a16dbe2adb0683ece976cc800c6d7fbc790" } # Direct dependencies of the copied livox wire/pipeline/pcap modules. See src/lib.rs. pointlio-core = { git = "https://github.com/dimensionalOS/dimos-module-pointlio.git", rev = "22a5403f827d030b248ae473c2808b5d405b3d55" } -dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6", version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } serde = { version = "1", features = ["derive"] } serde_json = "1" diff --git a/dimos/hardware/sensors/lidar/pointlio/rust/crate-hashes.json b/dimos/hardware/sensors/lidar/pointlio/rust/crate-hashes.json index 68580a734b5..ec628807aee 100644 --- a/dimos/hardware/sensors/lidar/pointlio/rust/crate-hashes.json +++ b/dimos/hardware/sensors/lidar/pointlio/rust/crate-hashes.json @@ -1,8 +1,9 @@ { - "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", "git+https://github.com/dimensionalOS/dimos-module-pointlio.git?rev=22a5403f827d030b248ae473c2808b5d405b3d55#pointlio-core@0.1.0": "sha256-FjUcsZVI8nTcmKw8WhdxBuHelBjOCeJmyEsyqRxDvX0=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module-macros@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module-macros@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=", "git+https://github.com/dimensionalOS/livox-wire?rev=441c4a16dbe2adb0683ece976cc800c6d7fbc790#0.1.0": "sha256-wDNVchRiqm4N9wPGwvYJ9JssmPOC33wBTE4QT6vZkGA=" } \ No newline at end of file diff --git a/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.lock b/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.lock index d5b83c0f4e9..15cfdd541df 100644 --- a/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.lock +++ b/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.lock @@ -609,7 +609,7 @@ dependencies = [ [[package]] name = "dimos-lcm" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", "socket2 0.5.10", @@ -619,7 +619,7 @@ dependencies = [ [[package]] name = "dimos-module" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "dimos-lcm", "dimos-module-macros", @@ -639,7 +639,7 @@ dependencies = [ [[package]] name = "dimos-module-macros" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "proc-macro2", "quote", @@ -1436,7 +1436,7 @@ checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" [[package]] name = "lcm-msgs" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", ] diff --git a/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.toml b/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.toml index ae0e2bf2183..54edb37e8e9 100644 --- a/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.toml +++ b/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.toml @@ -10,7 +10,7 @@ path = "src/main.rs" [dependencies] livox-wire = { git = "https://github.com/dimensionalOS/livox-wire", rev = "441c4a16dbe2adb0683ece976cc800c6d7fbc790" } # Direct dependencies of the copied livox wire/pipeline/pcap modules. See src/main.rs. -dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6", version = "=0.1.0" } tokio = { version = "1", features = ["rt-multi-thread", "macros", "time", "net", "sync"] } serde = { version = "1", features = ["derive"] } socket2 = "0.6" diff --git a/dimos/hardware/sensors/lidar/virtual_mid360/crate-hashes.json b/dimos/hardware/sensors/lidar/virtual_mid360/crate-hashes.json index 25a968ff3d3..c8f27978995 100644 --- a/dimos/hardware/sensors/lidar/virtual_mid360/crate-hashes.json +++ b/dimos/hardware/sensors/lidar/virtual_mid360/crate-hashes.json @@ -1,7 +1,7 @@ { - "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", - "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module-macros@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module-macros@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=", "git+https://github.com/dimensionalOS/livox-wire?rev=441c4a16dbe2adb0683ece976cc800c6d7fbc790#0.1.0": "sha256-wDNVchRiqm4N9wPGwvYJ9JssmPOC33wBTE4QT6vZkGA=" } \ No newline at end of file diff --git a/dimos/mapping/dim_slam/rust/Cargo.lock b/dimos/mapping/dim_slam/rust/Cargo.lock index 9308b22cb40..4aa5194e139 100644 --- a/dimos/mapping/dim_slam/rust/Cargo.lock +++ b/dimos/mapping/dim_slam/rust/Cargo.lock @@ -557,7 +557,7 @@ dependencies = [ "cu_vslam_rs", "dim_slam", "dimos-module", - "lcm-msgs", + "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen)", "serde", "tokio", "tracing", @@ -578,7 +578,7 @@ dependencies = [ [[package]] name = "dimos-lcm" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", "socket2 0.5.10", @@ -588,11 +588,11 @@ dependencies = [ [[package]] name = "dimos-module" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "dimos-lcm", "dimos-module-macros", - "lcm-msgs", + "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549)", "nalgebra", "rayon", "serde", @@ -608,7 +608,7 @@ dependencies = [ [[package]] name = "dimos-module-macros" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "proc-macro2", "quote", @@ -1395,6 +1395,14 @@ dependencies = [ "byteorder", ] +[[package]] +name = "lcm-msgs" +version = "0.1.0" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" +dependencies = [ + "byteorder", +] + [[package]] name = "libc" version = "0.2.189" diff --git a/dimos/mapping/dim_slam/rust/Cargo.toml b/dimos/mapping/dim_slam/rust/Cargo.toml index 72e51072660..5e063ca65dd 100644 --- a/dimos/mapping/dim_slam/rust/Cargo.toml +++ b/dimos/mapping/dim_slam/rust/Cargo.toml @@ -15,7 +15,7 @@ dim_slam = { git = "https://github.com/dimensionalOS/dimSLAM", tag = "v0.4.1" } # Unused directly, but cargo hands DEP_CUVSLAM_LIB_DIR only to immediate dependents # and build.rs needs it for the rpath. cu_vslam_rs = "0.2.0" -dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6", version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } tokio = { version = "1", features = ["rt-multi-thread", "macros"] } tracing = "0.1" diff --git a/dimos/mapping/dim_slam/rust/crate-hashes.json b/dimos/mapping/dim_slam/rust/crate-hashes.json index 980a12b1ffc..91d00ecf971 100644 --- a/dimos/mapping/dim_slam/rust/crate-hashes.json +++ b/dimos/mapping/dim_slam/rust/crate-hashes.json @@ -1,7 +1,8 @@ { "git+https://github.com/dimensionalOS/dimSLAM?tag=v0.4.1#dim_slam@0.1.0": "sha256-jx3QjYNclt3JFJOonGIAzfoJeJ5bhowx70a7LumF6IA=", - "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module-macros@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=" + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module-macros@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=" } \ No newline at end of file diff --git a/dimos/mapping/ray_tracing/rust/Cargo.lock b/dimos/mapping/ray_tracing/rust/Cargo.lock index 4412e44ee34..a2abd4749a3 100644 --- a/dimos/mapping/ray_tracing/rust/Cargo.lock +++ b/dimos/mapping/ray_tracing/rust/Cargo.lock @@ -588,7 +588,7 @@ dependencies = [ [[package]] name = "dimos-lcm" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", "socket2 0.5.10", @@ -598,11 +598,11 @@ dependencies = [ [[package]] name = "dimos-module" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "dimos-lcm", "dimos-module-macros", - "lcm-msgs", + "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549)", "nalgebra", "rayon", "serde", @@ -618,7 +618,7 @@ dependencies = [ [[package]] name = "dimos-module-macros" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "proc-macro2", "quote", @@ -633,7 +633,7 @@ dependencies = [ "ahash", "arrayvec", "dimos-module", - "lcm-msgs", + "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen)", "nalgebra", "rayon", "serde", @@ -1438,6 +1438,14 @@ dependencies = [ "byteorder", ] +[[package]] +name = "lcm-msgs" +version = "0.1.0" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" +dependencies = [ + "byteorder", +] + [[package]] name = "libc" version = "0.2.189" diff --git a/dimos/mapping/ray_tracing/rust/Cargo.toml b/dimos/mapping/ray_tracing/rust/Cargo.toml index b872c506948..8d248fd1dbe 100644 --- a/dimos/mapping/ray_tracing/rust/Cargo.toml +++ b/dimos/mapping/ray_tracing/rust/Cargo.toml @@ -35,7 +35,7 @@ region_bounds = "geometry_msgs.PoseStamped" map_regions = "sensor_msgs.PointCloud2" [dependencies] -dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6", version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal"] } serde = { version = "1", features = ["derive"] } diff --git a/dimos/mapping/ray_tracing/rust/crate-hashes.json b/dimos/mapping/ray_tracing/rust/crate-hashes.json index 4c5fde86d6c..187d28513c6 100644 --- a/dimos/mapping/ray_tracing/rust/crate-hashes.json +++ b/dimos/mapping/ray_tracing/rust/crate-hashes.json @@ -1,6 +1,7 @@ { - "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module-macros@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=" + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module-macros@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=" } \ No newline at end of file diff --git a/dimos/memory/module.py b/dimos/memory/module.py index a5867f0c53c..8bfdddccfea 100644 --- a/dimos/memory/module.py +++ b/dimos/memory/module.py @@ -35,7 +35,9 @@ from dimos.memory.store.sqlite import SqliteStore from dimos.memory.stream import Stream from dimos.memory.type.recording import OnExisting -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.message_utils.time import to_seconds +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.msgs.geometry_msgs.Pose import Pose from dimos.utils.data import backup_file from dimos.utils.logging_config import setup_logger @@ -47,7 +49,6 @@ SemanticSearch as SemanticSearch, SemanticSearchConfig as SemanticSearchConfig, ) - from dimos.msgs.geometry_msgs.Pose import Pose logger = setup_logger() @@ -395,7 +396,14 @@ async def _resolve_pose(self, name: str, msg: Any, ts: float) -> Pose | None: transform = self._tf.get( self.config.root_frame, frame_id, time_point=ts, time_tolerance=self.config.tf_tolerance ) - return transform.to_pose() if transform is not None else None + if transform is None: + return None + # Keep the recorder's internal Pose/pose-setter API; adapt only the TF boundary. + position = transform.transform.translation + rotation = transform.transform.rotation + return Pose( + position.x, position.y, position.z, rotation.x, rotation.y, rotation.z, rotation.w + ) def _collect_pose_setters(self) -> dict[str, PoseSetter]: """Map stream name -> bound ``@pose_setter_for`` method.""" @@ -416,7 +424,11 @@ def _record_tf(self) -> None: def on_tf(msg: TFMessage) -> None: try: for transform in msg.transforms: - tf_stream.append(TFMessage(transform), ts=transform.ts, pose=None) + tf_stream.append( + TFMessage(transforms=[transform]), + ts=to_seconds(transform.header.stamp), + pose=None, + ) except sqlite3.ProgrammingError: # A late LCM callback raced teardown and hit the closed store. pass diff --git a/dimos/memory/test_module.py b/dimos/memory/test_module.py index 2c06ea7b1e7..0861c3d71a1 100644 --- a/dimos/memory/test_module.py +++ b/dimos/memory/test_module.py @@ -17,15 +17,30 @@ from __future__ import annotations from collections.abc import Iterator +from contextlib import ExitStack +import threading import pytest +from reactivex.scheduler import ThreadPoolScheduler +from dimos.core.global_config import GlobalConfig from dimos.core.module import ModuleConfig from dimos.core.stream import In, Out -from dimos.memory.module import StreamModule +from dimos.core.transport_factory import make_transport +from dimos.memory.buffer import Unbounded +from dimos.memory.module import Recorder, StreamModule +from dimos.memory.store.sqlite import SqliteStore from dimos.memory.stream import Stream from dimos.memory.transform import Transformer from dimos.memory.type.observation import Observation +from dimos.messages.builtin_interfaces.msg import Time as GeneratedTime +from dimos.messages.dimos_msgs.msg import LineSegments3D as GeneratedSegments +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.std_msgs.msg import Header as GeneratedHeader +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.protocol.rpc.pubsubrpc import LCMRPC +from dimos.utils.threadpool import get_max_workers class Double(Transformer[int, int]): @@ -86,3 +101,96 @@ def test_blueprint_ports(module_cls: type[StreamModule]) -> None: stream_names = {s.name for s in atom.streams} assert "numbers" in stream_names assert "doubled" in stream_names + + +@pytest.mark.asyncio +async def test_recorder_reads_generated_header_without_network(mocker, tmp_path): + # Isolate the module's RPC and event-loop resource boundaries. + mocker.patch("dimos.core.module.get_loop", return_value=(None, None)) + mocker.patch("dimos.protocol.service.lcmservice.lcm_mod.LCM") + mocker.patch.object(LCMRPC, "start") + mocker.patch.object(LCMRPC, "serve_module_rpc") + recorder = Recorder(rpc_transport=LCMRPC, db_path=tmp_path / "unused.db", record_tf=False) + transform_tree = mocker.Mock() + mocker.patch.object(recorder, "_tf", transform_tree) + transform_tree.get.return_value = TransformStamped( + GeneratedHeader(GeneratedTime(0, 0), "world"), + "sensor_frame", + Transform(Vector3(1.0, 2.0, 3.0), Quaternion(0.0, 0.0, 0.0, 1.0)), + ) + message = GeneratedSegments(GeneratedHeader(GeneratedTime(0, 0), "sensor_frame"), []) + try: + assert recorder._resolve_ts("segments", message) == 0.0 + pose = await recorder._resolve_pose("segments", message, 0.0) + assert type(pose) is Pose + assert pose.position.x == 1.0 + assert pose.position.y == 2.0 + assert pose.position.z == 3.0 + assert pose.orientation.w == 1.0 + transform_tree.get.assert_called_once_with( + "world", + "sensor_frame", + time_point=0.0, + time_tolerance=0.5, + ) + finally: + recorder.stop() + assert not (tmp_path / "unused.db").exists() + + +class MemqRPC(LCMRPC): + def __init__(self, **kwargs): + super().__init__(url="memq://", **kwargs) + + +@pytest.fixture +def tf_recording_scheduler(monkeypatch): + scheduler = ThreadPoolScheduler(max_workers=get_max_workers()) + monkeypatch.setattr("dimos.utils.reactive.get_scheduler", lambda: scheduler) + monkeypatch.setattr("dimos.utils.threadpool.get_scheduler", lambda: scheduler) + yield scheduler + scheduler.executor.shutdown(wait=True) + + +def test_recorder_tf_cdr_boundary_records_each_transform_without_changing_header( + mocker, monkeypatch, tmp_path, tf_recording_scheduler +): + monkeypatch.setenv("LCM_DEFAULT_URL", "memq://") + mocker.patch("dimos.core.module.get_loop", return_value=(None, None)) + mocker.patch.object(LCMRPC, "start") + mocker.patch.object(LCMRPC, "serve_module_rpc") + path = tmp_path / "tf.db" + transforms = [ + TransformStamped( + GeneratedHeader(GeneratedTime(0, nanos), "world"), + "base_link", + Transform(Vector3(1.0, 2.0, 3.0), Quaternion(0.0, 0.0, 0.0, 1.0)), + ) + for nanos in (0, 1) + ] + ready = threading.Event() + observed = [] + + def receive(observation): + observed.append(observation) + if len(observed) == 2: + ready.set() + + with ExitStack() as cleanup: + recorder = Recorder(rpc_transport=MemqRPC, db_path=path) + cleanup.callback(recorder.stop) + transport = make_transport("tf", TFMessage, g=GlobalConfig(transport="lcm")) + cleanup.callback(transport.stop) + recorder.set_transport("tf", transport) + recorder._record_tf() + live = recorder.store.streams.tf.live(buffer=Unbounded()) + cleanup.callback(live.stop) + cleanup.enter_context(live.subscribe(receive)) + transport.publish(TFMessage(transforms=transforms)) + assert ready.wait(5), "TF recorder did not append both transforms" + assert [observation.ts for observation in observed] == [0.0, 1e-9] + with SqliteStore(path=str(path), must_exist=True) as store: + values = list(store.streams.tf) + assert [item.data for item in values] == [TFMessage([value]) for value in transforms] + assert [item.data.transforms[0].header.stamp.nanosec for item in values] == [0, 1] + assert transforms[1].header.frame_id == "world" diff --git a/dimos/memory/tf.py b/dimos/memory/tf.py index 53e720a9125..bf59ec103aa 100644 --- a/dimos/memory/tf.py +++ b/dimos/memory/tf.py @@ -20,11 +20,11 @@ from typing import TYPE_CHECKING, Any, cast from dimos.memory.stream import Stream -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.messages.tf2_msgs.msg import TFMessage from dimos.protocol.tf.tf import MultiTBuffer if TYPE_CHECKING: - from dimos.msgs.geometry_msgs.Transform import Transform + from dimos.messages.geometry_msgs.msg import TransformStamped from dimos.protocol.tf.tf import TFLookup @@ -58,7 +58,7 @@ def from_store(cls, store: Any, stream: str = "tf") -> StreamTF | None: recorded = tf_stream(store, stream) return None if recorded is None else cls(recorded) - def publish(self, *args: Transform) -> None: + def publish(self, *args: TransformStamped) -> None: raise NotImplementedError("StreamTF is a read-only replay service.") def _load(self, lo: float, hi: float) -> None: @@ -86,7 +86,7 @@ def get( *, forward_tolerance: float = 0.0, warn: bool = True, - ) -> Transform | None: + ) -> TransformStamped | None: tp = time_point if tp is None: last = next(iter(self.stream.order_by("ts", desc=True).limit(1)), None) diff --git a/dimos/models/embedding/base.py b/dimos/models/embedding/base.py index e3cad47a9ad..40d0602d4e9 100644 --- a/dimos/models/embedding/base.py +++ b/dimos/models/embedding/base.py @@ -26,7 +26,7 @@ from dimos.types.timestamped import Timestamped if TYPE_CHECKING: - from dimos.msgs.sensor_msgs.Image import Image + from dimos.messages.sensor_msgs.msg import Image class EmbeddingModelConfig(LocalModelConfig): @@ -52,7 +52,7 @@ class Embedding(Timestamped): def __init__(self, vector: torch.Tensor | np.ndarray, timestamp: float | None = None) -> None: self.vector = vector - if timestamp: + if timestamp is not None: self.timestamp = timestamp else: self.timestamp = time.time() diff --git a/dimos/models/embedding/clip.py b/dimos/models/embedding/clip.py index 87e7cefa5db..7c6580d70ae 100644 --- a/dimos/models/embedding/clip.py +++ b/dimos/models/embedding/clip.py @@ -22,9 +22,11 @@ import torch.nn.functional as functional from transformers import CLIPModel as HFCLIPModel, CLIPProcessor +from dimos.message_utils.image import image_to_rgb +from dimos.message_utils.time import to_seconds +from dimos.messages.sensor_msgs.msg import Image from dimos.models.base import HuggingFaceModel from dimos.models.embedding.base import Embedding, EmbeddingModel, HuggingFaceEmbeddingModelConfig -from dimos.msgs.sensor_msgs.Image import Image class CLIPModelConfig(HuggingFaceEmbeddingModelConfig): @@ -57,7 +59,7 @@ def embed(self, *images: Image) -> Embedding | list[Embedding]: Returns embeddings as torch.Tensor on device for efficient GPU comparisons. """ # Convert to PIL images - pil_images = [PILImage.fromarray(img.to_rgb().data) for img in images] + pil_images = [PILImage.fromarray(image_to_rgb(img)) for img in images] # Process images with torch.inference_mode(): @@ -70,7 +72,7 @@ def embed(self, *images: Image) -> Embedding | list[Embedding]: # Create embeddings (keep as torch.Tensor on device) embeddings: list[Embedding] = [] for i, feat in enumerate(image_features): - timestamp = images[i].ts + timestamp = to_seconds(images[i].header.stamp) embeddings.append(Embedding(vector=feat, timestamp=timestamp)) return embeddings[0] if len(images) == 1 else embeddings diff --git a/dimos/models/embedding/dino.py b/dimos/models/embedding/dino.py index 54d500c693d..3af6d4ed331 100644 --- a/dimos/models/embedding/dino.py +++ b/dimos/models/embedding/dino.py @@ -22,9 +22,11 @@ import torch.nn.functional as functional from transformers import AutoImageProcessor, AutoModel +from dimos.message_utils.image import image_to_rgb +from dimos.message_utils.time import to_seconds +from dimos.messages.sensor_msgs.msg import Image from dimos.models.base import HuggingFaceModel from dimos.models.embedding.base import Embedding, EmbeddingModel, HuggingFaceEmbeddingModelConfig -from dimos.msgs.sensor_msgs.Image import Image class DINOModelConfig(HuggingFaceEmbeddingModelConfig): @@ -51,7 +53,7 @@ def embed(self, image: Image, /) -> Embedding: ... @overload def embed(self, *images: Image) -> list[Embedding]: ... def embed(self, *images: Image) -> Embedding | list[Embedding]: - pil_images = [PILImage.fromarray(img.to_rgb().data) for img in images] + pil_images = [PILImage.fromarray(image_to_rgb(img)) for img in images] with torch.inference_mode(): inputs = self._processor(images=pil_images, return_tensors="pt").to(self.config.device) outputs = self._model(**inputs) @@ -68,7 +70,7 @@ def embed(self, *images: Image) -> Embedding | list[Embedding]: embeddings: list[Embedding] = [] for i, feat in enumerate(feats): - embeddings.append(Embedding(vector=feat, timestamp=images[i].ts)) + embeddings.append(Embedding(vector=feat, timestamp=to_seconds(images[i].header.stamp))) return embeddings[0] if len(images) == 1 else embeddings @overload diff --git a/dimos/models/embedding/siglip.py b/dimos/models/embedding/siglip.py index 225c98a04f4..da5f523d528 100644 --- a/dimos/models/embedding/siglip.py +++ b/dimos/models/embedding/siglip.py @@ -22,9 +22,11 @@ import torch.nn.functional as functional from transformers import SiglipModel as HFSiglipModel, SiglipProcessor +from dimos.message_utils.image import image_to_rgb +from dimos.message_utils.time import to_seconds +from dimos.messages.sensor_msgs.msg import Image from dimos.models.base import HuggingFaceModel from dimos.models.embedding.base import Embedding, EmbeddingModel, HuggingFaceEmbeddingModelConfig -from dimos.msgs.sensor_msgs.Image import Image class SigLIPModelConfig(HuggingFaceEmbeddingModelConfig): @@ -59,7 +61,7 @@ def embed(self, image: Image, /) -> Embedding: ... def embed(self, *images: Image) -> list[Embedding]: ... def embed(self, *images: Image) -> Embedding | list[Embedding]: """Embed one or more images into the shared image-text space.""" - pil_images = [PILImage.fromarray(img.to_rgb().data) for img in images] + pil_images = [PILImage.fromarray(image_to_rgb(img)) for img in images] with torch.inference_mode(): inputs = self._processor(images=pil_images, return_tensors="pt").to(self.config.device) @@ -68,7 +70,8 @@ def embed(self, *images: Image) -> Embedding | list[Embedding]: image_features = functional.normalize(image_features, dim=-1) embeddings = [ - Embedding(vector=feat, timestamp=images[i].ts) for i, feat in enumerate(image_features) + Embedding(vector=feat, timestamp=to_seconds(images[i].header.stamp)) + for i, feat in enumerate(image_features) ] return embeddings[0] if len(images) == 1 else embeddings diff --git a/dimos/models/embedding/siglip2.py b/dimos/models/embedding/siglip2.py index 963707fffe5..c6a2c5be572 100644 --- a/dimos/models/embedding/siglip2.py +++ b/dimos/models/embedding/siglip2.py @@ -22,9 +22,11 @@ import torch.nn.functional as functional from transformers import SiglipModel as HFSiglipModel, SiglipProcessor +from dimos.message_utils.image import image_to_rgb +from dimos.message_utils.time import to_seconds +from dimos.messages.sensor_msgs.msg import Image from dimos.models.base import HuggingFaceModel from dimos.models.embedding.base import Embedding, EmbeddingModel, HuggingFaceEmbeddingModelConfig -from dimos.msgs.sensor_msgs.Image import Image TEXT_MAX_LENGTH = 64 @@ -61,7 +63,7 @@ def embed(self, image: Image, /) -> Embedding: ... def embed(self, *images: Image) -> list[Embedding]: ... def embed(self, *images: Image) -> Embedding | list[Embedding]: """Embed one or more images into the shared image-text space.""" - pil_images = [PILImage.fromarray(img.to_rgb().data) for img in images] + pil_images = [PILImage.fromarray(image_to_rgb(img)) for img in images] with torch.inference_mode(): inputs = self._processor(images=pil_images, return_tensors="pt") @@ -71,7 +73,8 @@ def embed(self, *images: Image) -> Embedding | list[Embedding]: image_features = functional.normalize(image_features, dim=-1) embeddings = [ - Embedding(vector=feat, timestamp=images[i].ts) for i, feat in enumerate(image_features) + Embedding(vector=feat, timestamp=to_seconds(images[i].header.stamp)) + for i, feat in enumerate(image_features) ] return embeddings[0] if len(images) == 1 else embeddings diff --git a/dimos/models/embedding/test_generated_images.py b/dimos/models/embedding/test_generated_images.py new file mode 100644 index 00000000000..793e59acb2e --- /dev/null +++ b/dimos/models/embedding/test_generated_images.py @@ -0,0 +1,53 @@ +# Copyright 2025-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 unittest.mock import PropertyMock + +import numpy as np +import pytest +import torch +from transformers.feature_extraction_utils import BatchFeature +from transformers.modeling_outputs import BaseModelOutputWithPooling + +from dimos.message_utils.image import image_from_array, image_view +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.std_msgs.msg import Header +from dimos.models.embedding.clip import CLIPModel +from dimos.models.embedding.dino import DINOModel +from dimos.models.embedding.siglip import SigLIPModel +from dimos.models.embedding.siglip2 import SigLIP2Model + + +@pytest.mark.parametrize("model_type", [CLIPModel, DINOModel, SigLIPModel, SigLIP2Model]) +def test_generated_embeddings_keep_zero_source_time_and_convert_rgb(mocker, model_type): + processor, network = mocker.Mock(), mocker.Mock() + features = torch.tensor([[3.0, 4.0]]) + processor.return_value = BatchFeature(data={"pixel_values": torch.zeros(1, 3, 4, 8)}) + network.get_image_features.return_value = features + network.return_value = BaseModelOutputWithPooling( + last_hidden_state=features[:, None, :], pooler_output=features + ) + mocker.patch.object(model_type, "_processor", new_callable=PropertyMock, return_value=processor) + mocker.patch.object(model_type, "_model", new_callable=PropertyMock, return_value=network) + image = image_from_array( + np.full((4, 8, 3), [0, 0, 255], dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera", stamp=Time(sec=0, nanosec=0)), + ) + model = model_type(device="cpu") + result = model.embed(image) + assert result.timestamp == 0.0 + torch.testing.assert_close(result.vector, torch.tensor([0.6, 0.8])) + assert processor.call_args.kwargs["images"][0].getpixel((0, 0)) == (255, 0, 0) + np.testing.assert_array_equal(image_view(image)[0, 0], [0, 0, 255]) diff --git a/dimos/models/embedding/treid.py b/dimos/models/embedding/treid.py index 3c3e33dd27e..56e7cd54465 100644 --- a/dimos/models/embedding/treid.py +++ b/dimos/models/embedding/treid.py @@ -23,9 +23,11 @@ import torch.nn.functional as functional from torchreid import utils as torchreid_utils +from dimos.message_utils.image import image_to_bgr +from dimos.message_utils.time import to_seconds +from dimos.messages.sensor_msgs.msg import Image from dimos.models.base import LocalModel from dimos.models.embedding.base import Embedding, EmbeddingModel, EmbeddingModelConfig -from dimos.msgs.sensor_msgs.Image import Image from dimos.utils.data import get_data @@ -60,7 +62,7 @@ def embed(self, *images: Image) -> Embedding | list[Embedding]: Returns embeddings as torch.Tensor on device for efficient GPU comparisons. """ # Convert to numpy arrays - torchreid expects numpy arrays or file paths - np_images = [img.to_opencv() for img in images] + np_images = [image_to_bgr(img) for img in images] # Extract features with torch.inference_mode(): @@ -78,7 +80,7 @@ def embed(self, *images: Image) -> Embedding | list[Embedding]: # Create embeddings (keep as torch.Tensor on device) embeddings = [] for i, feat in enumerate(features_tensor): - timestamp = images[i].ts + timestamp = to_seconds(images[i].header.stamp) embeddings.append(Embedding(vector=feat, timestamp=timestamp)) return embeddings[0] if len(images) == 1 else embeddings diff --git a/dimos/models/segmentation/edge_tam.py b/dimos/models/segmentation/edge_tam.py index 667b05b4047..443fc9114cd 100644 --- a/dimos/models/segmentation/edge_tam.py +++ b/dimos/models/segmentation/edge_tam.py @@ -27,8 +27,9 @@ from PIL import Image as PILImage import torch +from dimos.message_utils.image import image_to_bgr, image_to_jpeg +from dimos.messages.sensor_msgs.msg import Image from dimos.models.base import default_torch_device -from dimos.msgs.sensor_msgs.Image import Image from dimos.perception.detection.detectors.base import Detector from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D @@ -172,7 +173,7 @@ def propose_all( ) self._mask_generators[points_per_side] = generator - rgb = cv2.cvtColor(image.to_opencv(), cv2.COLOR_BGR2RGB) + rgb = cv2.cvtColor(image_to_bgr(image), cv2.COLOR_BGR2RGB) amp = ( torch.autocast("cuda", dtype=torch.bfloat16) if self._predictor.device.type == "cuda" @@ -203,7 +204,7 @@ def segment( return ImageDetections2D(detections.image, []) image = detections.image - rgb = cv2.cvtColor(image.to_opencv(), cv2.COLOR_BGR2RGB) + rgb = cv2.cvtColor(image_to_bgr(image), cv2.COLOR_BGR2RGB) # bfloat16 autocast is CUDA-only; MPS runs in float32 amp = ( @@ -254,7 +255,7 @@ def _prepare_frame(self, image: Image) -> torch.Tensor: """Prepare frame for SAM2 (resize, normalize, convert to tensor).""" import cv2 - cv_image = image.to_opencv() + cv_image = image_to_bgr(image) rgb_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB) pil_image = PILImage.fromarray(rgb_image) @@ -407,7 +408,7 @@ def stop(self) -> None: def _temp_dir_context(image: Image) -> Generator[str, None, None]: path = tempfile.mkdtemp() - image.save(f"{path}/00000.jpg") + Path(path, "00000.jpg").write_bytes(image_to_jpeg(image)) try: yield path diff --git a/dimos/models/segmentation/test_generated_edge_tam_images.py b/dimos/models/segmentation/test_generated_edge_tam_images.py new file mode 100644 index 00000000000..f921fe971bd --- /dev/null +++ b/dimos/models/segmentation/test_generated_edge_tam_images.py @@ -0,0 +1,50 @@ +# 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 types import SimpleNamespace + +import cv2 +import numpy as np +import torch + +from dimos.message_utils.image import image_from_array +from dimos.models.segmentation import edge_tam +from dimos.perception.detection.type.detection2d.seg import Detection2DSeg + + +def test_generated_tracker_frame_and_initial_jpeg(monkeypatch): + monkeypatch.setattr( + edge_tam, "_build_model", lambda: SimpleNamespace(image_size=8, device="cpu") + ) + processor = edge_tam.EdgeTAMProcessor() + pixels = np.zeros((12, 16, 3), dtype=np.uint8) + pixels[:, :, 0] = 255 + image = image_from_array(pixels, encoding="rgb8") + frame = processor._prepare_frame(image) + assert frame.shape == (3, 8, 8) + torch.testing.assert_close( + frame[:, 0, 0], torch.tensor([(1 - 0.485) / 0.229, -0.456 / 0.224, -0.406 / 0.225]) + ) + with edge_tam._temp_dir_context(image) as directory: + path = Path(directory) + decoded = cv2.imread(str(path / "00000.jpg")) + assert decoded.shape == (12, 16, 3) + assert decoded[0, 0, 2] > 240 + assert not path.exists() + mask = torch.zeros((12, 16)) + mask[2:8, 3:10] = 1 + detection = Detection2DSeg.from_sam2_result(mask, 7, image) + assert detection.bbox == (3, 2, 9, 7) + assert detection.to_ros_detection2d().header == image.header diff --git a/dimos/models/segmentation/test_yoloe.py b/dimos/models/segmentation/test_yoloe.py index 5848f1aeca5..c1ed05664b4 100644 --- a/dimos/models/segmentation/test_yoloe.py +++ b/dimos/models/segmentation/test_yoloe.py @@ -19,19 +19,21 @@ import numpy as np from ultralytics.models.yolo.yoloe import YOLOEVPSegPredictor +from dimos.message_utils.image import image_from_array, image_to_bgr +from dimos.message_utils.time import time_from_seconds, to_seconds +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.std_msgs.msg import Header from dimos.models.segmentation.yoloe import YoloeBoxSegmenter -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.detection.type.detection2d.seg import Detection2DSeg def _image() -> Image: - return Image( - data=np.zeros((32, 48, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera", - ts=12.5, + return image_from_array( + np.zeros((32, 48, 3), dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera", stamp=time_from_seconds(12.5)), ) @@ -42,7 +44,7 @@ def _box(image: Image, *, bbox: tuple[float, float, float, float]) -> Detection2 class_id=7, confidence=0.73, name="white mug", - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, ) @@ -60,7 +62,7 @@ def _mask( class_id=prompt_id, confidence=0.11, name=f"prompt-{prompt_id}", - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, mask=np.full((32, 48), value, dtype=np.uint8), ) @@ -137,7 +139,7 @@ def test_visual_box_prompts_use_segmentation_predictor(mocker: Any) -> None: get_data.assert_called_once_with("models_yoloe") model_factory.assert_called_once_with(Path("/models/yoloe-11s-seg.pt")) kwargs = dict(model.predict.call_args.kwargs) - np.testing.assert_array_equal(kwargs.pop("source"), image.to_opencv()) + np.testing.assert_array_equal(kwargs.pop("source"), image_to_bgr(image)) visual_prompts = kwargs.pop("visual_prompts") np.testing.assert_array_equal( visual_prompts["bboxes"], np.asarray([detection.bbox], dtype=np.float64) diff --git a/dimos/models/segmentation/yoloe.py b/dimos/models/segmentation/yoloe.py index a13d0a9d009..47e2574c9f6 100644 --- a/dimos/models/segmentation/yoloe.py +++ b/dimos/models/segmentation/yoloe.py @@ -23,7 +23,8 @@ from numpy.typing import NDArray from ultralytics.models.yolo.yoloe import YOLOEVPSegPredictor -from dimos.msgs.sensor_msgs.Image import Image +from dimos.message_utils.image import image_to_bgr +from dimos.messages.sensor_msgs.msg import Image from dimos.perception.detection.detectors.yoloe import Yoloe2DDetector, YoloePromptMode from dimos.perception.detection.type.detection2d.bbox import Bbox, Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D @@ -58,7 +59,7 @@ def predict_image(self, image: Image) -> ImageDetections2D: raise RuntimeError("YOLO-E segmentation requires visual box prompts") with self._lock: results = self.model.predict( - source=image.to_opencv(), + source=image_to_bgr(image), device=self.device, conf=self.conf, iou=0.6, diff --git a/dimos/models/vl/base.py b/dimos/models/vl/base.py index 55b616176dc..14f0072ba04 100644 --- a/dimos/models/vl/base.py +++ b/dimos/models/vl/base.py @@ -19,8 +19,12 @@ from typing import Any import warnings +import cv2 + from dimos.core.resource import Resource -from dimos.msgs.sensor_msgs.Image import Image +from dimos.message_utils.image import image_from_array, image_view +from dimos.message_utils.time import header_now, to_seconds +from dimos.messages.sensor_msgs.msg import Image from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.detection.type.detection2d.point import Detection2DPoint @@ -83,7 +87,6 @@ def vlm_detection_to_detection2d( Detection2DBBox instance or None if invalid """ # Here to prevent unwanted imports in the file. - from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox # Validate list/tuple structure if not isinstance(vlm_detection, (list, tuple)): @@ -116,7 +119,7 @@ def vlm_detection_to_detection2d( class_id=-1, confidence=1.0, name=name, - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, ) @@ -140,7 +143,6 @@ def vlm_point_to_detection2d_point( Returns: Detection2DPoint instance or None if invalid """ - from dimos.perception.detection.type.detection2d.point import Detection2DPoint # Validate list/tuple structure if not isinstance(vlm_point, (list, tuple)): @@ -166,7 +168,7 @@ def vlm_point_to_detection2d_point( x=x, y=y, name=name, - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, track_id=track_id, ) @@ -198,7 +200,17 @@ def _prepare_image(self, image: Image) -> tuple[Image, float]: """ if self.config.auto_resize is not None: max_w, max_h = self.config.auto_resize - return image.resize_to_fit(max_w, max_h) + if max_w <= 0 or max_h <= 0: + raise ValueError("auto_resize dimensions must be positive") + scale = min(1.0, max_w / image.width, max_h / image.height) + if scale == 1.0: + return image, scale + pixels = cv2.resize( + image_view(image), + (max(1, int(image.width * scale)), max(1, int(image.height * scale))), + interpolation=cv2.INTER_AREA, + ) + return image_from_array(pixels, encoding=image.encoding, header=image.header), scale return image, 1.0 @abstractmethod @@ -252,10 +264,13 @@ def caption(self, image: Image) -> str: def start(self) -> None: """Start the model by running a simple query (Resource interface).""" try: - image = Image.from_file(get_data("cafe-smol.jpg")).to_rgb() + pixels = cv2.imread(str(get_data("cafe-smol.jpg"))) + if pixels is None: + raise ValueError("Could not read model warmup image") + image = image_from_array(pixels, encoding="bgr8", header=header_now()) self.query(image, "What is this?") except Exception: - pass + logger.debug("Vision model warmup was unavailable") # requery once if JSON parsing fails @retry(max_retries=2, on_exception=json.JSONDecodeError, delay=0.0) # type: ignore[untyped-decorator] @@ -267,7 +282,6 @@ def query_detections( self, image: Image, query: str, **kwargs: Any ) -> ImageDetections2D[Detection2DBBox]: # Here to prevent unwanted imports in the file. - from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D full_query = f"""show me bounding boxes in pixels for this query: `{query}` @@ -328,7 +342,6 @@ def query_points( ImageDetections2D containing Detection2DPoint instances """ # Here to prevent unwanted imports in the file. - from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D full_query = f"""Show me point coordinates in pixels for this query: `{query}` diff --git a/dimos/models/vl/qwen.py b/dimos/models/vl/qwen.py index 84542b60c68..a63415b9424 100644 --- a/dimos/models/vl/qwen.py +++ b/dimos/models/vl/qwen.py @@ -12,15 +12,19 @@ # See the License for the specific language governing permissions and # limitations under the License. +import base64 from functools import cached_property import os from typing import Any +import warnings import numpy as np from openai import OpenAI +from dimos.message_utils.image import image_from_array, image_to_jpeg +from dimos.message_utils.time import header_now +from dimos.messages.sensor_msgs.msg import Image from dimos.models.vl.base import VlModel, VlModelConfig -from dimos.msgs.sensor_msgs.Image import Image # Must be a model that returns ABSOLUTE pixel coordinates. The qwen3-vl-* models return # normalized 0-1000 coordinates and ignore prompts asking for pixels, which silently breaks @@ -53,20 +57,16 @@ def _client(self) -> OpenAI: def query(self, image: Image | np.ndarray, query: str) -> str: # type: ignore[override] if isinstance(image, np.ndarray): - import warnings - warnings.warn( "QwenVlModel.query should receive standard dimos Image type, not a numpy array", DeprecationWarning, stacklevel=2, ) - - image = Image.from_numpy(image) - + image = image_from_array(image, encoding="bgr8", header=header_now()) # Apply auto_resize if configured image, _ = self._prepare_image(image) - img_base64 = image.to_base64() + img_base64 = base64.b64encode(image_to_jpeg(image, quality=80)).decode("ascii") response = self._client.chat.completions.create( model=self.config.model_name, @@ -76,7 +76,7 @@ def query(self, image: Image | np.ndarray, query: str) -> str: # type: ignore[o "content": [ { "type": "image_url", - "image_url": {"url": f"data:image/png;base64,{img_base64}"}, + "image_url": {"url": f"data:image/jpeg;base64,{img_base64}"}, }, {"type": "text", "text": query}, ], @@ -101,7 +101,7 @@ def query_batch( { "type": "image_url", "image_url": { - "url": f"data:image/png;base64,{self._prepare_image(img)[0].to_base64()}" + "url": f"data:image/jpeg;base64,{base64.b64encode(image_to_jpeg(self._prepare_image(img)[0], quality=80)).decode('ascii')}" }, } for img in images diff --git a/dimos/models/vl/test_base.py b/dimos/models/vl/test_base.py index 92b8406f136..b10a53949a5 100644 --- a/dimos/models/vl/test_base.py +++ b/dimos/models/vl/test_base.py @@ -12,16 +12,18 @@ # See the License for the specific language governing permissions and # limitations under the License. -from unittest.mock import MagicMock +import base64 +from contextlib import nullcontext +import cv2 +import numpy as np +from openai.types.chat import ChatCompletion import pytest +from dimos.message_utils.image import image_from_array +from dimos.message_utils.time import header_now from dimos.models.vl.qwen import QwenVlModel -from dimos.msgs.sensor_msgs.Image import Image from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D -from dimos.utils.data import get_data - -pytestmark = pytest.mark.self_hosted # Captured actual response from Qwen API for cafe.jpg with query "humans" # Added garbage around JSON to ensure we are robustly extracting it @@ -41,14 +43,28 @@ """ -def test_query_detections_mocked() -> None: +def test_query_detections_mocked(mocker) -> None: """Test query_detections with mocked API response (no API key required).""" - # Load test image - image = Image.from_file(get_data("cafe.jpg")) + image = image_from_array( + np.zeros((864, 1152, 3), dtype=np.uint8), encoding="rgb8", header=header_now("camera") + ) # Create model and mock the query method - model = QwenVlModel() - model.query = MagicMock(return_value=MOCK_QWEN_RESPONSE) + client = mocker.patch("dimos.models.vl.qwen.OpenAI").return_value + client.chat.completions.create.return_value = ChatCompletion( + id="offline", + object="chat.completion", + created=0, + model="test", + choices=[ + { + "index": 0, + "finish_reason": "stop", + "message": {"role": "assistant", "content": MOCK_QWEN_RESPONSE}, + } + ], + ) + model = QwenVlModel(api_key="offline-test") # Query for humans in the image query = "humans" @@ -63,7 +79,7 @@ def test_query_detections_mocked() -> None: ) # Verify each detection - img_height, img_width = image.shape[:2] + img_height, img_width = image.height, image.width for i, detection in enumerate(detections.detections): # Verify attributes @@ -86,4 +102,36 @@ def test_query_detections_mocked() -> None: assert 0 <= y1 <= img_height, f"Detection {i}: y1={y1} out of bounds" assert 0 <= y2 <= img_height, f"Detection {i}: y2={y2} out of bounds" - print(f"✓ Successfully processed {len(detections.detections)} mocked detections") + +@pytest.mark.parametrize("as_array", [False, True]) +def test_qwen_encodes_source_pixels_and_keeps_numpy_compatibility(mocker, as_array): + client = mocker.patch("dimos.models.vl.qwen.OpenAI").return_value + client.chat.completions.create.return_value = ChatCompletion( + id="offline", + object="chat.completion", + created=0, + model="test", + choices=[ + { + "index": 0, + "finish_reason": "stop", + "message": {"role": "assistant", "content": "red"}, + } + ], + ) + pixels = np.full((20, 40, 3), [0, 0, 255], dtype=np.uint8) + image = image_from_array(pixels, encoding="bgr8", header=header_now("camera")) + model = QwenVlModel(api_key="offline-test", auto_resize=(20, 20)) + with pytest.warns(DeprecationWarning) if as_array else nullcontext(): + assert model.query(pixels if as_array else image, "color?") == "red" + request = client.chat.completions.create.call_args.kwargs + url = request["messages"][0]["content"][0]["image_url"]["url"] + prefix, encoded = url.split(",", 1) + assert prefix == "data:image/jpeg;base64" + decoded = cv2.imdecode( + np.frombuffer(base64.b64decode(encoded), dtype=np.uint8), cv2.IMREAD_COLOR + ) + assert decoded.shape == (10, 20, 3) + np.testing.assert_allclose(decoded[0, 0], [0, 0, 255], atol=2) + assert image.header.frame_id == "camera" + assert (image.width, image.height) == (40, 20) diff --git a/dimos/navigation/global_planner/mls_planner/rust/Cargo.lock b/dimos/navigation/global_planner/mls_planner/rust/Cargo.lock index 6e331d800ac..8af4f7f7a1e 100644 --- a/dimos/navigation/global_planner/mls_planner/rust/Cargo.lock +++ b/dimos/navigation/global_planner/mls_planner/rust/Cargo.lock @@ -582,7 +582,7 @@ dependencies = [ [[package]] name = "dimos-lcm" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", "socket2 0.5.10", @@ -595,7 +595,7 @@ version = "0.1.0" dependencies = [ "ahash", "dimos-module", - "lcm-msgs", + "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen)", "rayon", "serde", "smallvec", @@ -619,11 +619,11 @@ dependencies = [ [[package]] name = "dimos-module" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "dimos-lcm", "dimos-module-macros", - "lcm-msgs", + "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549)", "nalgebra", "rayon", "serde", @@ -639,7 +639,7 @@ dependencies = [ [[package]] name = "dimos-module-macros" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "proc-macro2", "quote", @@ -1433,6 +1433,14 @@ dependencies = [ "byteorder", ] +[[package]] +name = "lcm-msgs" +version = "0.1.0" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" +dependencies = [ + "byteorder", +] + [[package]] name = "libc" version = "0.2.189" diff --git a/dimos/navigation/global_planner/mls_planner/rust/Cargo.toml b/dimos/navigation/global_planner/mls_planner/rust/Cargo.toml index bb04033aac2..da594faefc7 100644 --- a/dimos/navigation/global_planner/mls_planner/rust/Cargo.toml +++ b/dimos/navigation/global_planner/mls_planner/rust/Cargo.toml @@ -37,7 +37,7 @@ node_edges = "nav_msgs.LineSegments3D" path = "nav_msgs.Path" [dependencies] -dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6", version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal", "time"] } serde = { version = "1", features = ["derive"] } diff --git a/dimos/navigation/global_planner/mls_planner/rust/crate-hashes.json b/dimos/navigation/global_planner/mls_planner/rust/crate-hashes.json index 4c5fde86d6c..187d28513c6 100644 --- a/dimos/navigation/global_planner/mls_planner/rust/crate-hashes.json +++ b/dimos/navigation/global_planner/mls_planner/rust/crate-hashes.json @@ -1,6 +1,7 @@ { - "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module-macros@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=" + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module-macros@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=" } \ No newline at end of file diff --git a/dimos/navigation/local_planner/rust/Cargo.lock b/dimos/navigation/local_planner/rust/Cargo.lock index 97e25524ce5..24e92d272ff 100644 --- a/dimos/navigation/local_planner/rust/Cargo.lock +++ b/dimos/navigation/local_planner/rust/Cargo.lock @@ -582,7 +582,7 @@ dependencies = [ [[package]] name = "dimos-lcm" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", "socket2 0.5.10", @@ -594,7 +594,7 @@ name = "dimos-local-planner" version = "0.1.0" dependencies = [ "dimos-module", - "lcm-msgs", + "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen)", "serde", "serde_json", "tokio", @@ -606,11 +606,11 @@ dependencies = [ [[package]] name = "dimos-module" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "dimos-lcm", "dimos-module-macros", - "lcm-msgs", + "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549)", "nalgebra", "rayon", "serde", @@ -626,7 +626,7 @@ dependencies = [ [[package]] name = "dimos-module-macros" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "proc-macro2", "quote", @@ -1405,6 +1405,14 @@ dependencies = [ "byteorder", ] +[[package]] +name = "lcm-msgs" +version = "0.1.0" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" +dependencies = [ + "byteorder", +] + [[package]] name = "libc" version = "0.2.189" diff --git a/dimos/navigation/local_planner/rust/Cargo.toml b/dimos/navigation/local_planner/rust/Cargo.toml index 5e747841873..1d90441cb59 100644 --- a/dimos/navigation/local_planner/rust/Cargo.toml +++ b/dimos/navigation/local_planner/rust/Cargo.toml @@ -17,7 +17,7 @@ required-features = ["module"] [dependencies] serde = { version = "1", features = ["derive"] } serde_json = { version = "1", features = ["float_roundtrip"] } -dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", optional = true, version = "=0.1.0" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6", optional = true, version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen", optional = true } tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal", "time"], optional = true } tracing = { version = "0.1", optional = true } diff --git a/dimos/navigation/local_planner/rust/crate-hashes.json b/dimos/navigation/local_planner/rust/crate-hashes.json index 4c5fde86d6c..187d28513c6 100644 --- a/dimos/navigation/local_planner/rust/crate-hashes.json +++ b/dimos/navigation/local_planner/rust/crate-hashes.json @@ -1,6 +1,7 @@ { - "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module-macros@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=" + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module-macros@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=" } \ No newline at end of file diff --git a/dimos/navigation/trajectory_follower/fancy/rust/Cargo.lock b/dimos/navigation/trajectory_follower/fancy/rust/Cargo.lock index f319e21e7c4..9bce5ee4a4b 100644 --- a/dimos/navigation/trajectory_follower/fancy/rust/Cargo.lock +++ b/dimos/navigation/trajectory_follower/fancy/rust/Cargo.lock @@ -582,7 +582,7 @@ dependencies = [ [[package]] name = "dimos-lcm" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", "socket2 0.5.10", @@ -594,7 +594,7 @@ name = "dimos-local-planner" version = "0.1.0" dependencies = [ "dimos-module", - "lcm-msgs", + "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen)", "serde", "serde_json", "tokio", @@ -606,11 +606,11 @@ dependencies = [ [[package]] name = "dimos-module" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "dimos-lcm", "dimos-module-macros", - "lcm-msgs", + "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549)", "nalgebra", "rayon", "serde", @@ -626,7 +626,7 @@ dependencies = [ [[package]] name = "dimos-module-macros" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "proc-macro2", "quote", @@ -640,7 +640,7 @@ version = "0.1.0" dependencies = [ "dimos-local-planner", "dimos-module", - "lcm-msgs", + "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen)", "numpy", "pyo3", "serde", @@ -1437,6 +1437,14 @@ dependencies = [ "byteorder", ] +[[package]] +name = "lcm-msgs" +version = "0.1.0" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" +dependencies = [ + "byteorder", +] + [[package]] name = "libc" version = "0.2.189" diff --git a/dimos/navigation/trajectory_follower/fancy/rust/Cargo.toml b/dimos/navigation/trajectory_follower/fancy/rust/Cargo.toml index 9baf7d0d17b..fd46937e34f 100644 --- a/dimos/navigation/trajectory_follower/fancy/rust/Cargo.toml +++ b/dimos/navigation/trajectory_follower/fancy/rust/Cargo.toml @@ -20,7 +20,7 @@ required-features = ["module"] # The body (`Emb`) the laws are parameterised from; python feature off, see adapter. dimos-local-planner = { path = "../../../local_planner/rust", default-features = false } serde = { version = "1", features = ["derive"], optional = true } -dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", optional = true, version = "=0.1.0" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6", optional = true, version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen", optional = true } tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal", "time"], optional = true } tracing = { version = "0.1", optional = true } diff --git a/dimos/navigation/trajectory_follower/fancy/rust/crate-hashes.json b/dimos/navigation/trajectory_follower/fancy/rust/crate-hashes.json index 4c5fde86d6c..187d28513c6 100644 --- a/dimos/navigation/trajectory_follower/fancy/rust/crate-hashes.json +++ b/dimos/navigation/trajectory_follower/fancy/rust/crate-hashes.json @@ -1,6 +1,7 @@ { - "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module-macros@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=" + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module-macros@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=" } \ No newline at end of file diff --git a/dimos/navigation/trajectory_follower/fancy/rust/flake.lock b/dimos/navigation/trajectory_follower/fancy/rust/flake.lock index eeb07ccc345..b4e7367f4ca 100644 --- a/dimos/navigation/trajectory_follower/fancy/rust/flake.lock +++ b/dimos/navigation/trajectory_follower/fancy/rust/flake.lock @@ -90,11 +90,11 @@ }, "locked": { "dir": "dimos/navigation/local_planner/rust", - "lastModified": 1792745145, - "narHash": "sha256-cJLM/uV6rBI3BAo0B8Tn/F225s48HdNIu7cKaLLC7PE=", + "lastModified": 1792746978, + "narHash": "sha256-AYFe5iA3tnfHiZA8vVpX43dAk8Wl380NKTs1IH2OiGs=", "owner": "dimensionalOS", "repo": "dimos", - "rev": "e5c07673474aabf2256f53fd660511514610b91e", + "rev": "77e57872657a4094dff31cdc91cabdd132cbe8a1", "type": "github" }, "original": { diff --git a/dimos/perception/depth2depth_cloud/module.py b/dimos/perception/depth2depth_cloud/module.py index 36d2cdc4d0d..a0d08425971 100644 --- a/dimos/perception/depth2depth_cloud/module.py +++ b/dimos/perception/depth2depth_cloud/module.py @@ -23,10 +23,8 @@ from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.CompressedImage import CompressedImage -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.messages.sensor_msgs.msg import CameraInfo, CompressedImage, PointCloud2 +from dimos.messages.tf2_msgs.msg import TFMessage def has_nvidia_gpu() -> bool: @@ -56,7 +54,6 @@ class Depth2DepthCloudConfig(NativeModuleConfig): world_frame: str = "odom" lidar_history_s: float = Field(default=2.0, ge=0.0, le=30.0) max_anchor_range_m: float = Field(default=12.0, ge=0.1, le=200.0) - tf_tolerance_s: float = Field(default=0.1, ge=0.0, le=5.0) # Frames and scans wait this long for their transform before they are dropped. max_tf_lag_s: float = Field(default=0.5, ge=0.0, le=2.0) # Calibration (depth2depth::CalibrationConfig): edge-aware spread near lidar, a smooth fit away from it. @@ -114,6 +111,7 @@ class Depth2DepthCloud(NativeModule): camera_info: In[CameraInfo] lidar: In[PointCloud2] tf: In[TFMessage] + tf_static: In[TFMessage] depth_cloud: Out[PointCloud2] diff --git a/dimos/perception/depth2depth_cloud/rust/Cargo.lock b/dimos/perception/depth2depth_cloud/rust/Cargo.lock index 57ec25d21e6..1b5a06edeb1 100644 --- a/dimos/perception/depth2depth_cloud/rust/Cargo.lock +++ b/dimos/perception/depth2depth_cloud/rust/Cargo.lock @@ -383,6 +383,15 @@ dependencies = [ "shlex", ] +[[package]] +name = "cdr" +version = "0.2.4" +source = "git+https://github.com/TomCC7/cdr-rs?rev=38a4368255d408faf374726653b39d4a4e9735cb#38a4368255d408faf374726653b39d4a4e9735cb" +dependencies = [ + "byteorder", + "serde", +] + [[package]] name = "cfg-if" version = "1.0.4" @@ -831,8 +840,8 @@ name = "dimos-depth2depth-cloud" version = "0.1.0" dependencies = [ "depth2depth", + "dimos-generated-messages", "dimos-module", - "lcm-msgs", "nalgebra", "rayon", "serde", @@ -842,10 +851,21 @@ dependencies = [ "validator", ] +[[package]] +name = "dimos-generated-messages" +version = "0.0.14" +dependencies = [ + "cdr", + "heapless", + "ros2msg", + "serde", + "serde-big-array", +] + [[package]] name = "dimos-lcm" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", "socket2 0.5.10", @@ -854,9 +874,10 @@ dependencies = [ [[package]] name = "dimos-module" -version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +version = "0.2.0" dependencies = [ + "cdr", + "dimos-generated-messages", "dimos-lcm", "dimos-module-macros", "lcm-msgs", @@ -867,6 +888,7 @@ dependencies = [ "tokio", "tracing", "tracing-subscriber", + "transforms", "url", "validator", "zenoh", @@ -875,7 +897,6 @@ dependencies = [ [[package]] name = "dimos-module-macros" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" dependencies = [ "proc-macro2", "quote", @@ -1616,6 +1637,15 @@ dependencies = [ "zerocopy", ] +[[package]] +name = "hash32" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "47d60b12902ba28e2730cd37e95b8c9223af2808df9e902d4df49588d1470606" +dependencies = [ + "byteorder", +] + [[package]] name = "hashbrown" version = "0.12.3" @@ -1659,6 +1689,22 @@ name = "hashbrown" version = "0.17.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ed5909b6e89a2db4456e54cd5f673791d7eca6732202bbf2a9cc504fe2f9b84a" +dependencies = [ + "allocator-api2", + "equivalent", + "foldhash 0.2.0", +] + +[[package]] +name = "heapless" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +dependencies = [ + "hash32", + "serde", + "stable_deref_trait", +] [[package]] name = "heck" @@ -2078,7 +2124,7 @@ checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" [[package]] name = "lcm-msgs" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", ] @@ -2828,6 +2874,16 @@ dependencies = [ "zerocopy", ] +[[package]] +name = "prettyplease" +version = "0.2.37" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "479ca8adacdd7ce8f1fb39ce9ecccbfe93a3f1344b3d0d97f20bc0196208f62b" +dependencies = [ + "proc-macro2", + "syn 2.0.119", +] + [[package]] name = "proc-macro-crate" version = "3.5.0" @@ -3284,6 +3340,23 @@ dependencies = [ "unicode-ident", ] +[[package]] +name = "ros2msg" +version = "0.5.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2fe291be5cb6d4b8e6bf9c9de80c40bd1cb09ecac50deafa2fdcae0027e04988" +dependencies = [ + "heck", + "pest", + "pest_derive", + "prettyplease", + "proc-macro2", + "quote", + "regex", + "syn 2.0.119", + "thiserror 2.0.20", +] + [[package]] name = "rustc-hash" version = "2.1.3" @@ -3566,6 +3639,15 @@ dependencies = [ "serde_derive", ] +[[package]] +name = "serde-big-array" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11fc7cc2c76d73e0f27ee52abbd64eec84d46f370c88371120433196934e4b7f" +dependencies = [ + "serde", +] + [[package]] name = "serde_core" version = "1.0.229" @@ -4348,6 +4430,18 @@ dependencies = [ "tracing-serde", ] +[[package]] +name = "transforms" +version = "2.2.0" +source = "git+https://github.com/TomCC7/transforms?rev=5ae9563de0c02638c04e393aadfad28444a4398e#5ae9563de0c02638c04e393aadfad28444a4398e" +dependencies = [ + "approx", + "hashbrown 0.17.1", + "libm", + "serde", + "thiserror 2.0.20", +] + [[package]] name = "turbojpeg" version = "1.5.1" diff --git a/dimos/perception/depth2depth_cloud/rust/Cargo.toml b/dimos/perception/depth2depth_cloud/rust/Cargo.toml index 5f32e3bb96d..db07f0f6afe 100644 --- a/dimos/perception/depth2depth_cloud/rust/Cargo.toml +++ b/dimos/perception/depth2depth_cloud/rust/Cargo.toml @@ -21,17 +21,18 @@ path = "dimos_depth2depth_cloud::module::Depth2DepthCloud" python = "dimos.perception.depth2depth_cloud.module:Depth2DepthCloud" [package.metadata.dimos.module.depth2depth_cloud.inputs] -image = "sensor_msgs.CompressedImage" -camera_info = "sensor_msgs.CameraInfo" -lidar = "sensor_msgs.PointCloud2" -tf = "tf2_msgs.TFMessage" +image = "sensor_msgs/msg/CompressedImage" +camera_info = "sensor_msgs/msg/CameraInfo" +lidar = "sensor_msgs/msg/PointCloud2" +tf = "tf2_msgs/msg/TFMessage" +tf_static = "tf2_msgs/msg/TFMessage" [package.metadata.dimos.module.depth2depth_cloud.outputs] -depth_cloud = "sensor_msgs.PointCloud2" +depth_cloud = "sensor_msgs/msg/PointCloud2" [dependencies] -dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } -lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } +dimos-module = { path = "../../../../native/rust/dimos-module" } +dimos-generated-messages = { path = "../../../../build/message-source/rust" } tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal"] } serde = { version = "1", features = ["derive"] } tracing = "0.1" diff --git a/dimos/perception/depth2depth_cloud/rust/crate-hashes.json b/dimos/perception/depth2depth_cloud/rust/crate-hashes.json index 4c5fde86d6c..3ee4f926e23 100644 --- a/dimos/perception/depth2depth_cloud/rust/crate-hashes.json +++ b/dimos/perception/depth2depth_cloud/rust/crate-hashes.json @@ -1,6 +1,6 @@ { - "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", - "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module-macros@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=" + "git+https://github.com/TomCC7/cdr-rs?rev=38a4368255d408faf374726653b39d4a4e9735cb#cdr@0.2.4": "sha256-akg/bLBY3wQ1YRr2bijE5ZqB211Cst5lKjELLrgiG6o=", + "git+https://github.com/TomCC7/transforms?rev=5ae9563de0c02638c04e393aadfad28444a4398e#transforms@2.2.0": "sha256-Kh8aVjj7zQ+Xd1QHOcAhSJbubg7azaWFSMpaI/ZAgM4=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=" } \ No newline at end of file diff --git a/dimos/perception/depth2depth_cloud/rust/flake.nix b/dimos/perception/depth2depth_cloud/rust/flake.nix index 14d86bcec2b..b3491167cd4 100644 --- a/dimos/perception/depth2depth_cloud/rust/flake.nix +++ b/dimos/perception/depth2depth_cloud/rust/flake.nix @@ -1,5 +1,5 @@ { - description = "depth2depth_cloud native module for DimOS: the depth2depth crate behind an LCM wrapper"; + description = "depth2depth_cloud native module for DimOS: the depth2depth crate with CDR messages over the selected transport"; inputs = { nix-filter.url = "github:numtide/nix-filter"; @@ -32,7 +32,34 @@ cudaPackages = if system == "aarch64-linux" then pkgs.cudaPackages_12_6 else pkgs.cudaPackages_12_8; name = "dimos-depth2depth-cloud"; - src = nix-filter.lib { root = ./.; exclude = [ "target" "build" "result" "__pycache__" ]; }; + moduleSource = nix-filter.lib { root = ./.; exclude = [ "target" "build" "result" "__pycache__" ]; }; + sharedArchive = pkgs.fetchurl { + url = "https://codeload.github.com/dimensionalOS/dimos/tar.gz/a9365487b4323151ea85041c6a944b509c0c3268"; + sha256 = "e339487dd2281980fcde7ab03c46dfd2e274491e0f73e66d9c6cbcaeea8902b2"; + }; + sharedSource = pkgs.runCommand "dimos-depth-cloud-sdk-source" { + nativeBuildInputs = [ pkgs.gnutar pkgs.gzip ]; + } '' + mkdir -p $out + tar -xzf ${sharedArchive} --strip-components=1 -C $out + ''; + messageSource = (import "${sharedSource}/native/message-source.nix" { inherit pkgs; }).source; + # Cargo resolves only these explicit local paths. No checkout or generation + # process is needed at runtime; the message derivation is shared with pip. + src = pkgs.runCommand "depth-cloud-cdr-source" { } '' + cp -r ${moduleSource} $out + chmod -R u+w $out + mkdir -p $out/sdk + cp -r ${sharedSource}/native/rust/dimos-module ${sharedSource}/native/rust/dimos-module-macros $out/sdk/ + cp -r ${messageSource}/rust $out/messages + substituteInPlace $out/sdk/dimos-module/Cargo.toml \ + --replace-fail '../../../build/message-source/rust' '../../messages' + substituteInPlace $out/Cargo.toml --replace-fail '[workspace]' '[workspace] + exclude = ["sdk/dimos-module", "sdk/dimos-module-macros", "messages"]' + substituteInPlace $out/Cargo.toml \ + --replace-fail '../../../../native/rust/dimos-module' 'sdk/dimos-module' \ + --replace-fail '../../../../build/message-source/rust' 'messages' + ''; generated = crate2nix.tools.${system}.generatedCargoNix { inherit name src; }; ours = [ name "dimos-module" "dimos-module-macros" ]; diff --git a/dimos/perception/depth2depth_cloud/rust/src/module.rs b/dimos/perception/depth2depth_cloud/rust/src/module.rs index 3e40d6498c4..32c06646cb3 100644 --- a/dimos/perception/depth2depth_cloud/rust/src/module.rs +++ b/dimos/perception/depth2depth_cloud/rust/src/module.rs @@ -31,10 +31,16 @@ use std::time::{Duration, Instant}; use depth2depth::{ Calibration, CalibrationConfig, CloudOptions, Config as ModelConfig, Depth2Depth, }; -use dimos_module::pointcloud::extract_xyz; +use dimos_generated_messages::builtin_interfaces::msg::time::Time; +use dimos_generated_messages::sensor_msgs::msg::camera_info::CameraInfo; +use dimos_generated_messages::sensor_msgs::msg::compressed_image::CompressedImage; +use dimos_generated_messages::sensor_msgs::msg::point_cloud2::PointCloud2; +use dimos_generated_messages::sensor_msgs::msg::point_field::PointField; +use dimos_generated_messages::std_msgs::msg::header::Header; + +use dimos_module::cdr; +use dimos_module::pointcloud::{extract_xyz_rows, ExtractError}; use dimos_module::{native_config, warn_throttled, Input, Module, Output, Tf}; -use lcm_msgs::sensor_msgs::{CameraInfo, CompressedImage, PointCloud2, PointField}; -use lcm_msgs::std_msgs::{Header, Time}; use nalgebra::{Isometry3, Point3}; use tokio::sync::Notify; use tokio::task::JoinHandle; @@ -42,6 +48,26 @@ use tracing::info; use crate::undistort::{Lens, UndistortMap}; +fn extract_xyz(cloud: &PointCloud2) -> Result, ExtractError> { + let offset = |name: &str| { + cloud + .fields + .iter() + .find(|field| { + field.name == name && field.datatype == PointField::FLOAT32 && field.count == 1 + }) + .map(|field| field.offset as usize) + .ok_or(ExtractError("missing a scalar float32 xyz field")) + }; + extract_xyz_rows( + &cloud.data, + [cloud.width as usize, cloud.height as usize], + [cloud.point_step as usize, cloud.row_step as usize], + [offset("x")?, offset("y")?, offset("z")?], + cloud.is_bigendian, + ) +} + const TIMING_REPORT_EVERY: Duration = Duration::from_secs(5); /// Scans up to this far after a frame still anchor it... const SCAN_LEAD_S: f64 = 0.5; @@ -75,9 +101,6 @@ pub struct Config { /// Lidar points farther than this from the camera are not anchors. #[validate(range(min = 0.1, max = 200.0))] max_anchor_range_m: f64, - /// Largest gap between a stamp and the transform used for it. - #[validate(range(min = 0.0, max = 5.0))] - tf_tolerance_s: f64, /// Frames and scans wait this long for their transform before they are dropped. #[validate(range(min = 0.0, max = 2.0))] max_tf_lag_s: f64, @@ -122,19 +145,19 @@ pub struct Config { #[derive(Module)] #[module(name = "depth2depth_cloud", setup = start, teardown = stop)] pub struct Depth2DepthCloud { - #[input(decode = CompressedImage::decode, handler = on_image)] + #[input(decode = cdr::decode, handler = on_image)] image: Input, - #[input(decode = CameraInfo::decode, handler = on_camera_info)] + #[input(decode = cdr::decode, handler = on_camera_info)] camera_info: Input, - #[input(decode = PointCloud2::decode, handler = on_lidar)] + #[input(decode = cdr::decode, handler = on_lidar)] lidar: Input, #[tf] tf: Tf, - #[output(encode = PointCloud2::encode)] + #[output(encode = cdr::encode)] depth_cloud: Output, #[config] @@ -353,7 +376,10 @@ impl Worker { let newest = self.shared.frames.lock().unwrap().back().map(|image| { let frame_id = resolve_frame_id(&cfg.frame_id, &info.header.frame_id, &image.header.frame_id); - (frame_id.to_string(), seconds(&image.header.stamp)) + ( + frame_id.to_string(), + image.header.stamp.clone(), + ) }); let Some((frame_id, stamp)) = newest else { frame_arrived.await; @@ -362,13 +388,13 @@ impl Worker { let wait = Duration::from_secs_f64(cfg.max_tf_lag_s); let lookup = |target: &str| { let tf = self.tf.clone(); - let (target, frame_id) = (target.to_string(), frame_id.clone()); + let (target, frame_id, stamp) = + (target.to_string(), frame_id.clone(), stamp.clone()); async move { target.is_empty() || tf .lookup(&target, &frame_id) - .at(stamp) - .tolerance(cfg.tf_tolerance_s) + .at(stamp.clone()) .within(wait) .await .is_some() @@ -385,7 +411,7 @@ impl Worker { .frames .lock() .unwrap() - .retain(|image| seconds(&image.header.stamp) > stamp); + .retain(|image| (image.header.stamp.sec, image.header.stamp.nanosec) > (stamp.sec, stamp.nanosec)); } } } @@ -418,8 +444,8 @@ impl Worker { }; let decoded = Instant::now(); if !undistort.as_ref().is_some_and(|(held, held_width, _)| { - held.K == info.K - && held.D == info.D + held.k == info.k + && held.d == info.d && held.distortion_model == info.distortion_model && held.width == info.width && *held_width == width @@ -560,18 +586,19 @@ impl Worker { scan_arrived.await; continue; }; - let stamp = seconds(&msg.header.stamp); + let query_stamp = msg.header.stamp.clone(); let Some(world_from_lidar) = self .tf .lookup(&cfg.world_frame, &msg.header.frame_id) - .at(stamp) - .tolerance(cfg.tf_tolerance_s) + .at(query_stamp) .within(Duration::from_secs_f64(cfg.max_tf_lag_s)) .await else { continue; }; - let pose = isometry(&world_from_lidar); + let Ok(pose) = dimos_module::tf::isometry(&world_from_lidar) else { + continue; + }; let points = match extract_xyz(&msg) { Ok(points) => points, Err(error) => { @@ -587,6 +614,7 @@ impl Worker { [p.x as f32, p.y as f32, p.z as f32] }) .collect(); + let stamp = seconds(&msg.header.stamp); let mut history = self.shared.history.lock().unwrap(); history.push_back(Scan { stamp, points }); let newest = history.iter().map(|scan| scan.stamp).fold(stamp, f64::max); @@ -604,7 +632,10 @@ impl Worker { &info.header.frame_id, &image.header.frame_id, ); - let poses = self.poses(frame_id, seconds(&image.header.stamp))?; + let poses = self.poses( + frame_id, + image.header.stamp.clone(), + )?; Some((i, (frame_id.to_string(), poses))) })?; let image = frames.drain(..=index).next_back()?; @@ -616,15 +647,10 @@ impl Worker { }) } - fn poses(&self, frame_id: &str, stamp: f64) -> Option { + fn poses(&self, frame_id: &str, stamp: Time) -> Option { let pose = |target: &str| { - let transform = self - .tf - .lookup(target, frame_id) - .at(stamp) - .tolerance(self.config.tf_tolerance_s) - .get()?; - Some(isometry(&transform)) + let transform = self.tf.lookup(target, frame_id).at(stamp.clone()).get()?; + dimos_module::tf::isometry(&transform).ok() }; let optional = |target: &str| match target { "" => Some(None), @@ -712,10 +738,10 @@ fn decode_rgb(image: &CompressedImage, scale: usize) -> Option<(Vec, usize, } fn make_cloud(points: &[[f32; 3]], source: &Header, frame_id: String) -> PointCloud2 { - let field = |name: &str, offset: i32| PointField { + let field = |name: &str, offset: u32| PointField { name: name.into(), offset, - datatype: PointField::FLOAT32 as u8, + datatype: PointField::FLOAT32, count: 1, }; let data: Vec = points @@ -725,25 +751,20 @@ fn make_cloud(points: &[[f32; 3]], source: &Header, frame_id: String) -> PointCl .collect(); PointCloud2 { header: Header { - seq: source.seq, stamp: source.stamp.clone(), frame_id, }, height: 1, - width: points.len() as i32, + width: points.len() as u32, fields: vec![field("x", 0), field("y", 4), field("z", 8)], is_bigendian: false, point_step: 12, - row_step: 12 * points.len() as i32, + row_step: 12 * points.len() as u32, data, is_dense: true, } } -fn isometry(transform: &dimos_module::tf::Transform) -> Isometry3 { - Isometry3::from_parts(transform.translation().into(), transform.rotation()) -} - /// A configured size, or the derived one when it is 0. fn or_derived(configured: usize, derived: usize) -> usize { if configured > 0 { @@ -754,7 +775,7 @@ fn or_derived(configured: usize, derived: usize) -> usize { } fn seconds(stamp: &Time) -> f64 { - stamp.sec as f64 + stamp.nsec as f64 * 1e-9 + stamp.sec as f64 + stamp.nanosec as f64 * 1e-9 } /// Config, then calibration, then image frame, since drivers stamp frames nobody publishes a transform for. @@ -824,6 +845,31 @@ impl Timing { mod tests { use super::*; + #[test] + fn generated_cloud_roundtrips_cdr_with_exact_negative_and_epoch_stamps() { + for stamp in [ + Time { + sec: -1, + nanosec: 999_999_999, + }, + Time { + sec: 1_700_000_000, + nanosec: 123_456_789, + }, + ] { + let header = Header { + stamp: stamp.clone(), + frame_id: "camera".into(), + }; + let cloud = make_cloud(&[[1., 2., 3.]], &header, "optical".into()); + let received: PointCloud2 = cdr::decode(&cdr::encode(&cloud).unwrap()).unwrap(); + assert_eq!(received.header.stamp.sec, stamp.sec); + assert_eq!(received.header.stamp.nanosec, stamp.nanosec); + assert_eq!(received.header.frame_id, "optical"); + assert_eq!(extract_xyz(&received).unwrap(), vec![[1., 2., 3.]]); + } + } + #[test] fn a_stage_behind_takes_the_newest_item_and_counts_the_ones_it_missed() { let latest = Latest::default(); @@ -845,8 +891,12 @@ mod tests { #[test] fn a_non_jpeg_format_is_refused_rather_than_fed_to_the_decoder() { let png = CompressedImage { + header: Header { + stamp: Time { sec: 0, nanosec: 0 }, + frame_id: String::new(), + }, format: "png".into(), - ..Default::default() + data: Vec::new(), }; assert!(decode_rgb(&png, 1).is_none()); } @@ -854,8 +904,7 @@ mod tests { #[test] fn the_cloud_carries_the_points_and_the_source_stamp() { let header = Header { - seq: 7, - stamp: Time { sec: 3, nsec: 4 }, + stamp: Time { sec: 3, nanosec: 4 }, frame_id: "image".into(), }; let cloud = make_cloud( @@ -867,7 +916,7 @@ mod tests { ( cloud.width, cloud.header.frame_id.as_str(), - cloud.header.stamp.nsec + cloud.header.stamp.nanosec ), (2, "camera", 4) ); diff --git a/dimos/perception/depth2depth_cloud/rust/src/undistort.rs b/dimos/perception/depth2depth_cloud/rust/src/undistort.rs index 41751feff60..23f1132e9ab 100644 --- a/dimos/perception/depth2depth_cloud/rust/src/undistort.rs +++ b/dimos/perception/depth2depth_cloud/rust/src/undistort.rs @@ -16,7 +16,7 @@ //! projection both assume straight lines stay straight. use depth2depth::Pinhole; -use lcm_msgs::sensor_msgs::CameraInfo; +use dimos_generated_messages::sensor_msgs::msg::camera_info::CameraInfo; use rayon::prelude::*; /// Pinhole + distortion, as it arrives in a `CameraInfo`. @@ -56,21 +56,21 @@ impl Lens { /// The lens as seen in an image `scale` times the calibration's resolution; None without usable intrinsics /// or for a distortion model this cannot undistort. pub fn from_info(info: &CameraInfo, scale: f64) -> Option { - let (fx, fy) = (info.K[0], info.K[4]); + let (fx, fy) = (info.k[0], info.k[4]); if !(fx > 0.0 && fy > 0.0 && fx.is_finite() && fy.is_finite()) { return None; } let model = Model::from_name(&info.distortion_model)?; // Eight coefficients mean the rational model whatever `distortion_model` claims; some drivers mislabel it plumb_bob. let mut distortion = [0.0; 8]; - for (slot, value) in distortion.iter_mut().zip(&info.D) { + for (slot, value) in distortion.iter_mut().zip(&info.d) { *slot = *value; } Some(Self { fx: fx * scale, fy: fy * scale, - cx: (info.K[2] + 0.5) * scale - 0.5, - cy: (info.K[5] + 0.5) * scale - 0.5, + cx: (info.k[2] + 0.5) * scale - 0.5, + cy: (info.k[5] + 0.5) * scale - 0.5, model, distortion, }) @@ -185,9 +185,27 @@ mod tests { CameraInfo { width: 1920, height: 1536, - K: [1000.0, 0.0, 959.5, 0.0, 1000.0, 767.5, 0.0, 0.0, 1.0], - D: coefficients.to_vec(), - ..Default::default() + k: [1000.0, 0.0, 959.5, 0.0, 1000.0, 767.5, 0.0, 0.0, 1.0], + d: coefficients.to_vec(), + header: dimos_generated_messages::std_msgs::msg::header::Header { + stamp: dimos_generated_messages::builtin_interfaces::msg::time::Time { + sec: 0, + nanosec: 0, + }, + frame_id: String::new(), + }, + distortion_model: "plumb_bob".into(), + r: [0.; 9], + p: [0.; 12], + binning_x: 0, + binning_y: 0, + roi: dimos_generated_messages::sensor_msgs::msg::region_of_interest::RegionOfInterest { + x_offset: 0, + y_offset: 0, + width: 0, + height: 0, + do_rectify: false, + }, } } @@ -227,7 +245,7 @@ mod tests { let mut head = info(&[ -0.6792, -0.638, 0.0002, -0.0002, -0.0302, -0.2868, -0.9987, -0.1752, ]); - head.K = [1012.59, 0.0, 962.21, 0.0, 1012.13, 765.67, 0.0, 0.0, 1.0]; + head.k = [1012.59, 0.0, 962.21, 0.0, 1012.13, 765.67, 0.0, 0.0, 1.0]; let lens = Lens::from_info(&head, 1.0).unwrap(); for ((x, y), (u, v)) in [ ((-0.9467, -0.7570), (300.77, 237.5)), @@ -246,7 +264,7 @@ mod tests { // The Go2 front camera (front_camera_720.yaml); OpenCV's fisheye::distortPoints puts these rays here. let mut go2 = info(&[-0.0730943, -0.0234114, -0.0069306, 0.0092387]); go2.distortion_model = "equidistant".into(); - go2.K = [ + go2.k = [ 797.4756, 0.0, 643.5352, 0.0, 796.4872, 349.2784, 0.0, 0.0, 1.0, ]; let lens = Lens::from_info(&go2, 1.0).unwrap(); @@ -274,7 +292,7 @@ mod tests { #[test] fn zero_focal_length_is_refused() { let mut bad = info(&[0.0; 5]); - bad.K[0] = 0.0; + bad.k[0] = 0.0; assert!(Lens::from_info(&bad, 1.0).is_none()); } diff --git a/dimos/perception/depth2depth_cloud/test_depth2depth_cloud_config.py b/dimos/perception/depth2depth_cloud/test_depth2depth_cloud_config.py index d04478f913b..47ba6aa1ad9 100644 --- a/dimos/perception/depth2depth_cloud/test_depth2depth_cloud_config.py +++ b/dimos/perception/depth2depth_cloud/test_depth2depth_cloud_config.py @@ -16,15 +16,16 @@ from __future__ import annotations +from pathlib import Path import re from pydantic import ValidationError import pytest -from dimos.constants import DIMOS_PROJECT_ROOT +from dimos.perception.depth2depth_cloud import module from dimos.perception.depth2depth_cloud.module import Depth2DepthCloudConfig -RUST_MODULE = DIMOS_PROJECT_ROOT / "dimos/perception/depth2depth_cloud/rust/src/module.rs" +RUST_MODULE = Path(__file__).with_name("rust") / "src/module.rs" def test_every_field_the_rust_config_requires_is_sent(): @@ -59,8 +60,6 @@ def test_a_jpeg_scale_the_decoder_cannot_do_is_refused(): def test_an_nvidia_gpu_builds_tensorrt_and_anything_else_the_cpu_build(monkeypatch): - from dimos.perception.depth2depth_cloud import module - monkeypatch.setattr(module, "has_nvidia_gpu", lambda: False) assert Depth2DepthCloudConfig().build_command == "nix build -L path:." monkeypatch.setattr(module, "has_nvidia_gpu", lambda: True) diff --git a/dimos/perception/detection/conftest.py b/dimos/perception/detection/conftest.py index a34bcf914f8..13437cdccba 100644 --- a/dimos/perception/detection/conftest.py +++ b/dimos/perception/detection/conftest.py @@ -13,19 +13,29 @@ # limitations under the License. from collections.abc import Callable, Generator +from contextlib import ExitStack +from copy import deepcopy import functools +import hashlib +from importlib import resources +import json +import math +from pathlib import Path from typing import TypedDict -from unittest import mock -from dimos_lcm.visualization_msgs.MarkerArray import MarkerArray +from dimos_cdrgen.registry import decode as cdr_decode import pytest from dimos.core.transport import LCMTransport -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray +from dimos.message_utils.camera_info import camera_info_from_yaml +from dimos.message_utils.geometry import transform_from_pose +from dimos.message_utils.time import to_nanoseconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import PoseStamped, TransformStamped +from dimos.messages.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos.messages.std_msgs.msg import Header +from dimos.messages.vision_msgs.msg import Detection2DArray +from dimos.perception.detection.detectors.yolo import Yolo2DDetector from dimos.perception.detection.module2D import Detection2DModule from dimos.perception.detection.module3D import Detection3DModule from dimos.perception.detection.type.detection2d.base import Detection2D @@ -33,22 +43,44 @@ from dimos.perception.detection.type.detection3d.imageDetections3DPC import ImageDetections3DPC from dimos.perception.detection.type.detection3d.pointcloud import Detection3DPC from dimos.perception.experimental.moduleDB import ObjectDBModule +from dimos.protocol.tf.static_tf_publisher import frames_to_edge_transforms from dimos.protocol.tf.tf import TF -from dimos.robot.unitree.go2 import connection -from dimos.robot.unitree.type.odometry import Odometry from dimos.utils.data import get_data -from dimos.utils.testing.legacy_pickle import LegacyPickleStore + + +def _test_data(name): + local = Path(__file__).resolve().parents[3] / "data" / name + return local if local.exists() else get_data(name) + + +@pytest.fixture(scope="session") +def detection_assets(): + return _test_data + + +def front_camera_calibration() -> CameraInfo: + source = resources.files("dimos.robot.unitree.go2").joinpath("front_camera_720.yaml") + with resources.as_file(source) as path: + return camera_info_from_yaml( + path, header=Header(frame_id="camera_optical", stamp=Time(sec=0, nanosec=0)) + ) + + +def _dispose_module(module): + try: + module.detector.stop() + finally: + module._close_module() class Moment(TypedDict, total=False): - odom_frame: Odometry + odom_frame: PoseStamped lidar_frame: PointCloud2 image_frame: Image camera_info: CameraInfo - transforms: list[Transform] + transforms: list[TransformStamped] tf: TF detections: ImageDetections3DPC | None - markers: MarkerArray | None class Moment2D(Moment): @@ -70,34 +102,48 @@ def tf(): def get_moment(tf): @functools.lru_cache(maxsize=1) def moment_provider(**kwargs) -> Moment: - print("MOMENT PROVIDER ARGS:", kwargs) seek = kwargs.get("seek", 10.0) - - data_dir = "unitree_go2_lidar_corrected" - get_data(data_dir) - - lidar_frame_result = LegacyPickleStore(f"{data_dir}/lidar").find_closest_seek(seek) - if lidar_frame_result is None: - raise ValueError("No lidar frame found") - lidar_frame: PointCloud2 = lidar_frame_result - - image_frame = LegacyPickleStore( - f"{data_dir}/video", - ).find_closest(lidar_frame.ts) - - if image_frame is None: - raise ValueError("No image frame found") - - image_frame.frame_id = "camera_optical" - - odom_frame = LegacyPickleStore(f"{data_dir}/odom", autocast=Odometry.from_msg).find_closest( - lidar_frame.ts + data_dir = _test_data("unitree_go2_detection_cdr") + manifest = json.loads((data_dir / "manifest.json").read_text()) + entry = next(item for item in manifest["moments"] if item["seek"] == seek) + + def payload(name): + stream = entry["streams"][name] + data = (data_dir / stream["file"]).read_bytes() + assert hashlib.sha256(data).hexdigest() == stream["cdr_sha256"] + value = cdr_decode( + data, {"lidar": PointCloud2, "video": Image, "odom": PoseStamped}[name] + ) + assert ( + to_nanoseconds(value.header.stamp) + == stream["source_sec"] * 1_000_000_000 + stream["source_nanosec"] + ) + return data + + lidar_frame = cdr_decode(payload("lidar"), PointCloud2) + image_frame = cdr_decode(payload("video"), Image) + image_frame.header.frame_id = "camera_optical" + odom_frame = cdr_decode(payload("odom"), PoseStamped) + # Match the Go2 connection's configured global parent for this recording. + odom_frame.header.frame_id = "world" + + transforms = [transform_from_pose(odom_frame, child_frame_id="base_link")] + # Recorded Go2 mounting geometry; no robot connection is constructed. + transforms.extend( + frames_to_edge_transforms( + [ + ("camera_link", "base_link", (0.3, 0, 0), (0, 0, 0)), + ( + "camera_optical", + "camera_link", + (0, 0, 0), + (-math.pi / 2, 0, -math.pi / 2), + ), + ] + ) ) - - if odom_frame is None: - raise ValueError("No odom frame found") - - transforms = connection.GO2Connection._odom_to_tf(odom_frame) + for transform in transforms: + transform.header.stamp = deepcopy(odom_frame.header.stamp) tf.receive_transform(*transforms) @@ -105,7 +151,7 @@ def moment_provider(**kwargs) -> Moment: "odom_frame": odom_frame, "lidar_frame": lidar_frame, "image_frame": image_frame, - "camera_info": connection._camera_info_static(), + "camera_info": front_camera_calibration(), "transforms": transforms, "tf": tf, } @@ -117,40 +163,28 @@ def moment_provider(**kwargs) -> Moment: @pytest.fixture(scope="session") def publish_moment(): def publisher(moment: Moment | Moment2D | Moment3D) -> None: - detections2d_val = moment.get("detections2d") - if detections2d_val: - detections: LCMTransport[Detection2DArray] = LCMTransport( - "/detections", Detection2DArray - ) - assert isinstance(detections2d_val, ImageDetections2D) - detections.publish(detections2d_val.to_ros_detection2d_array()) - detections.lcm.stop() - - lidar_frame = moment.get("lidar_frame") - if lidar_frame: - lidar: LCMTransport[PointCloud2] = LCMTransport("/lidar", PointCloud2) - lidar.publish(lidar_frame) - lidar.lcm.stop() - - image_frame = moment.get("image_frame") - if image_frame: - image: LCMTransport[Image] = LCMTransport("/image", Image) - image.publish(image_frame) - image.lcm.stop() - - camera_info_val = moment.get("camera_info") - if camera_info_val: - camera_info: LCMTransport[CameraInfo] = LCMTransport("/camera_info", CameraInfo) - camera_info.publish(camera_info_val) - camera_info.lcm.stop() - - tf = moment.get("tf") - transforms = moment.get("transforms") - if tf is not None and transforms is not None: - tf.publish(*transforms) - - # moduleDB.scene_update.transport = LCMTransport("/scene_update", SceneUpdate) - # moduleDB.target.transport = LCMTransport("/target", PoseStamped) + with ExitStack() as cleanup: + + def publish(topic, msg_type, value): + transport = LCMTransport(topic, msg_type) + cleanup.callback(transport.stop) + transport.publish(value) + + detections = moment.get("detections2d") + if detections is not None: + publish("/detections", Detection2DArray, detections.to_ros_detection2d_array()) + for key, topic, msg_type in [ + ("lidar_frame", "/lidar", PointCloud2), + ("image_frame", "/image", Image), + ("camera_info", "/camera_info", CameraInfo), + ]: + value = moment.get(key) + if value is not None: + publish(topic, msg_type, value) + buffer = moment.get("tf") + transforms = moment.get("transforms") + if buffer is not None and transforms is not None: + buffer.publish(*transforms) return publisher @@ -182,11 +216,13 @@ def detection3dpc(detections3dpc) -> Detection3DPC: @pytest.fixture(scope="session") -def get_moment_2d(get_moment) -> Generator[Callable[[], Moment2D], None, None]: - from dimos.perception.detection.detectors.yolo import Yolo2DDetector - - c = mock.create_autospec(CameraInfo, spec_set=True, instance=True) - module = Detection2DModule(detector=lambda: Yolo2DDetector(device="cpu"), camera_info=c) +def get_moment_2d(get_moment, request) -> Generator[Callable[[], Moment2D], None, None]: + c = front_camera_calibration() + module = Detection2DModule( + detector=lambda: Yolo2DDetector(model_path=str(_test_data("models_yolo")), device="cpu"), + camera_info=c, + ) + request.addfinalizer(lambda: _dispose_module(module)) @functools.lru_cache(maxsize=1) def moment_provider(**kwargs) -> Moment2D: @@ -201,11 +237,10 @@ def moment_provider(**kwargs) -> Moment2D: yield moment_provider moment_provider.cache_clear() - module._close_module() @pytest.fixture(scope="session") -def get_moment_3dpc(get_moment_2d) -> Generator[Callable[[], Moment3D], None, None]: +def get_moment_3dpc(get_moment_2d, request) -> Generator[Callable[[], Moment3D], None, None]: module: Detection3DModule | None = None @functools.lru_cache(maxsize=1) @@ -214,13 +249,19 @@ def moment_provider(**kwargs) -> Moment3D: moment = get_moment_2d(**kwargs) if not module: - module = Detection3DModule(camera_info=moment["camera_info"]) + module = Detection3DModule( + camera_info=moment["camera_info"], + detector=lambda: Yolo2DDetector( + model_path=str(_test_data("models_yolo")), device="cpu" + ), + ) + request.addfinalizer(lambda: _dispose_module(module)) lidar_frame = moment.get("lidar_frame") if lidar_frame is None: raise ValueError("No lidar frame found") - camera_transform = moment["tf"].get("camera_optical", lidar_frame.frame_id) + camera_transform = moment["tf"].get("camera_optical", lidar_frame.header.frame_id) if camera_transform is None: raise ValueError("No camera_optical transform in tf") @@ -235,19 +276,27 @@ def moment_provider(**kwargs) -> Moment3D: yield moment_provider moment_provider.cache_clear() - if module is not None: - module._close_module() @pytest.fixture(scope="session") -def object_db_module(get_moment): +def object_db_module(get_moment, request): """Create and populate an ObjectDBModule with detections from multiple frames.""" - from dimos.perception.detection.detectors.yolo import Yolo2DDetector - - c = mock.create_autospec(CameraInfo, spec_set=True, instance=True) - module2d = Detection2DModule(detector=lambda: Yolo2DDetector(device="cpu"), camera_info=c) - module3d = Detection3DModule(camera_info=connection._camera_info_static()) - moduleDB = ObjectDBModule(camera_info=connection._camera_info_static()) + c = front_camera_calibration() + module2d = Detection2DModule( + detector=lambda: Yolo2DDetector(model_path=str(_test_data("models_yolo")), device="cpu"), + camera_info=c, + ) + request.addfinalizer(lambda: _dispose_module(module2d)) + module3d = Detection3DModule( + camera_info=front_camera_calibration(), + detector=lambda: Yolo2DDetector(model_path=str(_test_data("models_yolo")), device="cpu"), + ) + request.addfinalizer(lambda: _dispose_module(module3d)) + moduleDB = ObjectDBModule( + camera_info=front_camera_calibration(), + detector=lambda: Yolo2DDetector(model_path=str(_test_data("models_yolo")), device="cpu"), + ) + request.addfinalizer(lambda: _dispose_module(moduleDB)) # Process 5 frames to build up object history for i in range(5): @@ -258,7 +307,7 @@ def object_db_module(get_moment): imageDetections2d = module2d.process_image_frame(moment["image_frame"]) # Get camera transform - camera_transform = moment["tf"].get("camera_optical", moment.get("lidar_frame").frame_id) + camera_transform = moment["tf"].get("camera_optical", moment["lidar_frame"].header.frame_id) # Process 3D detections imageDetections3d = module3d.process_frame( @@ -270,10 +319,6 @@ def object_db_module(get_moment): yield moduleDB - module2d._close_module() - module3d._close_module() - moduleDB._close_module() - @pytest.fixture(scope="session") def first_object(object_db_module): diff --git a/dimos/perception/detection/detectors/base.py b/dimos/perception/detection/detectors/base.py index 40aa82e5bdd..835b0d272bb 100644 --- a/dimos/perception/detection/detectors/base.py +++ b/dimos/perception/detection/detectors/base.py @@ -14,7 +14,7 @@ from abc import ABC, abstractmethod -from dimos.msgs.sensor_msgs.Image import Image +from dimos.messages.sensor_msgs.msg import Image from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D diff --git a/dimos/perception/detection/detectors/grounding_dino.py b/dimos/perception/detection/detectors/grounding_dino.py index f025acf470d..78c48e89375 100644 --- a/dimos/perception/detection/detectors/grounding_dino.py +++ b/dimos/perception/detection/detectors/grounding_dino.py @@ -22,8 +22,10 @@ from PIL import Image as PILImage import torch +from dimos.message_utils.image import image_to_rgb +from dimos.message_utils.time import to_seconds +from dimos.messages.sensor_msgs.msg import Image from dimos.models.base import HuggingFaceModel, HuggingFaceModelConfig -from dimos.msgs.sensor_msgs.Image import Image from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D @@ -83,7 +85,7 @@ def query_detections( class_id=label, confidence=float(row[label]), name=queries[label], - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, ) if det.is_valid(): @@ -107,7 +109,7 @@ def query_score_rows( from torchvision.ops.boxes import batched_nms # type: ignore[import-untyped] from transformers.image_transforms import center_to_corners_format - pil = PILImage.fromarray(image.to_rgb().data) + pil = PILImage.fromarray(image_to_rgb(image)) phrases = [" ".join(query.replace(".", " ").lower().split()) for query in queries] caption = " ".join(f"{phrase}." for phrase in phrases) with torch.inference_mode(): diff --git a/dimos/perception/detection/detectors/omdet.py b/dimos/perception/detection/detectors/omdet.py index aeee0f7433a..d5befe460d1 100644 --- a/dimos/perception/detection/detectors/omdet.py +++ b/dimos/perception/detection/detectors/omdet.py @@ -22,8 +22,10 @@ from PIL import Image as PILImage import torch +from dimos.message_utils.image import image_to_rgb +from dimos.message_utils.time import to_seconds +from dimos.messages.sensor_msgs.msg import Image from dimos.models.base import HuggingFaceModel, HuggingFaceModelConfig -from dimos.msgs.sensor_msgs.Image import Image from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D @@ -79,7 +81,7 @@ def query_detections( ``confidence`` is the per-box score. ``class_id`` indexes into ``queries``. """ - pil = PILImage.fromarray(image.to_rgb().data) + pil = PILImage.fromarray(image_to_rgb(image)) with torch.inference_mode(): inputs = self._processor(images=pil, text=queries, return_tensors="pt").to( self.config.device @@ -105,7 +107,7 @@ def query_detections( class_id=int(label), confidence=float(score), name=queries[int(label)], - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, ) if det.is_valid(): @@ -135,7 +137,7 @@ def query_score_rows( from torchvision.ops.boxes import batched_nms # type: ignore[import-untyped] from transformers.image_transforms import center_to_corners_format - pil = PILImage.fromarray(image.to_rgb().data) + pil = PILImage.fromarray(image_to_rgb(image)) with torch.inference_mode(): inputs = self._processor( images=pil, text=queries, task="Detect all objects.", return_tensors="pt" diff --git a/dimos/perception/detection/detectors/owlv2.py b/dimos/perception/detection/detectors/owlv2.py index b1cd5490803..3907665ea16 100644 --- a/dimos/perception/detection/detectors/owlv2.py +++ b/dimos/perception/detection/detectors/owlv2.py @@ -23,8 +23,10 @@ from PIL import Image as PILImage import torch +from dimos.message_utils.image import image_to_rgb +from dimos.message_utils.time import to_nanoseconds, to_seconds +from dimos.messages.sensor_msgs.msg import Image from dimos.models.base import HuggingFaceModel, HuggingFaceModelConfig -from dimos.msgs.sensor_msgs.Image import Image from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D @@ -56,7 +58,7 @@ def __init__(self, **kwargs: object) -> None: # (frame id, ts) to text-independent tower outputs; any query set scores # against a cached frame without rerunning the tower self._features: OrderedDict[ - tuple[str, float], tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor] + tuple[str, int], tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor] ] = OrderedDict() self._text_embeds: dict[str, torch.Tensor] = {} @@ -111,7 +113,7 @@ def query_detections_batch( results are per-image, in input order. """ self.forwards += len(images) - pils = [PILImage.fromarray(image.to_rgb().data) for image in images] + pils = [PILImage.fromarray(image_to_rgb(image)) for image in images] with torch.inference_mode(), self._autocast(): inputs = self._processor( text=[queries] * len(pils), images=pils, return_tensors="pt" @@ -138,7 +140,7 @@ def query_detections_batch( class_id=int(label), confidence=float(score), name=queries[int(label)], - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, ) if det.is_valid(): @@ -172,7 +174,7 @@ def _frame_features( logit shift and scale, and clipped pixel ``(x1, y1, x2, y2)`` boxes. A miss runs the image tower once; a hit runs nothing. """ - key = (image.frame_id, image.ts) + key = (image.header.frame_id, to_nanoseconds(image.header.stamp)) if key in self._features: self._features.move_to_end(key) return self._features[key] @@ -180,7 +182,7 @@ def _frame_features( from transformers.image_transforms import center_to_corners_format self.forwards += 1 - pil = PILImage.fromarray(image.to_rgb().data) + pil = PILImage.fromarray(image_to_rgb(image)) model = self._model head = model.class_head with torch.inference_mode(), self._autocast(): diff --git a/dimos/perception/detection/detectors/person/yolo.py b/dimos/perception/detection/detectors/person/yolo.py index 635a3d119b7..475067ac91b 100644 --- a/dimos/perception/detection/detectors/person/yolo.py +++ b/dimos/perception/detection/detectors/person/yolo.py @@ -12,10 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos.msgs.sensor_msgs.Image import Image +from dimos.message_utils.image import image_to_bgr +from dimos.messages.sensor_msgs.msg import Image from dimos.perception.detection.detectors.base import Detector from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D -from dimos.utils.data import get_data +from dimos.utils.data import resolve_named_path from dimos.utils.gpu_utils import is_cuda_available from dimos.utils.logging_config import setup_logger @@ -32,9 +33,9 @@ def __init__( # ~0.5s: deferred to keep module import (test collection) light. from ultralytics import YOLO # type: ignore[attr-defined] - self.model = YOLO(get_data(model_path) / model_name, task="track") + self.model = YOLO(resolve_named_path(model_path) / model_name, task="track") - self.tracker = get_data(model_path) / "botsort.yaml" + self.tracker = resolve_named_path(model_path) / "botsort.yaml" if device: self.device = device @@ -57,7 +58,7 @@ def process_image(self, image: Image) -> ImageDetections2D: ImageDetections2D containing Detection2DPerson objects with pose keypoints """ results = self.model.track( - source=image.to_opencv(), + source=image_to_bgr(image), verbose=False, conf=0.5, tracker=self.tracker, diff --git a/dimos/perception/detection/detectors/test_generated_images.py b/dimos/perception/detection/detectors/test_generated_images.py new file mode 100644 index 00000000000..865150f17ab --- /dev/null +++ b/dimos/perception/detection/detectors/test_generated_images.py @@ -0,0 +1,93 @@ +# 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 copy import deepcopy +from unittest.mock import PropertyMock + +from dimos_cdrgen.registry import decode, encode +import numpy as np +import pytest +import torch +from transformers.feature_extraction_utils import BatchFeature + +from dimos.message_utils.image import image_from_array, image_view +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.std_msgs.msg import Header +from dimos.messages.vision_msgs.msg import Detection2DArray +from dimos.perception.detection.detectors.omdet import OmDetDetector +from dimos.perception.detection.detectors.owlv2 import Owlv2Detector + + +@pytest.fixture +def source_image(): + return image_from_array( + np.full((4, 8, 3), [0, 0, 255], dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera", stamp=Time(sec=1700000000, nanosec=123456789)), + ) + + +@pytest.mark.parametrize("detector_type", [OmDetDetector, Owlv2Detector]) +def test_model_boundary_receives_rgb_and_exports_exact_source_header( + mocker, source_image, detector_type +): + processor, network = mocker.Mock(), mocker.Mock() + processor.return_value = BatchFeature(data={"pixel_values": torch.zeros(1, 3, 4, 8)}) + processor.post_process_grounded_object_detection.return_value = [ + { + "boxes": torch.tensor([[1, 1, 5, 3]]), + "scores": torch.tensor([0.9]), + "labels": torch.tensor([0]), + } + ] + mocker.patch.object( + detector_type, "_processor", new_callable=PropertyMock, return_value=processor + ) + mocker.patch.object(detector_type, "_model", new_callable=PropertyMock, return_value=network) + detector = detector_type(device="cpu") + detections = detector.query_detections(source_image, ["target"]) + assert len(detections) == 1 + assert detections[0].bbox == (1, 1, 5, 3) + assert detections[0].name == "target" + processed = processor.call_args.kwargs["images"] + pil = processed[0] if isinstance(processed, list) else processed + assert pil.getpixel((0, 0)) == (255, 0, 0) + message = decode(encode(detections.to_ros_detection2d_array()), Detection2DArray) + assert message.header == source_image.header + assert message.detections[0].header == source_image.header + np.testing.assert_array_equal(image_view(source_image)[0, 0], [0, 0, 255]) + + +def test_owl_feature_cache_distinguishes_adjacent_source_nanoseconds(mocker, source_image): + processor, network = mocker.Mock(), mocker.Mock() + processor.return_value = BatchFeature(data={"pixel_values": torch.zeros(1, 3, 4, 8)}) + network.image_embedder.return_value = (torch.ones(1, 1, 1, 2),) + network.box_predictor.return_value = torch.tensor([[[0.5, 0.5, 0.5, 0.5]]]) + network.class_head.dense0.return_value = torch.ones(1, 1, 2) + network.class_head.logit_shift.return_value = torch.zeros(1, 1, 1) + network.class_head.logit_scale.return_value = torch.zeros(1, 1, 1) + network.class_head.elu.return_value = torch.zeros(1, 1, 1) + mocker.patch.object( + Owlv2Detector, "_processor", new_callable=PropertyMock, return_value=processor + ) + mocker.patch.object(Owlv2Detector, "_model", new_callable=PropertyMock, return_value=network) + detector = Owlv2Detector(device="cpu") + second = deepcopy(source_image) + second.header.stamp.nanosec += 1 + first = detector._frame_features(source_image) + assert detector._frame_features(source_image) is first + detector._frame_features(second) + assert network.image_embedder.call_count == 2 + assert len(detector._features) == 2 + np.testing.assert_array_equal(first[3].numpy(), [[2, 2, 6, 4]]) diff --git a/dimos/perception/detection/detectors/yolo.py b/dimos/perception/detection/detectors/yolo.py index 20dd756ed4d..bf682da8f52 100644 --- a/dimos/perception/detection/detectors/yolo.py +++ b/dimos/perception/detection/detectors/yolo.py @@ -12,10 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos.msgs.sensor_msgs.Image import Image +from dimos.message_utils.image import image_to_bgr +from dimos.messages.sensor_msgs.msg import Image from dimos.perception.detection.detectors.base import Detector from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D -from dimos.utils.data import get_data +from dimos.utils.data import resolve_named_path from dimos.utils.gpu_utils import is_cuda_available from dimos.utils.logging_config import setup_logger @@ -33,7 +34,7 @@ def __init__( from ultralytics import YOLO # type: ignore[attr-defined] self.model = YOLO( - get_data(model_path) / model_name, + resolve_named_path(model_path) / model_name, task="detect", ) @@ -59,7 +60,7 @@ def process_image(self, image: Image) -> ImageDetections2D: ImageDetections2D containing all detected objects """ results = self.model.track( - source=image.to_opencv(), + source=image_to_bgr(image), device=self.device, conf=0.5, iou=0.6, diff --git a/dimos/perception/detection/detectors/yoloe.py b/dimos/perception/detection/detectors/yoloe.py index 4311f6783f7..952bf050b97 100644 --- a/dimos/perception/detection/detectors/yoloe.py +++ b/dimos/perception/detection/detectors/yoloe.py @@ -20,7 +20,8 @@ from numpy.typing import NDArray import torch -from dimos.msgs.sensor_msgs.Image import Image +from dimos.message_utils.image import image_to_bgr +from dimos.messages.sensor_msgs.msg import Image from dimos.perception.detection.detectors.base import Detector from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.utils.data import get_data @@ -144,7 +145,7 @@ def predict_image(self, image: Image) -> "ImageDetections2D[Any]": def _run(self, image: Image, *, track: bool) -> "ImageDetections2D[Any]": """Run tracking or prediction and apply common filtering.""" kwargs = { - "source": image.to_opencv(), + "source": image_to_bgr(image), "device": self.device, "conf": self.conf, "iou": 0.6, diff --git a/dimos/perception/detection/module2D.py b/dimos/perception/detection/module2D.py index 34e3e675915..5d1e11c1eba 100644 --- a/dimos/perception/detection/module2D.py +++ b/dimos/perception/detection/module2D.py @@ -14,6 +14,7 @@ from collections.abc import Callable, Sequence from typing import Annotated, Any +from pydantic import InstanceOf from pydantic.experimental.pipeline import validate_as from reactivex import operators as ops from reactivex.observable import Observable @@ -22,25 +23,26 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import IO, In, Out -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.msgs.sensor_msgs.Image import Image, sharpness_barrier -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray +from dimos.message_utils.image import image_sharpness +from dimos.message_utils.time import to_seconds +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.sensor_msgs.msg import CameraInfo, Image +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.messages.vision_msgs.msg import Detection2DArray from dimos.perception.detection.detectors.base import Detector from dimos.perception.detection.detectors.yolo import Yolo2DDetector from dimos.perception.detection.type.detection2d.base import Filter2D from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.utils.decorators.decorators import simple_mcache -from dimos.utils.reactive import backpressure +from dimos.utils.reactive import backpressure, quality_barrier class Config(ModuleConfig): max_freq: float = 10 detector: Callable[[Any], Detector] | None = Yolo2DDetector publish_detection_images: bool = True - camera_info: CameraInfo + camera_info: InstanceOf[CameraInfo] filter: Annotated[ Sequence[Filter2D], validate_as(Sequence[Filter2D] | Filter2D).transform( @@ -80,7 +82,7 @@ def process_image_frame(self, image: Image) -> ImageDetections2D: def sharp_image_stream(self) -> Observable[Image]: return backpressure( self.color_image.pure_observable().pipe( - sharpness_barrier(self.config.max_freq), + quality_barrier(image_sharpness, self.config.max_freq), ) ) @@ -89,7 +91,9 @@ def detection_stream_2d(self) -> Observable[ImageDetections2D]: return backpressure(self.sharp_image_stream().pipe(ops.map(self.process_image_frame))) def track(self, detections: ImageDetections2D) -> None: - sensor_frame = self.tfbuffer.get("sensor", "camera_optical", detections.image.ts, 5.0) + sensor_frame = self.tfbuffer.get( + "sensor", "camera_optical", to_seconds(detections.image.header.stamp), 5.0 + ) if not sensor_frame: return @@ -110,19 +114,21 @@ def track(self, detections: ImageDetections2D) -> None: detection = detections.detections[index] position_3d = self.pixel_to_3d( # type: ignore[attr-defined] detection.center_bbox, - self.config.camera_info, assumed_depth=1.0, ) else: # No detection at this index - publish zero transform - position_3d = Vector3(0.0, 0.0, 0.0) + position_3d = Vector3(x=0.0, y=0.0, z=0.0) transforms.append( - Transform( - frame_id=sensor_frame.child_frame_id, + TransformStamped( + header=Header( + frame_id=sensor_frame.child_frame_id, stamp=detections.image.header.stamp + ), child_frame_id=f"det_{index}", - ts=detections.image.ts, - translation=position_3d, + transform=Transform( + translation=position_3d, rotation=Quaternion(w=1, x=0.0, y=0.0, z=0.0) + ), ) ) diff --git a/dimos/perception/detection/module3D.py b/dimos/perception/detection/module3D.py index 861dc14e873..0e662ed6087 100644 --- a/dimos/perception/detection/module3D.py +++ b/dimos/perception/detection/module3D.py @@ -19,21 +19,30 @@ from dimos.agents.annotation import skill from dimos.core.core import rpc from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -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.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray +from dimos.message_utils.time import to_seconds +from dimos.messages.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + TransformStamped, + Vector3, +) +from dimos.messages.sensor_msgs.msg import Image, PointCloud2 +from dimos.messages.std_msgs.msg import Header +from dimos.messages.vision_msgs.msg import Detection2DArray from dimos.perception.detection.module2D import Detection2DModule from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.detection.type.detection3d.imageDetections3DPC import ImageDetections3DPC from dimos.perception.detection.type.detection3d.pointcloud import Detection3DPC -from dimos.types.timestamped import align_timestamped +from dimos.types.timestamped import TimestampedData, align_timestamped from dimos.utils.reactive import backpressure +def _timed_cloud(cloud: PointCloud2) -> TimestampedData[PointCloud2]: + return TimestampedData(cloud, to_seconds(cloud.header.stamp)) + + class Detection3DModule(Detection2DModule): color_image: In[Image] pointcloud: In[PointCloud2] @@ -56,7 +65,7 @@ def process_frame( self, detections: ImageDetections2D, pointcloud: PointCloud2, - transform: Transform | None, + transform: TransformStamped | None, ) -> ImageDetections3DPC: if not transform: return ImageDetections3DPC(detections.image, []) @@ -89,8 +98,8 @@ def pixel_to_3d( Vector3 position in camera optical frame coordinates """ # Extract camera intrinsics - fx, fy = self.config.camera_info.K[0], self.config.camera_info.K[4] - cx, cy = self.config.camera_info.K[2], self.config.camera_info.K[5] + fx, fy = self.config.camera_info.k[0], self.config.camera_info.k[4] + cx, cy = self.config.camera_info.k[2], self.config.camera_info.k[5] # Unproject pixel to normalized camera coordinates x_norm = (pixel[0] - cx) / fx @@ -98,7 +107,7 @@ def pixel_to_3d( # Create 3D point at assumed depth in camera optical frame # Camera optical frame: X right, Y down, Z forward - return Vector3(x_norm * assumed_depth, y_norm * assumed_depth, assumed_depth) + return Vector3(x=x_norm * assumed_depth, y=y_norm * assumed_depth, z=assumed_depth) @skill def ask_vlm(self, question: str) -> str: @@ -113,7 +122,7 @@ def ask_vlm(self, question: str) -> str: # @skill @rpc - def nav_vlm(self, question: str) -> str: + def nav_vlm(self, question: str) -> PoseStamped | None: """ query visual model about the view in front of the camera you can ask to mark objects like: @@ -131,17 +140,19 @@ def nav_vlm(self, question: str) -> str: print("VLM result:", result, "for", image, "and question", question) if isinstance(result, str) or not result or not len(result): - return None # type: ignore[return-value] + return None detections: ImageDetections2D = result print(detections) if not len(detections): print("No 2d detections") - return None # type: ignore[return-value] + return None pc = self.pointcloud.get_next() - transform = self.tfbuffer.get("camera_optical", pc.frame_id, detections.image.ts, 5.0) + transform = self.tfbuffer.get( + "camera_optical", pc.header.frame_id, to_seconds(detections.image.header.stamp), 5.0 + ) detections3d = self.process_frame(detections, pc, transform) @@ -150,11 +161,13 @@ def nav_vlm(self, question: str) -> str: print("No 3d detections, projecting 2d") center = detections[0].get_bbox_center() + position = self.pixel_to_3d(center, assumed_depth=1.5) return PoseStamped( - ts=detections.image.ts, - frame_id="world", - position=self.pixel_to_3d(center, assumed_depth=1.5), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + header=Header(stamp=detections.image.header.stamp, frame_id="world"), + pose=Pose( + position=Point(x=position.x, y=position.y, z=position.z), + orientation=Quaternion(w=1, x=0.0, y=0.0, z=0.0), + ), ) @rpc @@ -162,13 +175,16 @@ def start(self) -> None: super().start() def detection2d_to_3d(args): # type: ignore[no-untyped-def] - detections, pc = args - transform = self.tfbuffer.get("camera_optical", pc.frame_id, detections.image.ts, 5.0) + detections, timed_pc = args + pc = timed_pc.value + transform = self.tfbuffer.get( + "camera_optical", pc.header.frame_id, to_seconds(detections.image.header.stamp), 5.0 + ) return self.process_frame(detections, pc, transform) self.detection_stream_3d = align_timestamped( # type: ignore[type-var] backpressure(self.detection_stream_2d()), - self.pointcloud.observable(), + self.pointcloud.observable().pipe(ops.map(_timed_cloud)), match_tolerance=0.25, buffer_size=20.0, ).pipe(ops.map(detection2d_to_3d)) diff --git a/dimos/perception/detection/person_tracker.py b/dimos/perception/detection/person_tracker.py index 1fb28dd29e7..5a6a35be049 100644 --- a/dimos/perception/detection/person_tracker.py +++ b/dimos/perception/detection/person_tracker.py @@ -13,6 +13,7 @@ # limitations under the License. +from copy import deepcopy from typing import Any from reactivex import operators as ops @@ -21,18 +22,52 @@ from dimos.core.core import rpc from dimos.core.module import Module from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -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.msgs.sensor_msgs.Image import Image -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray +from dimos.message_utils.geometry import ( + compose_transforms, + pose_from_transform, + transform_from_pose, +) +from dimos.message_utils.time import to_nanoseconds +from dimos.messages.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Vector3 +from dimos.messages.sensor_msgs.msg import CameraInfo, Image +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.messages.vision_msgs.msg import Detection2DArray from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D -from dimos.types.timestamped import align_timestamped +from dimos.types.timestamped import TimestampedData, align_timestamped from dimos.utils.reactive import backpressure +def _timed_image(message: Image) -> TimestampedData[Image]: + return TimestampedData(message, to_nanoseconds(message.header.stamp)) + + +def _timed_detections(message: Detection2DArray) -> TimestampedData[Detection2DArray]: + return TimestampedData(message, to_nanoseconds(message.header.stamp)) + + +def _has_detections(message: Detection2DArray) -> bool: + return len(message.detections) > 0 + + +def _paired_detections( + pair: tuple[TimestampedData[Image], TimestampedData[Detection2DArray]], +) -> ImageDetections2D: + return ImageDetections2D.from_ros_detection2d_array(pair[0].value, pair[1].value) + + +def _align_images_and_detections( + images: Observable[Image], detections: Observable[Detection2DArray] +) -> Observable[ImageDetections2D]: + # Match source Headers exactly; retain the existing two-second window. + return align_timestamped( + images.pipe(ops.map(_timed_image)), + detections.pipe(ops.filter(_has_detections), ops.map(_timed_detections)), + match_tolerance=0, + buffer_size=2_000_000_000, + ).pipe(ops.map(_paired_detections)) + + class PersonTracker(Module): detections: In[Detection2DArray] color_image: In[Image] @@ -61,8 +96,8 @@ def center_to_3d( Vector3 position in camera_link frame coordinates (Z up, X forward) """ # Extract camera intrinsics - fx, fy = camera_info.K[0], camera_info.K[4] - cx, cy = camera_info.K[2], camera_info.K[5] + fx, fy = camera_info.k[0], camera_info.k[4] + cx, cy = camera_info.k[2], camera_info.k[5] # Unproject pixel to normalized camera coordinates x_norm = (pixel[0] - cx) / fx @@ -78,21 +113,12 @@ def center_to_3d( # Optical: X right, Y down, Z forward # Link: X forward, Y left, Z up # Transformation: x_link = z_optical, y_link = -x_optical, z_link = -y_optical - return Vector3(z_optical, -x_optical, -y_optical) + return Vector3(x=z_optical, y=-x_optical, z=-y_optical) def detections_stream(self) -> Observable[ImageDetections2D]: return backpressure( - align_timestamped( - self.color_image.pure_observable(), - self.detections.pure_observable().pipe( - ops.filter(lambda d: d.detections_length > 0) # type: ignore[attr-defined] - ), - match_tolerance=0.0, - buffer_size=2.0, - ).pipe( - ops.map( - lambda pair: ImageDetections2D.from_ros_detection2d_array(*pair) # type: ignore[arg-type, misc] - ) + _align_images_and_detections( + self.color_image.pure_observable(), self.detections.pure_observable() ) ) @@ -112,17 +138,20 @@ def track(self, detections2D: ImageDetections2D) -> None: vector = self.center_to_3d(target.center_bbox, self.camera_info, 2.0) pose_in_camera = PoseStamped( - ts=detections2D.ts, - position=vector, - frame_id="camera_link", + header=Header(frame_id="camera_link", stamp=detections2D.image.header.stamp), + pose=Pose( + position=Point(x=vector.x, y=vector.y, z=vector.z), + orientation=Quaternion(w=1, x=0.0, y=0.0, z=0.0), + ), ) tf_world_to_camera = self.tfbuffer.get("world", "camera_link", detections2D.ts, 5.0) if not tf_world_to_camera: return - tf_camera_to_target = Transform.from_pose("target", pose_in_camera) - tf_world_to_target = tf_world_to_camera + tf_camera_to_target - pose_in_world = tf_world_to_target.to_pose(ts=detections2D.ts) + tf_camera_to_target = transform_from_pose(pose_in_camera, child_frame_id="target") + tf_world_to_target = compose_transforms(tf_world_to_camera, tf_camera_to_target) + pose_in_world = pose_from_transform(tf_world_to_target) + pose_in_world.header.stamp = deepcopy(detections2D.image.header.stamp) self.target.publish(pose_in_world) diff --git a/dimos/perception/detection/project.py b/dimos/perception/detection/project.py index 1f9a8561ab1..01d6432a385 100644 --- a/dimos/perception/detection/project.py +++ b/dimos/perception/detection/project.py @@ -21,17 +21,22 @@ import numpy as np from dimos.memory.transform import Transformer -from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.message_utils.geometry import ( + compose_transforms, + inverse_transform, + transform_matrix, +) +from dimos.message_utils.image import image_view +from dimos.message_utils.time import time_from_seconds +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.std_msgs.msg import Header from dimos.perception.detection.type.detection3d.imageDetections3DPC import ImageDetections3DPC if TYPE_CHECKING: from collections.abc import Callable, Iterator - from dimos_lcm.sensor_msgs import CameraInfo - from dimos.memory.type.observation import Observation - from dimos.msgs.sensor_msgs.Image import Image - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + from dimos.messages.sensor_msgs.msg import CameraInfo, Image, PointCloud2 from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.detection.type.detection3d.pointcloud_filters import PointCloudFilter from dimos.protocol.tf.tf import TFLookup @@ -41,17 +46,26 @@ def _world_to_optical( obs: Observation[Any], world_frame: str, tf: TFLookup | None, - base_to_optical: Transform | None, + base_to_optical: TransformStamped | None, optical_frame: str, time_tolerance: float, -) -> Transform | None: +) -> TransformStamped | None: """Resolve the world→optical transform for an observation.""" if tf is not None: return tf.get(optical_frame, world_frame, obs.ts, time_tolerance) - pose = obs.pose_stamped + pose = obs.pose_tuple if base_to_optical is None or pose is None: return None - return -(Transform.from_pose("base_link", pose) + base_to_optical) + x, y, z, qx, qy, qz, qw = pose + world_to_base = TransformStamped( + header=Header(frame_id=world_frame, stamp=time_from_seconds(obs.ts)), + child_frame_id="base_link", + transform=Transform( + translation=Vector3(x=x, y=y, z=z), + rotation=Quaternion(x=qx, y=qy, z=qz, w=qw), + ), + ) + return inverse_transform(compose_transforms(world_to_base, base_to_optical)) class ProjectDepthTo3D(Transformer["ImageDetections2D", ImageDetections3DPC]): @@ -67,7 +81,7 @@ def __init__( depth: Callable[[Observation[ImageDetections2D]], Image | None], camera_info: CameraInfo, tf: TFLookup | None = None, - base_to_optical: Transform | None = None, + base_to_optical: TransformStamped | None = None, world_frame: str = "world", optical_frame: str = "camera_optical", time_tolerance: float = 5.0, @@ -116,7 +130,7 @@ def sees( point: Any, camera_info: CameraInfo, tf: TFLookup | None = None, - base_to_optical: Transform | None = None, + base_to_optical: TransformStamped | None = None, world_frame: str = "world", optical_frame: str = "camera_optical", time_tolerance: float = 5.0, @@ -151,8 +165,8 @@ def sees( raise ValueError("sees needs either tf or base_to_optical") target = np.array([point.x, point.y, point.z]) if hasattr(point, "x") else np.asarray(point) - fx, fy = camera_info.K[0], camera_info.K[4] - cx, cy = camera_info.K[2], camera_info.K[5] + fx, fy = camera_info.k[0], camera_info.k[4] + cx, cy = camera_info.k[2], camera_info.k[5] width, height = float(camera_info.width), float(camera_info.height) if extent is not None: @@ -174,7 +188,7 @@ def _sees(obs: Observation[Any]) -> bool: ) if transform is None: return False - matrix = transform.to_matrix() + matrix = transform_matrix(transform.transform) pts = (matrix @ np.column_stack([samples, np.ones(len(samples))]).T).T[:, :3] in_front = pts[:, 2] > 0 if not in_front.any(): @@ -192,8 +206,9 @@ def _sees(obs: Observation[Any]) -> bool: if depth is not None and visible.any(): depth_frame = depth(obs) if depth_frame is not None: - depth_m = np.asarray(depth_frame.data, dtype=np.float32) - if depth_frame.data.dtype == np.uint16: + pixels = image_view(depth_frame) + depth_m = np.asarray(pixels, dtype=np.float32) + if pixels.dtype.kind == "u" and pixels.dtype.itemsize == 2: depth_m = depth_m * 0.001 rows = np.clip(v[visible].astype(int), 0, depth_m.shape[0] - 1) cols = np.clip(u[visible].astype(int), 0, depth_m.shape[1] - 1) @@ -225,7 +240,7 @@ def __init__( pointcloud: PointCloud2 | Callable[[Observation[ImageDetections2D]], PointCloud2 | None], camera_info: CameraInfo, tf: TFLookup | None = None, - base_to_optical: Transform | None = None, + base_to_optical: TransformStamped | None = None, optical_frame: str = "camera_optical", time_tolerance: float = 5.0, filters: list[PointCloudFilter] | None = None, @@ -249,7 +264,7 @@ def __call__( continue transform = _world_to_optical( obs, - pointcloud.frame_id, + pointcloud.header.frame_id, self.tf, self.base_to_optical, self.optical_frame, diff --git a/dimos/perception/detection/reid/module.py b/dimos/perception/detection/reid/module.py index c5420a42e23..eba13fd58ee 100644 --- a/dimos/perception/detection/reid/module.py +++ b/dimos/perception/detection/reid/module.py @@ -18,15 +18,46 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray +from dimos.message_utils.time import to_nanoseconds +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.vision_msgs.msg import Detection2DArray from dimos.perception.detection.reid.embedding_id_system import EmbeddingIDSystem from dimos.perception.detection.reid.type import IDSystem from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D -from dimos.types.timestamped import align_timestamped +from dimos.types.timestamped import TimestampedData, align_timestamped from dimos.utils.reactive import backpressure +def _timed_image(image: Image) -> TimestampedData[Image]: + return TimestampedData(image, to_nanoseconds(image.header.stamp)) + + +def _timed_detections(detections: Detection2DArray) -> TimestampedData[Detection2DArray]: + return TimestampedData(detections, to_nanoseconds(detections.header.stamp)) + + +def _has_detections(detections: Detection2DArray) -> bool: + return bool(detections.detections) + + +def _image_detections( + pair: tuple[TimestampedData[Image], TimestampedData[Detection2DArray]], +) -> ImageDetections2D: + return ImageDetections2D.from_ros_detection2d_array(pair[0].value, pair[1].value) + + +def _align_images_and_detections( + images: Observable[Image], detections: Observable[Detection2DArray] +) -> Observable[ImageDetections2D]: + # Match source Headers exactly; retain the existing two-second window. + return align_timestamped( + images.pipe(ops.map(_timed_image)), + detections.pipe(ops.filter(_has_detections), ops.map(_timed_detections)), + match_tolerance=0, + buffer_size=2_000_000_000, + ).pipe(ops.map(_image_detections)) + + class Config(ModuleConfig): idsystem: IDSystem @@ -37,7 +68,6 @@ class ReidModule(Module): image: In[Image] def __init__(self, idsystem: IDSystem | None = None, **kwargs) -> None: # type: ignore[no-untyped-def] - super().__init__(**kwargs) if idsystem is None: try: from dimos.models.embedding.treid import TorchReIDModel @@ -45,21 +75,17 @@ def __init__(self, idsystem: IDSystem | None = None, **kwargs) -> None: # type: idsystem = EmbeddingIDSystem(model=TorchReIDModel, padding=0) except Exception as e: raise RuntimeError( - "TorchReIDModel not available. Please install with: pip install dimos[torchreid]" + "TorchReIDModel not available. Please install with: pip install dimos[misc]" ) from e + super().__init__(idsystem=idsystem, **kwargs) self.idsystem = idsystem def detections_stream(self) -> Observable[ImageDetections2D]: return backpressure( - align_timestamped( - self.image.pure_observable(), - self.detections.pure_observable().pipe( - ops.filter(lambda d: d.detections_length > 0) # type: ignore[attr-defined] - ), - match_tolerance=0.0, - buffer_size=2.0, - ).pipe(ops.map(lambda pair: ImageDetections2D.from_ros_detection2d_array(*pair))) # type: ignore[arg-type, misc] + _align_images_and_detections( + self.image.pure_observable(), self.detections.pure_observable() + ) ) @rpc diff --git a/dimos/perception/detection/reid/test_generated_module.py b/dimos/perception/detection/reid/test_generated_module.py new file mode 100644 index 00000000000..6e2e438f208 --- /dev/null +++ b/dimos/perception/detection/reid/test_generated_module.py @@ -0,0 +1,141 @@ +# Copyright 2025-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 contextlib import ExitStack +from copy import deepcopy +from threading import Event + +from dimos_cdrgen.registry import decode, encode +import numpy as np +import pytest +from reactivex.subject import Subject + +from dimos.core.transport import LCMTransport +from dimos.message_utils.camera_info import camera_info_from_intrinsics +from dimos.message_utils.image import image_from_array +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import PoseStamped +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.messages.vision_msgs.msg import Detection2DArray +from dimos.perception.detection.person_tracker import ( + PersonTracker, + _align_images_and_detections as align_person, +) +from dimos.perception.detection.reid.module import ( + ReidModule, + _align_images_and_detections as align_reid, +) +from dimos.perception.detection.reid.type import PassthroughIDSystem +from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox +from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D +from dimos.protocol.tf.static_tf_publisher import frames_to_edge_transforms + + +@pytest.mark.parametrize("align", [align_person, align_reid], ids=["person", "reid"]) +@pytest.mark.parametrize("seconds", [0, 1700000000]) +def test_exact_header_alignment_rejects_adjacent_nanosecond_and_empty_arrays(align, seconds): + image = image_from_array( + np.zeros((32, 32, 3), dtype=np.uint8), + encoding="rgb8", + header=Header(frame_id="camera", stamp=Time(sec=seconds, nanosec=123456789)), + ) + second = deepcopy(image) + second.header.stamp.nanosec += 1 + box = Detection2DBBox( + image=second, + bbox=(12, 12, 20, 20), + track_id=7, + class_id=0, + confidence=0.9, + name="person", + ts=0.0, + ) + packet = decode( + encode(ImageDetections2D(second, [box]).to_ros_detection2d_array()), Detection2DArray + ) + received = [] + with ExitStack() as cleanup: + images, detections = Subject(), Subject() + cleanup.callback(images.dispose) + cleanup.callback(detections.dispose) + subscription = align(images, detections).subscribe(received.append) + cleanup.callback(subscription.dispose) + detections.on_next(Detection2DArray(header=second.header, detections=[])) + images.on_next(second) + assert received == [] + detections.on_next(packet) + assert len(received) == 1 + assert received[0].image is second + assert received[0][0].track_id == 7 + assert received[0][0].bbox == (12, 12, 20, 20) + images.on_next(image) + assert len(received) == 1 + + +def test_explicit_identity_system_constructs_normal_module_and_generated_ports(): + system = PassthroughIDSystem() + module = ReidModule(idsystem=system) + try: + assert module.config.idsystem is system + assert module.image.type is Image + assert module.detections.type is Detection2DArray + finally: + module.stop() + + +def test_person_target_composition_keeps_detection_source_stamp(): + header = Header(frame_id="camera_optical", stamp=Time(sec=1700000000, nanosec=123456789)) + image = image_from_array(np.zeros((32, 32, 3), dtype=np.uint8), encoding="rgb8", header=header) + box = Detection2DBBox( + image=image, + bbox=(12, 12, 20, 20), + track_id=7, + class_id=0, + confidence=1.0, + name="person", + ts=1700000000.123456789, + ) + camera = camera_info_from_intrinsics(10, 10, 16, 16, 32, 32, header=header) + with ExitStack() as cleanup: + tf_wire = LCMTransport("tracking_tf", TFMessage, url="memq://") + cleanup.callback(tf_wire.stop) + target_wire = LCMTransport("tracking_target", PoseStamped, url="memq://") + cleanup.callback(target_wire.stop) + module = PersonTracker(cameraInfo=camera) + cleanup.callback(module.stop) + module.tf.transport = tf_wire + module.target.transport = target_wire + buffer = module.tfbuffer + cleanup.callback(buffer.dispose) + mount = frames_to_edge_transforms([("camera_link", "world", (1, 2, 3), (0, 0, 0))])[0] + mount.header.stamp = Time(sec=1699999999, nanosec=123456789) + buffer.receive_transform(mount) + received = [] + ready = Event() + + def receive(value): + received.append(value) + ready.set() + + cleanup.callback(target_wire.subscribe(receive)) + module.track(ImageDetections2D(image, [box])) + assert ready.wait(2) + assert len(received) == 1 + target = received[0] + assert target.header.frame_id == "world" + assert target.header.stamp == image.header.stamp + position = target.pose.position + assert (position.x, position.y, position.z) == (3, 2, 3) diff --git a/dimos/perception/detection/type/detection2d/base.py b/dimos/perception/detection/type/detection2d/base.py index 7ee1d5bf1ad..19961f9d8e3 100644 --- a/dimos/perception/detection/type/detection2d/base.py +++ b/dimos/perception/detection/type/detection2d/base.py @@ -15,9 +15,8 @@ from abc import abstractmethod from collections.abc import Callable -from dimos_lcm.vision_msgs import Detection2D as ROSDetection2D - -from dimos.msgs.sensor_msgs.Image import Image +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.vision_msgs.msg import Detection2D as ROSDetection2D from dimos.types.timestamped import Timestamped diff --git a/dimos/perception/detection/type/detection2d/bbox.py b/dimos/perception/detection/type/detection2d/bbox.py index 8facd3ac776..3c1ee8ea006 100644 --- a/dimos/perception/detection/type/detection2d/bbox.py +++ b/dimos/perception/detection/type/detection2d/bbox.py @@ -18,13 +18,22 @@ import hashlib from typing import TYPE_CHECKING, Any +from dimos.messages.geometry_msgs.msg import Point, Pose, PoseWithCovariance, Quaternion + if TYPE_CHECKING: from typing_extensions import Self from ultralytics.engine.results import Results - from dimos.msgs.sensor_msgs.Image import Image + from dimos.messages.sensor_msgs.msg import Image + +import cv2 +import numpy as np +from rich.console import Console +from rich.text import Text -from dimos_lcm.vision_msgs import ( +from dimos.message_utils.image import image_from_array, image_to_bgr, image_view +from dimos.message_utils.time import to_seconds +from dimos.messages.vision_msgs.msg import ( BoundingBox2D, Detection2D as ROSDetection2D, ObjectHypothesis, @@ -32,12 +41,7 @@ Point2D, Pose2D, ) -from rich.console import Console -from rich.text import Text - -from dimos.msgs.std_msgs.Header import Header from dimos.perception.detection.type.detection2d.base import Detection2D -from dimos.types.timestamped import to_timestamp from dimos.utils.decorators.decorators import simple_mcache Bbox = tuple[float, float, float, float] @@ -93,9 +97,6 @@ def to_repr_dict(self) -> dict[str, Any]: def draw_on(self, img: Any, scale: float = 1.0) -> None: """Draw this detection's bbox and label onto a BGR numpy array (in-place).""" - import cv2 - import numpy as np - x1, y1, x2, y2 = map(int, self.bbox) h = hashlib.md5(self.name.encode()).digest()[0] @@ -127,11 +128,9 @@ def draw_on(self, img: Any, scale: float = 1.0) -> None: def annotated_image(self, scale: float = 1.0) -> Image: """Return the full image with this detection's bbox and label drawn on it.""" - img = self.image.to_opencv().copy() + img = image_to_bgr(self.image) self.draw_on(img, scale=scale) - from dimos.msgs.sensor_msgs.Image import Image - - return Image.from_opencv(img, ts=self.ts) + return image_from_array(img, encoding="bgr8", header=self.image.header) # return focused image, only on the bbox def cropped_image(self, padding: int = 20) -> Image: @@ -144,9 +143,12 @@ def cropped_image(self, padding: int = 20) -> Image: Cropped Image containing only the detection area plus padding """ x1, y1, x2, y2 = map(int, self.bbox) - return self.image.crop( - x1 - padding, y1 - padding, x2 - x1 + 2 * padding, y2 - y1 + 2 * padding - ) + pixels = image_view(self.image) + cropped = pixels[ + max(0, y1 - padding) : min(self.image.height, y2 + padding), + max(0, x1 - padding) : min(self.image.width, x2 + padding), + ] + return image_from_array(cropped, encoding=self.image.encoding, header=self.image.header) def __str__(self) -> str: console = Console(force_terminal=True, legacy_windows=False) @@ -158,7 +160,7 @@ def __str__(self) -> str: ] # Add any extra fields (e.g., points for Detection3D) - extra_keys = [k for k in d.keys() if k not in ["class"]] + extra_keys = [k for k in d if k not in ["class"]] for key in extra_keys: if d[key] == "None": parts.append(Text(f"{key}={d[key]}", style="dim")) @@ -202,8 +204,8 @@ def is_valid(self) -> bool: return False # Check if within image bounds (if image has shape) - if self.image.shape: - h, w = self.image.shape[:2] + if self.image.height and self.image.width: + h, w = self.image.height, self.image.width if not (0 <= x1 <= w and 0 <= y1 <= h and 0 <= x2 <= w and 0 <= y2 <= h): return False @@ -255,7 +257,7 @@ def from_ultralytics_result(cls, result: Results, idx: int, image: Image) -> Det class_id=class_id, confidence=confidence, name=name, - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, ) @@ -299,14 +301,14 @@ def from_ros_detection2d(cls, ros_det: ROSDetection2D, **kwargs) -> Self: # typ confidence = 0.0 if ros_det.results: hypothesis = ros_det.results[0].hypothesis - class_id = hypothesis.class_id + class_id = int(hypothesis.class_id) confidence = hypothesis.score # Extract track_id track_id = int(ros_det.id) if ros_det.id.isdigit() else 0 # Extract timestamp - ts = to_timestamp(ros_det.header.stamp) + ts = to_seconds(ros_det.header.stamp) name = kwargs.pop("name", f"class_{class_id}") @@ -322,14 +324,21 @@ def from_ros_detection2d(cls, ros_det: ROSDetection2D, **kwargs) -> Self: # typ def to_ros_detection2d(self) -> ROSDetection2D: return ROSDetection2D( - header=Header(self.ts, "camera_link"), + header=self.image.header, bbox=self.to_ros_bbox(), results=[ ObjectHypothesisWithPose( - ObjectHypothesis( - class_id=self.class_id, + hypothesis=ObjectHypothesis( + class_id=str(self.class_id), score=self.confidence, - ) + ), + pose=PoseWithCovariance( + pose=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + covariance=np.zeros(36, dtype=np.float64), + ), ) ], id=str(self.track_id), diff --git a/dimos/perception/detection/type/detection2d/imageDetections2D.py b/dimos/perception/detection/type/detection2d/imageDetections2D.py index 507125c333f..e0ab7a447b2 100644 --- a/dimos/perception/detection/type/detection2d/imageDetections2D.py +++ b/dimos/perception/detection/type/detection2d/imageDetections2D.py @@ -27,8 +27,8 @@ if TYPE_CHECKING: from ultralytics.engine.results import Results - from dimos.msgs.sensor_msgs.Image import Image - from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray + from dimos.messages.sensor_msgs.msg import Image + from dimos.messages.vision_msgs.msg import Detection2DArray T2D = TypeVar("T2D", bound=Detection2D, default=Detection2DBBox) diff --git a/dimos/perception/detection/type/detection2d/person.py b/dimos/perception/detection/type/detection2d/person.py index 30100756e1e..415d765471f 100644 --- a/dimos/perception/detection/type/detection2d/person.py +++ b/dimos/perception/detection/type/detection2d/person.py @@ -19,7 +19,8 @@ import numpy as np -from dimos.msgs.sensor_msgs.Image import Image +from dimos.message_utils.time import to_seconds +from dimos.messages.sensor_msgs.msg import Image from dimos.perception.detection.type.detection2d.bbox import Bbox, Detection2DBBox from dimos.utils.decorators.decorators import simple_mcache @@ -152,7 +153,7 @@ def from_ultralytics_result( class_id=class_id, confidence=confidence, name=name, - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, # Person specific fields keypoints=keypoints, diff --git a/dimos/perception/detection/type/detection2d/point.py b/dimos/perception/detection/type/detection2d/point.py index 4e6d14ed27e..4e77730f25a 100644 --- a/dimos/perception/detection/type/detection2d/point.py +++ b/dimos/perception/detection/type/detection2d/point.py @@ -17,7 +17,11 @@ from dataclasses import dataclass from typing import TYPE_CHECKING -from dimos_lcm.vision_msgs import ( +import numpy as np + +from dimos.message_utils.image import image_from_array, image_view +from dimos.messages.geometry_msgs.msg import Point, Pose, PoseWithCovariance, Quaternion +from dimos.messages.vision_msgs.msg import ( BoundingBox2D, Detection2D as ROSDetection2D, ObjectHypothesis, @@ -25,12 +29,10 @@ Point2D, Pose2D, ) - -from dimos.msgs.std_msgs.Header import Header from dimos.perception.detection.type.detection2d.base import Detection2D if TYPE_CHECKING: - from dimos.msgs.sensor_msgs.Image import Image + from dimos.messages.sensor_msgs.msg import Image @dataclass @@ -65,17 +67,17 @@ def cropped_image(self, padding: int = 20) -> Image: Cropped Image containing the area around the point """ x, y = int(self.x), int(self.y) - return self.image.crop( - x - padding, - y - padding, - 2 * padding, - 2 * padding, - ) + pixels = image_view(self.image) + cropped = pixels[ + max(0, y - padding) : min(self.image.height, y + padding), + max(0, x - padding) : min(self.image.width, x + padding), + ] + return image_from_array(cropped, encoding=self.image.encoding, header=self.image.header) def to_ros_detection2d(self) -> ROSDetection2D: """Convert point to ROS Detection2D message (as zero-size bbox at point).""" return ROSDetection2D( - header=Header(self.ts, "camera_link"), + header=self.image.header, bbox=BoundingBox2D( center=Pose2D( position=Point2D(x=self.x, y=self.y), @@ -86,10 +88,17 @@ def to_ros_detection2d(self) -> ROSDetection2D: ), results=[ ObjectHypothesisWithPose( - ObjectHypothesis( - class_id=self.class_id, + hypothesis=ObjectHypothesis( + class_id=str(self.class_id), score=self.confidence, - ) + ), + pose=PoseWithCovariance( + pose=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + covariance=np.zeros(36, dtype=np.float64), + ), ) ], id=str(self.track_id), @@ -97,7 +106,7 @@ def to_ros_detection2d(self) -> ROSDetection2D: def is_valid(self) -> bool: """Check if the point is within image bounds.""" - if self.image.shape: - h, w = self.image.shape[:2] + if self.image.height and self.image.width: + h, w = self.image.height, self.image.width return bool(0 <= self.x <= w and 0 <= self.y <= h) return True diff --git a/dimos/perception/detection/type/detection2d/seg.py b/dimos/perception/detection/type/detection2d/seg.py index d96f2a8e7a1..61a7d6ec022 100644 --- a/dimos/perception/detection/type/detection2d/seg.py +++ b/dimos/perception/detection/type/detection2d/seg.py @@ -19,13 +19,14 @@ import numpy as np +from dimos.message_utils.time import to_seconds from dimos.perception.detection.type.detection2d.bbox import Bbox, Detection2DBBox if TYPE_CHECKING: import torch from ultralytics.engine.results import Results - from dimos.msgs.sensor_msgs.Image import Image + from dimos.messages.sensor_msgs.msg import Image @dataclass @@ -90,7 +91,7 @@ def from_sam2_result( class_id=class_id, confidence=confidence, name=name, - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, mask=mask.astype(np.uint8), ) @@ -164,7 +165,7 @@ def from_ultralytics_result(cls, result: Results, idx: int, image: Image) -> Det class_id=class_id, confidence=confidence, name=name, - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, mask=mask, ) diff --git a/dimos/perception/detection/type/detection2d/test_bbox.py b/dimos/perception/detection/type/detection2d/test_bbox.py index cca4c8e59c2..4beda1117d8 100644 --- a/dimos/perception/detection/type/detection2d/test_bbox.py +++ b/dimos/perception/detection/type/detection2d/test_bbox.py @@ -13,6 +13,8 @@ # limitations under the License. import pytest +from dimos.message_utils.image import image_view + @pytest.mark.self_hosted def test_detection2d(detection2d) -> None: @@ -69,7 +71,7 @@ def test_detection2d(detection2d) -> None: # The actual cropped image is (260, 192, 3) assert cropped.width == 192 assert cropped.height == 260 - assert cropped.shape == (260, 192, 3) + assert image_view(cropped).shape == (260, 192, 3) # def test_to_ros_bbox(detection2d): """Test ROS bounding box conversion.""" diff --git a/dimos/perception/detection/type/detection2d/test_generated_bbox.py b/dimos/perception/detection/type/detection2d/test_generated_bbox.py new file mode 100644 index 00000000000..777cfc81fea --- /dev/null +++ b/dimos/perception/detection/type/detection2d/test_generated_bbox.py @@ -0,0 +1,102 @@ +# Copyright 2025-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 dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode +import numpy as np + +from dimos.message_utils.image import image_from_array, image_view +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.std_msgs.msg import Header +from dimos.messages.vision_msgs.msg import Detection2D, Detection2DArray +from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox +from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D +from dimos.perception.detection.type.detection2d.point import Detection2DPoint + + +def test_generated_bbox_image_and_wire_round_trip(): + header = Header(frame_id="camera_optical", stamp=Time(sec=1700000000, nanosec=123456789)) + pixels = np.zeros((40, 60, 3), dtype=np.uint8) + pixels[:, :, 0] = 250 + image = image_from_array(pixels, encoding="rgb8", header=header) + det = Detection2DBBox( + bbox=(10, 10, 30, 30), + track_id=7, + class_id=2, + confidence=0.9, + name="target", + ts=1.0, + image=image, + ) + assert det.is_valid() + crop = det.cropped_image(padding=2) + assert (crop.width, crop.height) == (24, 24) + assert crop.header == header + np.testing.assert_array_equal(image_view(crop), pixels[8:32, 8:32]) + annotated = det.annotated_image() + assert annotated.encoding == "bgr8" + assert annotated.header == header + np.testing.assert_array_equal(image_view(annotated)[39, 59], [0, 0, 250]) + np.testing.assert_array_equal(image_view(image), pixels) + wire = cdr_decode(cdr_encode(det.to_ros_detection2d()), Detection2D) + assert wire.header == header + assert wire.results[0].hypothesis.class_id == "2" + restored = Detection2DBBox.from_ros_detection2d(wire, image=image) + assert restored.bbox == det.bbox + assert restored.track_id == 7 + assert restored.class_id == 2 + + +def test_generated_detection_collection_round_trip(): + pixels = np.zeros((40, 60, 3), dtype=np.uint8) + image = image_from_array( + pixels, encoding="rgb8", header=Header(frame_id="camera", stamp=Time(nanosec=123, sec=0)) + ) + det = Detection2DBBox( + bbox=(10, 10, 30, 30), + track_id=7, + class_id=2, + confidence=0.9, + name="target", + ts=0, + image=image, + ) + collection = ImageDetections2D(image=image, detections=[det]) + wire = cdr_decode(cdr_encode(collection.to_ros_detection2d_array()), Detection2DArray) + assert wire.header == image.header + assert len(wire.detections) == 1 + restored = ImageDetections2D.from_ros_detection2d_array(image, wire) + assert len(restored) == 1 + assert restored[0].bbox == det.bbox + annotated = collection.annotated_image() + assert annotated.header == image.header + assert annotated.encoding == "bgr8" + assert np.any(image_view(annotated)) + assert not np.any(image_view(image)) + + +def test_generated_point_detection_crop_and_cdr(): + pixels = np.arange(100, dtype=np.uint8).reshape(10, 10) + image = image_from_array( + pixels, encoding="mono8", header=Header(frame_id="camera", stamp=Time(nanosec=789, sec=0)) + ) + point = Detection2DPoint(x=1, y=1, name="point", ts=0, image=image) + assert point.is_valid() + crop = point.cropped_image(padding=2) + np.testing.assert_array_equal(image_view(crop), pixels[:3, :3]) + assert crop.header == image.header + wire = cdr_decode(cdr_encode(point.to_ros_detection2d()), Detection2D) + assert wire.header == image.header + assert wire.bbox.center.position.x == wire.bbox.center.position.y == 1 + assert wire.bbox.size_x == wire.bbox.size_y == 0 + assert wire.results[0].hypothesis.class_id == "-1" diff --git a/dimos/perception/detection/type/detection2d/test_person.py b/dimos/perception/detection/type/detection2d/test_person.py index 3223380f4dc..201aff833c3 100644 --- a/dimos/perception/detection/type/detection2d/test_person.py +++ b/dimos/perception/detection/type/detection2d/test_person.py @@ -13,23 +13,25 @@ # limitations under the License. import pytest +from dimos.message_utils.image import image_from_file +from dimos.perception.detection.detectors.person.yolo import YoloPersonDetector +from dimos.perception.detection.type.detection2d.person import Detection2DPerson + @pytest.mark.self_hosted -def test_person_ros_confidence() -> None: +def test_person_ros_confidence(detection_assets) -> None: """Test that Detection2DPerson preserves confidence when converting to ROS format.""" - from dimos.msgs.sensor_msgs.Image import Image - from dimos.perception.detection.detectors.person.yolo import YoloPersonDetector - from dimos.perception.detection.type.detection2d.person import Detection2DPerson - from dimos.utils.data import get_data - # Load test image - image_path = get_data("cafe.jpg") - image = Image.from_file(image_path) + image_path = detection_assets("cafe.jpg") + image = image_from_file(image_path) # Run pose detection - detector = YoloPersonDetector(device="cpu") - detections = detector.process_image(image) + detector = YoloPersonDetector(model_path=str(detection_assets("models_yolo")), device="cpu") + try: + detections = detector.process_image(image) + finally: + detector.stop() # Find a Detection2DPerson (should have at least one person in cafe.jpg) person_detections = [d for d in detections.detections if isinstance(d, Detection2DPerson)] @@ -52,16 +54,9 @@ def test_person_ros_confidence() -> None: f"Confidence mismatch: {original_confidence} != {ros_confidence}" ) - print("\nSuccessfully preserved confidence in ROS conversion for Detection2DPerson:") - print(f" Original confidence: {original_confidence:.3f}") - print(f" ROS confidence: {ros_confidence:.3f}") - print(f" Track ID: {person_det.track_id}") - print(f" Visible keypoints: {len(person_det.get_visible_keypoints(threshold=0.3))}/17") - def test_person_from_ros_raises() -> None: """Test that Detection2DPerson.from_ros_detection2d() raises NotImplementedError.""" - from dimos.perception.detection.type.detection2d.person import Detection2DPerson with pytest.raises(NotImplementedError) as exc_info: Detection2DPerson.from_ros_detection2d() diff --git a/dimos/perception/detection/type/detection3d/base.py b/dimos/perception/detection/type/detection3d/base.py index afe37aac6e2..f1b20d28fb1 100644 --- a/dimos/perception/detection/type/detection3d/base.py +++ b/dimos/perception/detection/type/detection3d/base.py @@ -18,11 +18,13 @@ from dataclasses import dataclass, field from typing import TYPE_CHECKING -from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.std_msgs.msg import Header from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox if TYPE_CHECKING: - from dimos_lcm.sensor_msgs import CameraInfo + from dimos.messages.sensor_msgs.msg import CameraInfo @dataclass @@ -30,7 +32,16 @@ class Detection3D(Detection2DBBox): """Abstract base class for 3D detections.""" frame_id: str = "" - transform: Transform = field(default_factory=Transform.identity) + transform: TransformStamped = field( + default_factory=lambda: TransformStamped( + transform=Transform( + rotation=Quaternion(w=1, x=0.0, y=0.0, z=0.0), + translation=Vector3(x=0.0, y=0.0, z=0.0), + ), + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + child_frame_id="", + ) + ) @classmethod @abstractmethod @@ -39,7 +50,7 @@ def from_2d( det: Detection2DBBox, distance: float, camera_info: CameraInfo, - world_to_optical_transform: Transform, + world_to_optical_transform: TransformStamped, ) -> Detection3D | None: """Create a 3D detection from a 2D detection.""" ... diff --git a/dimos/perception/detection/type/detection3d/bbox.py b/dimos/perception/detection/type/detection3d/bbox.py index be3b9b2f96d..0632d606198 100644 --- a/dimos/perception/detection/type/detection3d/bbox.py +++ b/dimos/perception/detection/type/detection3d/bbox.py @@ -18,15 +18,26 @@ import functools from typing import Any -from dimos_lcm.vision_msgs import BoundingBox3D, ObjectHypothesis, ObjectHypothesisWithPose - -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -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.std_msgs.Header import Header -from dimos.msgs.vision_msgs.Detection3D import Detection3D +import numpy as np + +from dimos.message_utils.geometry import point_distance +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + PoseWithCovariance, + Quaternion, + TransformStamped, + Vector3, +) +from dimos.messages.std_msgs.msg import Header +from dimos.messages.vision_msgs.msg import ( + BoundingBox3D, + Detection3D, + ObjectHypothesis, + ObjectHypothesisWithPose, +) from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox @@ -39,9 +50,9 @@ class Detection3DBBox(Detection2DBBox): center: Vector3 # Center point in world frame size: Vector3 # Width, height, depth - transform: Transform | None = None # Camera to world transform + transform: TransformStamped | None = None # Camera to world transform frame_id: str = "" # Frame ID (e.g., "world", "map") - orientation: Quaternion = field(default_factory=lambda: Quaternion(0.0, 0.0, 0.0, 1.0)) + orientation: Quaternion = field(default_factory=lambda: Quaternion(w=1, x=0.0, y=0.0, z=0.0)) @functools.cached_property def pose(self) -> PoseStamped: @@ -50,16 +61,28 @@ def pose(self) -> PoseStamped: Returns pose in world frame with the detection's orientation. """ return PoseStamped( - ts=self.ts, - frame_id=self.frame_id, - position=self.center, - orientation=self.orientation, + header=Header(stamp=self.image.header.stamp, frame_id=self.frame_id), + pose=Pose( + position=Point(x=self.center.x, y=self.center.y, z=self.center.z), + orientation=self.orientation, + ), ) def to_detection3d_msg(self) -> Detection3D: """Convert to ROS Detection3D message.""" - msg = Detection3D() - msg.header = Header(self.ts, self.frame_id) + msg = Detection3D( + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + results=[], + bbox=BoundingBox3D( + center=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + size=Vector3(x=0.0, y=0.0, z=0.0), + ), + id="", + ) + msg.header = Header(stamp=self.image.header.stamp, frame_id=self.frame_id) # Results msg.results = [ @@ -67,16 +90,20 @@ def to_detection3d_msg(self) -> Detection3D: hypothesis=ObjectHypothesis( class_id=str(self.class_id), score=self.confidence, - ) + ), + pose=PoseWithCovariance( + pose=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + covariance=np.zeros(36, dtype=np.float64), + ), ) ] - msg.results_length = len(msg.results) - # The generated LCM constructor reuses a shared default bbox object. - # Assign a new BoundingBox3D so this message does not share bbox state with others. msg.bbox = BoundingBox3D( center=Pose( - position=self.center, + position=Point(x=self.center.x, y=self.center.y, z=self.center.z), orientation=self.orientation, ), size=self.size, @@ -88,8 +115,11 @@ def to_repr_dict(self) -> dict[str, Any]: # Calculate distance from camera if self.transform is None: return super().to_repr_dict() - camera_pos = self.transform.translation - distance = (self.center - camera_pos).magnitude() + camera_pos = self.transform.transform.translation + distance = point_distance( + Point(x=self.center.x, y=self.center.y, z=self.center.z), + Point(x=camera_pos.x, y=camera_pos.y, z=camera_pos.z), + ) parent_dict = super().to_repr_dict() # Remove bbox key if present diff --git a/dimos/perception/detection/type/detection3d/imageDetections3D.py b/dimos/perception/detection/type/detection3d/imageDetections3D.py index 93b33d15660..892b56189fa 100644 --- a/dimos/perception/detection/type/detection3d/imageDetections3D.py +++ b/dimos/perception/detection/type/detection3d/imageDetections3D.py @@ -18,8 +18,8 @@ from typing_extensions import TypeVar -from dimos.msgs.std_msgs.Header import Header -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.messages.std_msgs.msg import Header +from dimos.messages.vision_msgs.msg import Detection3DArray from dimos.perception.detection.type.detection3d.bbox import Detection3DBBox from dimos.perception.detection.type.imageDetections import ImageDetections @@ -36,7 +36,6 @@ def to_ros_detection3d_array(self, frame_id: str | None = None) -> Detection3DAr detections = [det.to_detection3d_msg() for det in self.detections] return Detection3DArray( - detections_length=len(detections), - header=Header(self.image.ts, resolved_frame_id), + header=Header(stamp=self.image.header.stamp, frame_id=resolved_frame_id), detections=detections, ) diff --git a/dimos/perception/detection/type/detection3d/imageDetections3DPC.py b/dimos/perception/detection/type/detection3d/imageDetections3DPC.py index 310b2651da0..2c7737bdb13 100644 --- a/dimos/perception/detection/type/detection3d/imageDetections3DPC.py +++ b/dimos/perception/detection/type/detection3d/imageDetections3DPC.py @@ -16,15 +16,16 @@ from typing import TYPE_CHECKING +import numpy as np + +from dimos.message_utils.pointcloud import select_points +from dimos.message_utils.time import to_seconds from dimos.perception.detection.type.detection3d.pointcloud import Detection3DPC, lattice_quantum from dimos.perception.detection.type.imageDetections import ImageDetections if TYPE_CHECKING: - from dimos_lcm.sensor_msgs import CameraInfo - - from dimos.msgs.geometry_msgs.Transform import Transform - from dimos.msgs.sensor_msgs.Image import Image - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + from dimos.messages.geometry_msgs.msg import TransformStamped + from dimos.messages.sensor_msgs.msg import CameraInfo, Image, PointCloud2 from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.detection.type.detection3d.pointcloud_filters import PointCloudFilter @@ -38,7 +39,7 @@ def from_2d( detections_2d: ImageDetections2D, world_pointcloud: PointCloud2, camera_info: CameraInfo, - world_to_optical_transform: Transform, + world_to_optical_transform: TransformStamped, filters: list[PointCloudFilter] | None = None, ) -> ImageDetections3DPC: """Project every 2D detection into 3D, dropping any that yield no valid points. @@ -47,9 +48,12 @@ def from_2d( selects its points from that shared projection, with the mask splat radius taken from the cloud's own lattice pitch. """ - world_points, points_2d = Detection3DPC.project_cloud( + world_points, points_2d, indices = Detection3DPC._project_cloud_indices( world_pointcloud, camera_info, world_to_optical_transform ) + keep = np.zeros(world_pointcloud.width * world_pointcloud.height, dtype=bool) + keep[indices] = True + projected_cloud = select_points(world_pointcloud, keep) quantum = lattice_quantum(world_points) detections_3d = [ d3d @@ -61,10 +65,11 @@ def from_2d( points_2d, camera_info, world_to_optical_transform, - world_pointcloud.frame_id, - world_pointcloud.ts, + world_pointcloud.header.frame_id, + to_seconds(world_pointcloud.header.stamp), filters, splat_m=quantum / 2 if quantum is not None else None, + projected_cloud=projected_cloud, ) ) is not None @@ -77,7 +82,7 @@ def from_depth( detections_2d: ImageDetections2D, depth: Image, camera_info: CameraInfo, - world_to_optical_transform: Transform, + world_to_optical_transform: TransformStamped, filters: list[PointCloudFilter] | None = None, ) -> ImageDetections3DPC: """Unproject every detection's depth pixels into 3D, dropping empty results.""" diff --git a/dimos/perception/detection/type/detection3d/marker.py b/dimos/perception/detection/type/detection3d/marker.py index 9fd58cc6d85..ac9f8768953 100644 --- a/dimos/perception/detection/type/detection3d/marker.py +++ b/dimos/perception/detection/type/detection3d/marker.py @@ -19,7 +19,7 @@ import numpy as np -from dimos.msgs.vision_msgs.Detection3D import Detection3D +from dimos.messages.vision_msgs.msg import Detection3D from dimos.perception.detection.type.detection3d.bbox import Detection3DBBox @@ -35,7 +35,9 @@ class Detection3DMarker(Detection3DBBox): """ marker_id: int = -1 - corners_px: np.ndarray = field(default_factory=lambda: np.zeros((4, 2), dtype=np.float32)) + corners_px: np.ndarray[Any, np.dtype[np.float32]] = field( + default_factory=lambda: np.zeros((4, 2), dtype=np.float32) + ) dictionary: str = "" reprojection_error: float = 0.0 @@ -52,8 +54,9 @@ def to_detection3d_msg(self) -> Detection3D: msg = super().to_detection3d_msg() msg.id = str(self.marker_id) if msg.results: - msg.results[0].hypothesis.class_id = self.marker_label - msg.results_length = len(msg.results) + result = msg.results[0] + result.hypothesis.class_id = self.marker_label + msg.results[0] = result return msg def to_repr_dict(self) -> dict[str, Any]: diff --git a/dimos/perception/detection/type/detection3d/pointcloud.py b/dimos/perception/detection/type/detection3d/pointcloud.py index d1d2d6eb75b..c82ac3c5458 100644 --- a/dimos/perception/detection/type/detection3d/pointcloud.py +++ b/dimos/perception/detection/type/detection3d/pointcloud.py @@ -18,12 +18,31 @@ import functools from typing import TYPE_CHECKING, Any +import cv2 import numpy as np - -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from numpy.typing import NDArray +import open3d as o3d + +from dimos.message_utils.geometry import inverse_transform, transform_matrix +from dimos.message_utils.image import image_view +from dimos.message_utils.pointcloud import ( + concatenate_clouds, + pointcloud_from_xyz, + pointcloud_xyz, + select_points, +) +from dimos.message_utils.time import time_from_seconds, to_seconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + TransformStamped, + Vector3, +) +from dimos.messages.sensor_msgs.msg import PointCloud2 +from dimos.messages.std_msgs.msg import Header from dimos.perception.detection.type.detection3d.base import Detection3D from dimos.perception.detection.type.detection3d.pointcloud_filters import ( PointCloudFilter, @@ -33,9 +52,7 @@ ) if TYPE_CHECKING: - from dimos_lcm.sensor_msgs import CameraInfo - - from dimos.msgs.sensor_msgs.Image import Image + from dimos.messages.sensor_msgs.msg import CameraInfo, Image from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox @@ -65,11 +82,24 @@ def lattice_quantum(points: np.ndarray) -> float | None: @dataclass class Detection3DPC(Detection3D): - pointcloud: PointCloud2 = field(default_factory=PointCloud2) + pointcloud: PointCloud2 = field( + default_factory=lambda: PointCloud2( + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + height=0, + width=0, + fields=[], + is_bigendian=False, + point_step=0, + row_step=0, + data=np.array([], dtype=np.uint8), + is_dense=False, + ) + ) @functools.cached_property def center(self) -> Vector3: - return Vector3(*self.pointcloud.center) + center = pointcloud_xyz(self.pointcloud).mean(axis=0) + return Vector3(x=float(center[0]), y=float(center[1]), z=float(center[2])) def __add__(self, other: Detection3DPC) -> Detection3DPC: """Union of two sightings of one object. @@ -88,7 +118,7 @@ def __add__(self, other: Detection3DPC) -> Detection3DPC: confidence=stronger.confidence, name=stronger.name, ts=later.ts, - pointcloud=self.pointcloud + other.pointcloud, + pointcloud=concatenate_clouds(self.pointcloud, other.pointcloud), transform=later.transform, frame_id=self.frame_id, ) @@ -101,36 +131,51 @@ def pose(self) -> PoseStamped: The pointcloud is already in world frame. """ return PoseStamped( - ts=self.ts, - frame_id=self.frame_id, - position=self.center, - orientation=(0.0, 0.0, 0.0, 1.0), # Identity quaternion + header=self.pointcloud.header, + pose=Pose( + position=Point(x=self.center.x, y=self.center.y, z=self.center.z), + orientation=Quaternion(w=1, x=0.0, y=0.0, z=0.0), + ), ) - def get_bounding_box(self): # type: ignore[no-untyped-def] + def _open3d(self) -> o3d.geometry.PointCloud: + return o3d.geometry.PointCloud(o3d.utility.Vector3dVector(pointcloud_xyz(self.pointcloud))) + + def get_bounding_box(self) -> o3d.geometry.AxisAlignedBoundingBox: """Get axis-aligned bounding box of the detection's pointcloud.""" - return self.pointcloud.axis_aligned_bounding_box + return self._open3d().get_axis_aligned_bounding_box() - def get_oriented_bounding_box(self): # type: ignore[no-untyped-def] + def get_oriented_bounding_box(self) -> o3d.geometry.OrientedBoundingBox: """Get oriented bounding box of the detection's pointcloud.""" - return self.pointcloud.oriented_bounding_box + return self._open3d().get_oriented_bounding_box() def get_bounding_box_dimensions(self) -> tuple[float, float, float]: """Get dimensions (width, height, depth) of the detection's bounding box.""" - return self.pointcloud.bounding_box_dimensions + extent = self.get_bounding_box().get_extent() + return float(extent[0]), float(extent[1]), float(extent[2]) def bounding_box_intersects(self, other: Detection3DPC) -> bool: """Check if this detection's bounding box intersects with another's.""" - return self.pointcloud.bounding_box_intersects(other.pointcloud) + first, second = self.get_bounding_box(), other.get_bounding_box() + return bool( + np.all(first.get_min_bound() <= second.get_max_bound()) + and np.all(second.get_min_bound() <= first.get_max_bound()) + ) def to_repr_dict(self) -> dict[str, Any]: # Calculate distance from camera # The pointcloud is in world frame, and transform gives camera position in world center_world = self.center # Camera position in world frame is the translation part of the transform - camera_pos = self.transform.translation + camera_pos = inverse_transform(self.transform).transform.translation # Use Vector3 subtraction and magnitude - distance = (center_world - camera_pos).magnitude() + distance = np.linalg.norm( + [ + center_world.x - camera_pos.x, + center_world.y - camera_pos.y, + center_world.z - camera_pos.z, + ] + ) parent_dict = super().to_repr_dict() # Remove bbox key if present @@ -139,7 +184,7 @@ def to_repr_dict(self) -> dict[str, Any]: return { **parent_dict, "dist": f"{distance:.2f}m", - "points": str(len(self.pointcloud)), + "points": str(self.pointcloud.width * self.pointcloud.height), } @classmethod @@ -148,7 +193,7 @@ def from_depth( det: Detection2DBBox, depth: Image, camera_info: CameraInfo, - world_to_optical_transform: Transform, + world_to_optical_transform: TransformStamped, filters: list[PointCloudFilter] | None = None, max_depth: float = 10.0, depth_gap: float = 0.1, @@ -168,8 +213,9 @@ def from_depth( if filters is None: filters = [statistical()] - depth_m = np.asarray(depth.data, dtype=np.float32) - if depth.data.dtype == np.uint16: + pixels = image_view(depth) + depth_m = pixels.astype(np.float32) + if pixels.dtype.kind == "u" and pixels.dtype.itemsize == 2: depth_m *= 0.001 height, width = depth_m.shape[:2] @@ -177,8 +223,6 @@ def from_depth( if seg_mask is not None: pixel_mask = seg_mask > 0 if mask_scale < 1.0: - import cv2 - radius = float(np.sqrt(pixel_mask.sum() / np.pi)) erode_px = round((1.0 - mask_scale) * radius) if erode_px > 0: @@ -214,12 +258,16 @@ def from_depth( rows, cols, z = rows[keep], cols[keep], z[keep] break - fx, fy = camera_info.K[0], camera_info.K[4] - cx, cy = camera_info.K[2], camera_info.K[5] + fx, fy = camera_info.k[0], camera_info.k[4] + cx, cy = camera_info.k[2], camera_info.k[5] points_optical = np.column_stack(((cols - cx) * z / fx, (rows - cy) * z / fy, z)) - detection_pc = PointCloud2.from_numpy(points_optical, timestamp=det.ts).transform( - -world_to_optical_transform + optical_to_world = inverse_transform(world_to_optical_transform) + matrix = transform_matrix(optical_to_world.transform) + points_world = points_optical @ matrix[:3, :3].T + matrix[:3, 3] + detection_pc = pointcloud_from_xyz( + points_world, + header=Header(stamp=depth.header.stamp, frame_id=optical_to_world.header.frame_id), ) for filter_func in filters: @@ -228,7 +276,7 @@ def from_depth( return None detection_pc = result - if len(detection_pc.pointcloud.points) == 0: + if detection_pc.width * detection_pc.height == 0: return None return cls( @@ -241,7 +289,7 @@ def from_depth( ts=det.ts, pointcloud=detection_pc, transform=world_to_optical_transform, - frame_id=detection_pc.frame_id, + frame_id=detection_pc.header.frame_id, ) @staticmethod @@ -253,17 +301,15 @@ def project_pixels(points_camera: np.ndarray, camera_info: CameraInfo) -> np.nda an undistorted calibration falls through to the pinhole projection. Points beyond the model's monotonic radius get out-of-image pixels. """ - fx, fy = camera_info.K[0], camera_info.K[4] - cx, cy = camera_info.K[2], camera_info.K[5] + fx, fy = camera_info.k[0], camera_info.k[4] + cx, cy = camera_info.k[2], camera_info.k[5] coefficients = np.asarray( - camera_info.D if camera_info.D is not None else (), dtype=np.float64 + camera_info.d if camera_info.d is not None else (), dtype=np.float64 ) xy = points_camera[:, :2] / points_camera[:, 2:3] if coefficients.size == 0 or not np.any(coefficients): return np.column_stack((xy[:, 0] * fx + cx, xy[:, 1] * fy + cy)) - import cv2 - fisheye = camera_info.distortion_model == "equidistant" # The polynomial is calibrated only out to the image corner; beyond # it the projection extrapolates or folds back into the image, so @@ -317,28 +363,42 @@ def project_pixels(points_camera: np.ndarray, camera_info: CameraInfo) -> np.nda def project_cloud( world_pointcloud: PointCloud2, camera_info: CameraInfo, - world_to_optical_transform: Transform, + world_to_optical_transform: TransformStamped, ) -> tuple[np.ndarray, np.ndarray]: + """Project a frame once for the existing batched detection API.""" + points, pixels, _ = Detection3DPC._project_cloud_indices( + world_pointcloud, camera_info, world_to_optical_transform + ) + return points, pixels + + @staticmethod + def _project_cloud_indices( + world_pointcloud: PointCloud2, + camera_info: CameraInfo, + world_to_optical_transform: TransformStamped, + ) -> tuple[np.ndarray, np.ndarray, NDArray[np.int64]]: """Project a world cloud through the camera once. Returns the world points that land inside the image and their pixel coordinates - the detection-independent half of ``from_2d``, shared by every detection of one frame via ``from_projection``. """ - world_points, _ = world_pointcloud.as_numpy() + world_points = pointcloud_xyz(world_pointcloud) + indices = np.arange(len(world_points), dtype=np.int64) # Project points to camera frame points_homogeneous = np.hstack([world_points, np.ones((world_points.shape[0], 1))]) - extrinsics_matrix = world_to_optical_transform.to_matrix() + extrinsics_matrix = transform_matrix(world_to_optical_transform.transform) points_camera = (extrinsics_matrix @ points_homogeneous.T).T # Filter out points behind the camera valid_mask = points_camera[:, 2] > 0 points_camera = points_camera[valid_mask] world_points = world_points[valid_mask] + indices = indices[valid_mask] if len(world_points) == 0: - return world_points, np.empty((0, 2)) + return world_points, np.empty((0, 2)), indices points_2d = Detection3DPC.project_pixels(points_camera, camera_info) @@ -349,7 +409,7 @@ def project_cloud( & (points_2d[:, 1] >= 0) & (points_2d[:, 1] < camera_info.height) ) - return world_points[in_image_mask], points_2d[in_image_mask] + return world_points[in_image_mask], points_2d[in_image_mask], indices[in_image_mask] @classmethod def from_projection( @@ -358,11 +418,13 @@ def from_projection( world_points: np.ndarray, points_2d: np.ndarray, camera_info: CameraInfo, - world_to_optical_transform: Transform, + world_to_optical_transform: TransformStamped, frame_id: str, timestamp: float, filters: list[PointCloudFilter] | None = None, splat_m: float | None = None, + *, + projected_cloud: PointCloud2 | None = None, ) -> Detection3DPC | None: """Create a Detection3D by selecting from a shared frame projection. @@ -399,9 +461,11 @@ def mask_hit(cols_f: np.ndarray, rows_f: np.ndarray) -> np.ndarray: in_det_mask = mask_hit(points_2d[:, 0], points_2d[:, 1]) if splat_m is not None: - camera = world_to_optical_transform.inverse().translation.to_numpy() + camera = transform_matrix(inverse_transform(world_to_optical_transform).transform)[ + :3, 3 + ] ranges = np.linalg.norm(world_points - camera, axis=1) - radius = splat_m * camera_info.K[0] / np.maximum(ranges, 1e-6) + radius = splat_m * camera_info.k[0] / np.maximum(ranges, 1e-6) for dx, dy in ((1, 0), (-1, 0), (0, 1), (0, -1)): in_det_mask |= mask_hit( points_2d[:, 0] + dx * radius, points_2d[:, 1] + dy * radius @@ -422,10 +486,13 @@ def mask_hit(cols_f: np.ndarray, rows_f: np.ndarray) -> np.ndarray: return None # Create initial pointcloud for this detection - initial_pc = PointCloud2.from_numpy( - detection_points, - frame_id=frame_id, - timestamp=timestamp, + initial_pc = ( + select_points(projected_cloud, in_det_mask) + if projected_cloud is not None + else pointcloud_from_xyz( + detection_points, + header=Header(frame_id=frame_id, stamp=time_from_seconds(timestamp)), + ) ) # Apply filters - each filter gets all arguments @@ -437,7 +504,7 @@ def mask_hit(cols_f: np.ndarray, rows_f: np.ndarray) -> np.ndarray: detection_pc = result # Final check for empty pointcloud - if len(detection_pc.pointcloud.points) == 0: + if detection_pc.width * detection_pc.height == 0: return None # Create Detection3D with filtered pointcloud @@ -460,7 +527,7 @@ def from_2d( # type: ignore[override] det: Detection2DBBox, world_pointcloud: PointCloud2, camera_info: CameraInfo, - world_to_optical_transform: Transform, + world_to_optical_transform: TransformStamped, # filters are to be adjusted based on the sensor noise characteristics if feeding # sensor data directly filters: list[PointCloudFilter] | None = None, @@ -473,9 +540,12 @@ def from_2d( # type: ignore[override] radius comes from the cloud's own lattice pitch, so every caller samples a grid-quantized source the same way. """ - world_points, points_2d = cls.project_cloud( + world_points, points_2d, indices = cls._project_cloud_indices( world_pointcloud, camera_info, world_to_optical_transform ) + keep = np.zeros(world_pointcloud.width * world_pointcloud.height, dtype=bool) + keep[indices] = True + projected_cloud = select_points(world_pointcloud, keep) quantum = lattice_quantum(world_points) return cls.from_projection( det, @@ -483,8 +553,9 @@ def from_2d( # type: ignore[override] points_2d, camera_info, world_to_optical_transform, - world_pointcloud.frame_id, - world_pointcloud.ts, + world_pointcloud.header.frame_id, + to_seconds(world_pointcloud.header.stamp), filters, splat_m=quantum / 2 if quantum is not None else None, + projected_cloud=projected_cloud, ) diff --git a/dimos/perception/detection/type/detection3d/pointcloud_filters.py b/dimos/perception/detection/type/detection3d/pointcloud_filters.py index f55d0d09f36..f2d95a7c85d 100644 --- a/dimos/perception/detection/type/detection3d/pointcloud_filters.py +++ b/dimos/perception/detection/type/detection3d/pointcloud_filters.py @@ -15,34 +15,48 @@ from __future__ import annotations from collections.abc import Callable +from typing import TYPE_CHECKING -from dimos_lcm.sensor_msgs import CameraInfo +import numpy as np +import open3d as o3d -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox +from dimos.message_utils.geometry import inverse_transform, transform_matrix +from dimos.message_utils.pointcloud import pointcloud_xyz, select_points +from dimos.messages.geometry_msgs.msg import TransformStamped +from dimos.messages.sensor_msgs.msg import CameraInfo, PointCloud2 + +if TYPE_CHECKING: + from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox -# Filters take Detection2DBBox, PointCloud2, CameraInfo, Transform and return filtered PointCloud2 or None PointCloudFilter = Callable[ - [Detection2DBBox, PointCloud2, CameraInfo, Transform], PointCloud2 | None + ["Detection2DBBox", PointCloud2, CameraInfo, TransformStamped], PointCloud2 | None ] +def _open3d_cloud(pc: PointCloud2) -> o3d.geometry.PointCloud: + return o3d.geometry.PointCloud(o3d.utility.Vector3dVector(pointcloud_xyz(pc))) + + +def _selected(pc: PointCloud2, indices: list[int]) -> PointCloud2: + keep = np.zeros(pc.width * pc.height, dtype=bool) + keep[indices] = True + return select_points(pc, keep) + + def height_filter(height: float = 0.1) -> PointCloudFilter: - return lambda det, pc, ci, tf: pc.filter_by_height(height) + return lambda det, pc, ci, tf: select_points(pc, pointcloud_xyz(pc)[:, 2] >= height) def statistical(nb_neighbors: int = 40, std_ratio: float = 0.5) -> PointCloudFilter: def filter_func( - det: Detection2DBBox, pc: PointCloud2, ci: CameraInfo, tf: Transform + det: Detection2DBBox, pc: PointCloud2, ci: CameraInfo, tf: TransformStamped ) -> PointCloud2 | None: try: - statistical, _removed = pc.pointcloud.remove_statistical_outlier( + _, indices = _open3d_cloud(pc).remove_statistical_outlier( nb_neighbors=nb_neighbors, std_ratio=std_ratio ) - return PointCloud2(statistical, pc.frame_id, pc.ts) - except Exception: - # print("statistical filter failed:", e) + return _selected(pc, indices) + except RuntimeError: return None return filter_func @@ -55,15 +69,14 @@ def range_cluster(gap: float = 0.3) -> PointCloudFilter: points a mask collects across a range discontinuity are background seen through or around the object, not the object. """ - import numpy as np def filter_func( - det: Detection2DBBox, pc: PointCloud2, ci: CameraInfo, tf: Transform + det: Detection2DBBox, pc: PointCloud2, ci: CameraInfo, tf: TransformStamped ) -> PointCloud2 | None: - points, _ = pc.as_numpy() + points = pointcloud_xyz(pc) if len(points) == 0: return None - camera = tf.inverse().translation.to_numpy() + camera = transform_matrix(inverse_transform(tf).transform)[:3, 3] ranges = np.linalg.norm(points - camera, axis=1) order = np.argsort(ranges) ranges_sorted = ranges[order] @@ -74,7 +87,7 @@ def filter_func( for start, end in zip(starts, ends, strict=False): if start <= median_idx < end: keep = order[start:end] - return PointCloud2.from_numpy(points[keep], frame_id=pc.frame_id, timestamp=pc.ts) + return _selected(pc, keep.tolist()) return pc return filter_func @@ -82,33 +95,27 @@ def filter_func( def raycast() -> PointCloudFilter: def filter_func( - det: Detection2DBBox, pc: PointCloud2, ci: CameraInfo, tf: Transform + det: Detection2DBBox, pc: PointCloud2, ci: CameraInfo, tf: TransformStamped ) -> PointCloud2 | None: try: - camera_pos = tf.inverse().translation - camera_pos_np = camera_pos.to_numpy() - _, visible_indices = pc.pointcloud.hidden_point_removal(camera_pos_np, radius=100.0) - visible_pcd = pc.pointcloud.select_by_index(visible_indices) - return PointCloud2(visible_pcd, pc.frame_id, pc.ts) - except Exception: - # print("raycast filter failed:", e) + camera_pos = inverse_transform(tf).transform.translation + _, indices = _open3d_cloud(pc).hidden_point_removal( + np.array([camera_pos.x, camera_pos.y, camera_pos.z]), radius=100.0 + ) + return _selected(pc, indices) + except RuntimeError: return None return filter_func def radius_outlier(min_neighbors: int = 20, radius: float = 0.3) -> PointCloudFilter: - """ - Remove isolated points: keep only points that have at least `min_neighbors` - neighbors within `radius` meters (same units as your point cloud). - """ + """Keep points with at least ``min_neighbors`` within ``radius`` meters.""" def filter_func( - det: Detection2DBBox, pc: PointCloud2, ci: CameraInfo, tf: Transform + det: Detection2DBBox, pc: PointCloud2, ci: CameraInfo, tf: TransformStamped ) -> PointCloud2 | None: - filtered_pcd, _removed = pc.pointcloud.remove_radius_outlier( - nb_points=min_neighbors, radius=radius - ) - return PointCloud2(filtered_pcd, pc.frame_id, pc.ts) + _, indices = _open3d_cloud(pc).remove_radius_outlier(nb_points=min_neighbors, radius=radius) + return _selected(pc, indices) return filter_func diff --git a/dimos/perception/detection/type/detection3d/test_generated_filters.py b/dimos/perception/detection/type/detection3d/test_generated_filters.py new file mode 100644 index 00000000000..7e35242b950 --- /dev/null +++ b/dimos/perception/detection/type/detection3d/test_generated_filters.py @@ -0,0 +1,98 @@ +# Copyright 2025-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 dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode +import numpy as np +import pytest + +from dimos.message_utils.image import image_from_array +from dimos.message_utils.pointcloud import pointcloud_view +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.sensor_msgs.msg import CameraInfo, PointCloud2, PointField, RegionOfInterest +from dimos.messages.std_msgs.msg import Header +from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox +from dimos.perception.detection.type.detection3d.pointcloud_filters import ( + height_filter, + radius_outlier, + raycast, + statistical, +) + + +@pytest.mark.parametrize("make_filter", [height_filter, statistical, radius_outlier, raycast]) +def test_filters_preserve_custom_point_records(make_filter): + rng = np.random.default_rng(42) + values = np.zeros(101, dtype=[("x", " Image: - return Image( - data=np.zeros((80, 100, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera_optical", - ts=ts, + return image_from_array( + np.zeros((80, 100, 3), dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera_optical", stamp=time_from_seconds(ts)), ) @@ -52,12 +54,12 @@ def _marker( class_id=marker_id, confidence=confidence, name="", - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, - center=Vector3(float(marker_id), 2.0, 3.0), - size=Vector3(0.16, 0.16, 0.0), + center=Vector3(x=float(marker_id), y=2.0, z=3.0), + size=Vector3(x=0.16, y=0.16, z=0.0), frame_id=frame_id, - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), marker_id=marker_id, corners_px=np.array( [[x1, y1], [x2, y1], [x2, y2], [x1, y2]], @@ -76,22 +78,22 @@ def test_to_ros_detection3d_array_serializes_plain_bbox_results() -> None: class_id=9, confidence=0.75, name="box", - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, - center=Vector3(1.0, 2.0, 3.0), - size=Vector3(0.4, 0.5, 0.6), + center=Vector3(x=1.0, y=2.0, z=3.0), + size=Vector3(x=0.4, y=0.5, z=0.6), frame_id="world", - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), ) msg = ImageDetections3D(image, [det]).to_ros_detection3d_array() - assert msg.detections_length == 1 - assert msg.detections[0].results_length == 1 + assert len(msg.detections) == 1 + assert len(msg.detections[0].results) == 1 - decoded = Detection3DArray.lcm_decode(msg.lcm_encode()) + decoded = cdr_decode(cdr_encode(msg), Detection3DArray) decoded_det = decoded.detections[0] - assert decoded_det.results_length == 1 + assert len(decoded_det.results) == 1 assert decoded_det.results[0].hypothesis.class_id == "9" assert decoded_det.results[0].hypothesis.score == pytest.approx(0.75) assert decoded_det.bbox.center.position.x == pytest.approx(1.0) @@ -111,23 +113,22 @@ def test_to_ros_detection3d_array_preserves_marker_wire_identity() -> None: msg = detections.to_ros_detection3d_array() assert msg.header.frame_id == "world" - assert msg.ts == pytest.approx(image.ts) - assert msg.detections_length == 2 + assert to_seconds(msg.header.stamp) == pytest.approx(to_seconds(image.header.stamp)) assert len(msg.detections) == 2 first = msg.detections[0] assert first.header.frame_id == "world" assert first.id == "7" - assert first.results_length == 1 + assert len(first.results) == 1 assert first.results[0].hypothesis.class_id == "DICT_APRILTAG_36h11:7" assert first.results[0].hypothesis.score == pytest.approx(1.0) assert first.bbox.center.position.x == pytest.approx(7.0) assert first.bbox.size.x == pytest.approx(0.16) assert first.bbox.size.z == pytest.approx(0.0) - decoded = Detection3DArray.lcm_decode(msg.lcm_encode()) + decoded = cdr_decode(cdr_encode(msg), Detection3DArray) assert decoded.header.frame_id == "world" - assert decoded.detections_length == 2 + assert len(decoded.detections) == 2 assert decoded.detections[1].id == "42" assert decoded.detections[1].results[0].hypothesis.class_id == "DICT_APRILTAG_36h11:42" @@ -140,15 +141,14 @@ def test_to_ros_detection3d_array_uses_override_and_handles_empty_frames() -> No ) assert msg.header.frame_id == "map" - assert msg.ts == pytest.approx(image.ts) - assert msg.detections_length == 1 + assert to_seconds(msg.header.stamp) == pytest.approx(to_seconds(image.header.stamp)) + assert len(msg.detections) == 1 empty = ImageDetections3D(image, []).to_ros_detection3d_array(frame_id="world") assert empty.header.frame_id == "world" - assert empty.ts == pytest.approx(image.ts) - assert empty.detections_length == 0 - assert empty.detections == [] + assert to_seconds(empty.header.stamp) == pytest.approx(to_seconds(image.header.stamp)) + assert len(empty.detections) == 0 def test_filter_and_annotated_image_work_for_3d_marker_detections() -> None: @@ -162,10 +162,28 @@ def test_filter_and_annotated_image_work_for_3d_marker_detections() -> None: assert isinstance(filtered, ImageDetections3D) assert filtered.detections == [keep] ros_msg = filtered.to_ros_detection3d_array() - assert ros_msg.detections_length == 1 + assert len(ros_msg.detections) == 1 assert ros_msg.detections[0].id == "5" annotated = filtered.annotated_image() - assert annotated.ts == pytest.approx(image.ts) - assert np.count_nonzero(annotated.data) > 0 - assert np.count_nonzero(image.data) == 0 + assert to_seconds(annotated.header.stamp) == pytest.approx(to_seconds(image.header.stamp)) + assert np.count_nonzero(image_view(annotated)) > 0 + assert np.count_nonzero(image_view(image)) == 0 + + +def test_marker_pose_and_array_keep_exact_source_stamp() -> None: + image = _image() + image.header.stamp.sec = 1700000000 + image.header.stamp.nanosec = 123456789 + marker = _marker(image, marker_id=42) + pose = marker.pose + message = marker.to_detection3d_msg() + array = ImageDetections3D(image, [marker]).to_ros_detection3d_array() + decoded = cdr_decode(cdr_encode(array), Detection3DArray) + assert pose.header.stamp == image.header.stamp + assert message.header.stamp == image.header.stamp + assert decoded.header.stamp == image.header.stamp + assert decoded.detections[0].header.stamp == image.header.stamp + marker.center.x = 999 + assert pose.pose.position.x == 42 + assert decoded.detections[0].bbox.center.position.x == 42 diff --git a/dimos/perception/detection/type/detection3d/test_marker.py b/dimos/perception/detection/type/detection3d/test_marker.py index 79a71f0debb..5a688dfc9c0 100644 --- a/dimos/perception/detection/type/detection3d/test_marker.py +++ b/dimos/perception/detection/type/detection3d/test_marker.py @@ -13,13 +13,15 @@ # limitations under the License. import cv2 +from dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode import numpy as np import pytest -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.vision_msgs.Detection3D import Detection3D +from dimos.message_utils.image import image_from_array +from dimos.message_utils.time import time_from_seconds, to_seconds +from dimos.messages.geometry_msgs.msg import Quaternion, Vector3 +from dimos.messages.std_msgs.msg import Header +from dimos.messages.vision_msgs.msg import Detection3D from dimos.perception.detection.type.detection3d.marker import Detection3DMarker from dimos.perception.fiducial.marker_pose import marker_reprojection_error @@ -41,9 +43,9 @@ def _project_synthetic_marker_corners( def test_marker_detection3d_msg_preserves_marker_identity_on_wire() -> None: - center = Vector3(1.0, 2.0, 3.0) - orientation = Quaternion(0.1, 0.2, 0.3, 0.9) - size = Vector3(0.16, 0.16, 0.0) + center = Vector3(x=1.0, y=2.0, z=3.0) + orientation = Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + size = Vector3(x=0.16, y=0.16, z=0.0) marker_length_m = 0.16 camera_matrix = np.array( [[420.0, 0.0, 60.0], [0.0, 420.0, 50.0], [0.0, 0.0, 1.0]], @@ -68,10 +70,10 @@ def test_marker_detection3d_msg_preserves_marker_identity_on_wire() -> None: rvec, tvec, ) - image = Image( - data=np.zeros((100, 120, 3), dtype=np.uint8), - frame_id="camera_optical", - ts=123.456, + image = image_from_array( + np.zeros((100, 120, 3), dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera_optical", stamp=time_from_seconds(123.456)), ) det = Detection3DMarker( @@ -80,7 +82,7 @@ def test_marker_detection3d_msg_preserves_marker_identity_on_wire() -> None: class_id=999, confidence=1.0, name="marker_42", - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, center=center, size=size, @@ -99,7 +101,7 @@ def test_marker_detection3d_msg_preserves_marker_identity_on_wire() -> None: assert msg.header.frame_id == "world" assert msg.id == "42" - assert msg.results_length == 1 + assert len(msg.results) == 1 assert msg.results[0].hypothesis.class_id == "DICT_APRILTAG_36h11:42" assert msg.results[0].hypothesis.score == pytest.approx(1.0) assert msg.bbox.center.position.x == pytest.approx(center.x) @@ -113,7 +115,7 @@ def test_marker_detection3d_msg_preserves_marker_identity_on_wire() -> None: assert msg.bbox.size.y == pytest.approx(size.y) assert msg.bbox.size.z == pytest.approx(size.z) - decoded = Detection3D.lcm_decode(msg.lcm_encode()) + decoded = cdr_decode(cdr_encode(msg), Detection3D) assert decoded.id == "42" - assert decoded.results_length == 1 + assert len(decoded.results) == 1 assert decoded.results[0].hypothesis.class_id == "DICT_APRILTAG_36h11:42" diff --git a/dimos/perception/detection/type/detection3d/test_pointcloud.py b/dimos/perception/detection/type/detection3d/test_pointcloud.py index 74b28a09b38..32ba767a732 100644 --- a/dimos/perception/detection/type/detection3d/test_pointcloud.py +++ b/dimos/perception/detection/type/detection3d/test_pointcloud.py @@ -16,10 +16,13 @@ import numpy as np import pytest -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.message_utils.geometry import transform_from_matrix +from dimos.message_utils.image import image_from_array +from dimos.message_utils.pointcloud import pointcloud_from_xyz, pointcloud_xyz +from dimos.message_utils.time import time_from_seconds, to_seconds +from dimos.messages.geometry_msgs.msg import TransformStamped +from dimos.messages.sensor_msgs.msg import CameraInfo, Image, PointCloud2, RegionOfInterest +from dimos.messages.std_msgs.msg import Header from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.detection.type.detection2d.seg import Detection2DSeg @@ -31,15 +34,44 @@ PITCH = 0.05 K = [100.0, 0.0, 320.0, 0.0, 100.0, 240.0, 0.0, 0.0, 1.0] -CAMERA_INFO = CameraInfo(height=480, width=640, K=K) + + +def _camera(distortion=()): + return CameraInfo( + header=Header(stamp=time_from_seconds(1.0), frame_id="camera_optical"), + height=480, + width=640, + distortion_model="plumb_bob", + d=np.asarray(distortion, dtype=np.float64), + k=np.asarray(K, dtype=np.float64), + r=np.eye(3).reshape(-1), + p=np.zeros(12, dtype=np.float64), + binning_x=0, + binning_y=0, + roi=RegionOfInterest(x_offset=0, y_offset=0, height=0, width=0, do_rectify=False), + ) + + +CAMERA_INFO = _camera() + + +def _identity(): + return TransformStamped( + header=Header(stamp=time_from_seconds(1.0), frame_id="camera_optical"), + child_frame_id="world", + transform=transform_from_matrix(np.eye(4)), + ) + + +def _cloud(points, ts=1.0): + return pointcloud_from_xyz(points, header=Header(stamp=time_from_seconds(ts), frame_id="world")) def _image() -> Image: - return Image( - data=np.zeros((480, 640, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera_optical", - ts=1.0, + return image_from_array( + np.zeros((480, 640, 3), dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera_optical", stamp=time_from_seconds(1.0)), ) @@ -57,7 +89,7 @@ def _sighting(ts: float, confidence: float, name: str, points: int) -> Detection confidence=confidence, name=name, ts=ts, - pointcloud=PointCloud2.from_numpy(np.full((points, 3), ts), frame_id="world", timestamp=ts), + pointcloud=_cloud(np.full((points, 3), ts), ts), frame_id="world", ) @@ -103,10 +135,10 @@ def test_detection3dpc(detection3dpc) -> None: # def test_point_cloud_properties(detection3dpc): """Test point cloud data and boundaries.""" - points, _ = detection3dpc.pointcloud.as_numpy() + points = pointcloud_xyz(detection3dpc.pointcloud) assert len(points) > 60 - assert detection3dpc.pointcloud.frame_id == "world", ( - f"Expected frame_id 'world', got '{detection3dpc.pointcloud.frame_id}'" + assert detection3dpc.pointcloud.header.frame_id == "world", ( + f"Expected frame_id 'world', got '{detection3dpc.pointcloud.header.frame_id}'" ) min_pt = np.min(points, axis=0) @@ -128,11 +160,11 @@ def test_detection3dpc(detection3dpc) -> None: # def test_detection_pose(detection3dpc): """Test detection pose and frame information.""" - assert detection3dpc.pose.x == pytest.approx(-3.327, abs=0.1) - assert detection3dpc.pose.y == pytest.approx(-0.202, abs=0.1) - assert detection3dpc.pose.z == pytest.approx(0.160, abs=0.1) - assert detection3dpc.pose.frame_id == "world", ( - f"Expected frame_id 'world', got '{detection3dpc.pose.frame_id}'" + assert detection3dpc.pose.pose.position.x == pytest.approx(-3.327, abs=0.1) + assert detection3dpc.pose.pose.position.y == pytest.approx(-0.202, abs=0.1) + assert detection3dpc.pose.pose.position.z == pytest.approx(0.160, abs=0.1) + assert detection3dpc.pose.header.frame_id == "world", ( + f"Expected frame_id 'world', got '{detection3dpc.pose.header.frame_id}'" ) @@ -160,7 +192,7 @@ def test_add() -> None: merged = strong + later - assert len(merged.pointcloud.as_numpy()[0]) == 10 + assert len(pointcloud_xyz(merged.pointcloud)) == 10 assert (merged.name, merged.confidence) == ("cup", 0.9) assert (merged.ts, merged.bbox) == (2.0, later.bbox) @@ -171,7 +203,7 @@ def test_add() -> None: def test_project_pixels(D: list[float]) -> None: # matches OpenCV with and without distortion points = np.array([[0.3, -0.2, 1.0], [-0.6, 0.4, 1.5]]) - info = CameraInfo(height=480, width=640, distortion_model="plumb_bob", D=D, K=K) + info = _camera(D) expected, _ = cv2.projectPoints( points, np.zeros(3), np.zeros(3), np.reshape(K, (3, 3)), np.array(D) ) @@ -181,9 +213,9 @@ def test_project_pixels(D: list[float]) -> None: def test_project_cloud_empty() -> None: # nothing in front of the camera - cloud = PointCloud2.from_numpy(np.array([[0.0, 0.0, -1.0]]), frame_id="world", timestamp=1.0) + cloud = _cloud(np.array([[0.0, 0.0, -1.0]])) - world_points, pixels = Detection3DPC.project_cloud(cloud, CAMERA_INFO, Transform.identity()) + world_points, pixels = Detection3DPC.project_cloud(cloud, CAMERA_INFO, _identity()) assert len(world_points) == len(pixels) == 0 @@ -199,21 +231,21 @@ def test_from_2d_splat() -> None: class_id=0, confidence=0.9, name="stripe", - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, mask=mask, ) - cloud = PointCloud2.from_numpy(_lattice(), frame_id="world", timestamp=1.0) + cloud = _cloud(_lattice()) lifted = ImageDetections3DPC.from_2d( ImageDetections2D(image=image, detections=[stripe]), cloud, CAMERA_INFO, - Transform.identity(), + _identity(), [], ) - columns = np.unique(np.round(lifted.detections[0].pointcloud.as_numpy()[0][:, 0], 3)) + columns = np.unique(np.round(pointcloud_xyz(lifted.detections[0].pointcloud)[:, 0], 3)) np.testing.assert_allclose(columns, [0.0, PITCH]) @@ -232,10 +264,10 @@ def test_range_cluster() -> None: ) def keep(points: np.ndarray) -> PointCloud2 | None: - cloud = PointCloud2.from_numpy(points, frame_id="world", timestamp=1.0) - return range_cluster()(det, cloud, CAMERA_INFO, Transform.identity()) + cloud = _cloud(points) + return range_cluster()(det, cloud, CAMERA_INFO, _identity()) kept = keep(np.vstack((near, far))) assert kept is not None - np.testing.assert_allclose(np.sort(kept.as_numpy()[0][:, 2]), near[:, 2], rtol=1e-6) + np.testing.assert_allclose(np.sort(pointcloud_xyz(kept)[:, 2]), near[:, 2], rtol=1e-6) assert keep(np.empty((0, 3))) is None diff --git a/dimos/perception/detection/type/imageDetections.py b/dimos/perception/detection/type/imageDetections.py index 98fd0e5388f..9b92c63f82d 100644 --- a/dimos/perception/detection/type/imageDetections.py +++ b/dimos/perception/detection/type/imageDetections.py @@ -22,15 +22,15 @@ else: from typing_extensions import Self -from dimos_lcm.vision_msgs import Detection2DArray - -from dimos.msgs.std_msgs.Header import Header +from dimos.message_utils.image import image_from_array, image_to_bgr +from dimos.message_utils.time import to_seconds +from dimos.messages.vision_msgs.msg import Detection2DArray from dimos.perception.detection.type.utils import TableStr if TYPE_CHECKING: from collections.abc import Callable, Iterator - from dimos.msgs.sensor_msgs.Image import Image + from dimos.messages.sensor_msgs.msg import Image from dimos.perception.detection.type.detection2d.base import Detection2D T = TypeVar("T", bound=Detection2D) @@ -40,20 +40,20 @@ T = TypeVar("T", bound=Detection2D) -class ImageDetections(Generic[T], TableStr): +class ImageDetections(TableStr, Generic[T]): image: Image detections: list[T] @property def ts(self) -> float: - return self.image.ts + return to_seconds(self.image.header.stamp) def __init__(self, image: Image, detections: list[T] | None = None) -> None: self.image = image self.detections = detections or [] for det in self.detections: if not det.ts: - det.ts = image.ts + det.ts = to_seconds(image.header.stamp) def __len__(self) -> int: return len(self.detections) @@ -80,18 +80,15 @@ def filter(self, *predicates: Callable[[T], bool]) -> Self: def to_ros_detection2d_array(self) -> Detection2DArray: return Detection2DArray( - detections_length=len(self.detections), - header=Header(self.image.ts, "camera_optical"), + header=self.image.header, detections=[det.to_ros_detection2d() for det in self.detections], ) def annotated_image(self, scale: float = 1.0) -> Image: """Return the image with all detection bboxes and labels drawn on it.""" - img = self.image.to_opencv().copy() + img = image_to_bgr(self.image) for det in self.detections: if hasattr(det, "draw_on"): det.draw_on(img, scale=scale) - from dimos.msgs.sensor_msgs.Image import Image as ImageMsg - - return ImageMsg.from_opencv(img, ts=self.image.ts) + return image_from_array(img, encoding="bgr8", header=self.image.header) diff --git a/dimos/perception/detection/type/test_detection3d.py b/dimos/perception/detection/type/test_detection3d.py index 1b0ed95a520..deb09dcf232 100644 --- a/dimos/perception/detection/type/test_detection3d.py +++ b/dimos/perception/detection/type/test_detection3d.py @@ -12,28 +12,26 @@ # See the License for the specific language governing permissions and # limitations under the License. -import time - import pytest +from dimos.message_utils.geometry import transform_matrix +from dimos.message_utils.time import to_seconds + pytestmark = pytest.mark.self_hosted -def test_guess_projection(get_moment_2d, publish_moment) -> None: +def test_recording_keeps_source_and_recorded_time_domains(get_moment_2d): moment = get_moment_2d() - for key, value in moment.items(): - print(key, "====================================") - print(value) - - moment.get("camera_info") - detection2d = moment.get("detections2d")[0] - tf = moment.get("tf") - tf.get("camera_optical", "world", detection2d.ts, 5.0) - - # for stash - # detection3d = Detection3D.from_2d(detection2d, 1.5, camera_info, transform) - # print(detection3d) - - publish_moment(moment) - time.sleep(0.1) - publish_moment(moment) + image = moment["image_frame"] + odom = moment["odom_frame"] + buffer = moment["tf"] + # These paired recorder entries have different sensor source clocks. + # Migration must preserve that fact rather than rewrite timestamps. + assert to_seconds(image.header.stamp) - to_seconds(odom.header.stamp) > 50 + assert buffer.get("camera_optical", "world", to_seconds(image.header.stamp), 5.0) is None + transform = buffer.get("camera_optical", "world", to_seconds(odom.header.stamp), 1e-6) + assert transform is not None + assert transform.header.frame_id == "camera_optical" + assert transform.child_frame_id == "world" + assert transform_matrix(transform.transform).shape == (4, 4) + assert len(moment["detections2d"]) > 0 diff --git a/dimos/perception/experimental/moduleDB.py b/dimos/perception/experimental/moduleDB.py index 305bca063a0..c68d83995be 100644 --- a/dimos/perception/experimental/moduleDB.py +++ b/dimos/perception/experimental/moduleDB.py @@ -16,17 +16,16 @@ import time from typing import Any +import numpy as np from reactivex.observable import Observable from dimos.core.core import rpc from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -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.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray +from dimos.message_utils.pointcloud import concatenate_clouds +from dimos.messages.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Vector3 +from dimos.messages.sensor_msgs.msg import Image, PointCloud2 +from dimos.messages.std_msgs.msg import Header +from dimos.messages.vision_msgs.msg import Detection2DArray from dimos.perception.detection.module3D import Detection3DModule from dimos.perception.detection.type.detection3d.imageDetections3DPC import ImageDetections3DPC from dimos.perception.detection.type.detection3d.pointcloud import Detection3DPC @@ -45,7 +44,9 @@ def to_repr_dict(self) -> dict[str, Any]: center_str = "None" else: center_str = ( - "[" + ", ".join(list(map(lambda n: f"{n:1f}", self.center.to_list()))) + "]" + "[" + + ", ".join([f"{n:1f}" for n in [self.center.x, self.center.y, self.center.z]]) + + "]" ) return { "object_id": self.track_id, @@ -82,9 +83,16 @@ def __add__(self, detection: Detection3DPC) -> "Object3D": new_object.class_id = self.class_id new_object.name = self.name new_object.transform = self.transform - new_object.pointcloud = self.pointcloud + detection.pointcloud + new_object.pointcloud = concatenate_clouds(self.pointcloud, detection.pointcloud) new_object.frame_id = self.frame_id - new_object.center = (self.center + detection.center) / 2 + if self.center is None: + raise ValueError("Cannot merge an object without a center") + center = detection.center + new_object.center = Vector3( + x=(self.center.x + center.x) / 2, + y=(self.center.y + center.y) / 2, + z=(self.center.z + center.z) / 2, + ) new_object.detections = self.detections + 1 if detection.bbox_2d_volume() > self.bbox_2d_volume(): @@ -113,26 +121,14 @@ def to_pose(self) -> PoseStamped: if self.best_detection is None or self.center is None: raise ValueError("Cannot compute pose without best_detection and center") - optical_inverse = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), - frame_id="camera_link", - child_frame_id="camera_optical", - ).inverse() - - print("transform is", self.best_detection.transform) - - global_transform = optical_inverse + self.best_detection.transform - - print("inverse optical is", global_transform) - - print("obj center is", self.center) - global_pose = global_transform.to_pose() - print("Global pose:", global_pose) - global_pose.frame_id = self.best_detection.frame_id - print("remap to", self.best_detection.frame_id) return PoseStamped( - position=self.center, orientation=Quaternion(), frame_id=self.best_detection.frame_id + header=Header( + frame_id=self.best_detection.frame_id, stamp=self.pointcloud.header.stamp + ), + pose=Pose( + position=Point(x=self.center.x, y=self.center.y, z=self.center.z), + orientation=Quaternion(w=1, x=0.0, y=0.0, z=0.0), + ), ) @@ -177,13 +173,26 @@ def __init__(self, *args, **kwargs) -> None: # type: ignore[no-untyped-def] def closest_object(self, detection: Detection3DPC) -> Object3D | None: # Filter objects to only those with matching names - matching_objects = [obj for obj in self.objects.values() if obj.name == detection.name] + matching_objects = [ + obj + for obj in self.objects.values() + if obj.name == detection.name and obj.center is not None + ] if not matching_objects: return None # Sort by distance - distances = sorted(matching_objects, key=lambda obj: detection.center.distance(obj.center)) + def distance(obj: Object3D) -> float: + assert obj.center is not None + center = detection.center + return float( + np.linalg.norm( + [center.x - obj.center.x, center.y - obj.center.y, center.z - obj.center.z] + ) + ) + + distances = sorted(matching_objects, key=distance) return distances[0] diff --git a/dimos/perception/experimental/object.py b/dimos/perception/experimental/object.py index 76f4cc425c1..16af65d63cb 100644 --- a/dimos/perception/experimental/object.py +++ b/dimos/perception/experimental/object.py @@ -19,25 +19,43 @@ from typing import TYPE_CHECKING, Any import uuid -from dimos_lcm.geometry_msgs import Pose -from dimos_lcm.vision_msgs import BoundingBox3D, ObjectHypothesis, ObjectHypothesisWithPose import numpy as np -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -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.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.std_msgs.Header import Header -from dimos.msgs.vision_msgs.Detection3D import Detection3D as ROSDetection3D -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.message_utils.geometry import quaternion_from_matrix, transform_matrix +from dimos.message_utils.image import image_to_rgb, image_view +from dimos.message_utils.pointcloud import ( + concatenate_clouds, + pointcloud_from_xyz, + pointcloud_from_xyz_rgb, + pointcloud_rgb, + pointcloud_to_open3d, + pointcloud_xyz, + voxel_downsample_cloud, +) +from dimos.message_utils.time import time_from_seconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + PoseWithCovariance, + Quaternion, + TransformStamped, + Vector3, +) +from dimos.messages.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos.messages.std_msgs.msg import Header +from dimos.messages.vision_msgs.msg import ( + BoundingBox3D, + Detection3D as ROSDetection3D, + Detection3DArray, + ObjectHypothesis, + ObjectHypothesisWithPose, +) from dimos.perception.detection.type.detection2d.seg import Detection2DSeg from dimos.perception.detection.type.detection3d.base import Detection3D if TYPE_CHECKING: - from dimos_lcm.sensor_msgs import CameraInfo - from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D @@ -54,7 +72,7 @@ class Object(Detection3D): size: Vector3 pose: PoseStamped pointcloud: PointCloud2 - camera_transform: Transform | None = None + camera_transform: TransformStamped | None = None mask: np.ndarray[Any, np.dtype[np.uint8]] | None = None detections_count: int = 1 visual_embedding: np.ndarray[Any, np.dtype[np.float32]] | None = None @@ -68,8 +86,8 @@ def __post_init__(self) -> None: def set_center(self, center: Vector3) -> None: """Update the canonical center and its pose representation together.""" - self.center = Vector3(center) - self.pose.position = self.center + self.center = Vector3(x=center.x, y=center.y, z=center.z) + self.pose.pose.position = Point(x=center.x, y=center.y, z=center.z) def update_object(self, other: Object, *, accumulate_pointcloud: bool = True) -> None: """Update this object with data from another detection. @@ -89,7 +107,7 @@ def update_object(self, other: Object, *, accumulate_pointcloud: bool = True) -> and other.camera_transform is not None and self.camera_transform is not None ): - self.pointcloud = self.pointcloud + other.pointcloud + self.pointcloud = concatenate_clouds(self.pointcloud, other.pointcloud) else: self.pointcloud = other.pointcloud @@ -115,20 +133,20 @@ def update_object(self, other: Object, *, accumulate_pointcloud: bool = True) -> def get_oriented_bounding_box(self) -> Any: """Get oriented bounding box of the pointcloud.""" - return self.pointcloud.oriented_bounding_box + return pointcloud_to_open3d(self.pointcloud).get_oriented_bounding_box() def _detection3d_bbox_components(self) -> tuple[Vector3, Quaternion, Vector3]: """Return canonical geometry without refitting the point cloud.""" center = self.center size = self.size - orientation = self.pose.orientation + orientation = self.pose.pose.orientation return ( - Vector3(center.x, center.y, center.z), - Quaternion(orientation.x, orientation.y, orientation.z, orientation.w), + Vector3(x=center.x, y=center.y, z=center.z), + Quaternion(x=orientation.x, y=orientation.y, z=orientation.z, w=orientation.w), Vector3( - max(float(size.x), 1e-3), - max(float(size.y), 1e-3), - max(float(size.z), 1e-3), + x=max(float(size.x), 1e-3), + y=max(float(size.y), 1e-3), + z=max(float(size.z), 1e-3), ), ) @@ -142,21 +160,38 @@ def to_detection3d_msg(self) -> ROSDetection3D: """Convert to ROS Detection3D message.""" center, orientation, size = self._detection3d_bbox_components() - msg = ROSDetection3D() - msg.header = Header(self.ts, self.frame_id) + msg = ROSDetection3D( + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + results=[], + bbox=BoundingBox3D( + center=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + size=Vector3(x=0.0, y=0.0, z=0.0), + ), + id="", + ) + msg.header = self.pose.header msg.id = self.object_id msg.results = [ ObjectHypothesisWithPose( hypothesis=ObjectHypothesis( class_id=self.name, score=self.confidence, - ) + ), + pose=PoseWithCovariance( + pose=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + covariance=np.zeros(36, dtype=np.float64), + ), ) ] - msg.results_length = len(msg.results) msg.bbox = BoundingBox3D( center=Pose( - position=center, + position=Point(x=center.x, y=center.y, z=center.z), orientation=orientation, ), size=size, @@ -179,6 +214,7 @@ def agent_encode(self) -> dict[str, Any]: def to_dict(self) -> dict[str, Any]: """Convert object to dictionary with all relevant data.""" + colors = pointcloud_rgb(self.pointcloud) return { "object_id": self.object_id, "track_id": self.track_id, @@ -188,8 +224,11 @@ def to_dict(self) -> dict[str, Any]: "identity_basis": self.identity_basis, "last_seen_ts": self.last_seen_ts, "mask": self.mask, - "pointcloud": self.pointcloud.as_numpy(), - "image": self.image.as_numpy() if self.image else None, + "pointcloud": ( + pointcloud_xyz(self.pointcloud), + None if colors is None else colors.astype(np.float64) / 255.0, + ), + "image": image_view(self.image) if self.image else None, } @classmethod @@ -199,7 +238,7 @@ def from_2d_to_list( color_image: Image, depth_image: Image, camera_info: CameraInfo, - camera_transform: Transform | None = None, + camera_transform: TransformStamped | None = None, depth_scale: float = 1.0, depth_trunc: float = 10.0, statistical_nb_neighbors: int = 10, @@ -241,16 +280,13 @@ def from_2d_to_list( import cv2 import open3d as o3d # type: ignore[import-untyped] - color_cv = color_image.to_opencv() - if color_cv.ndim == 3 and color_cv.shape[2] == 3: - color_cv = cv2.cvtColor(color_cv, cv2.COLOR_BGR2RGB) - - depth_cv = depth_image.to_opencv() + color_cv = image_to_rgb(color_image) + depth_cv = image_view(depth_image) h, w = depth_cv.shape[:2] # Build Open3D camera intrinsics - fx, fy = camera_info.K[0], camera_info.K[4] - cx, cy = camera_info.K[2], camera_info.K[5] + fx, fy = camera_info.k[0], camera_info.k[4] + cx, cy = camera_info.k[2], camera_info.k[5] intrinsic_o3d = o3d.camera.PinholeCameraIntrinsic(w, h, fx, fy, cx, cy) objects: list[Object] = [] @@ -289,56 +325,56 @@ def from_2d_to_list( ) pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd, intrinsic_o3d) - pc0 = PointCloud2( - pcd, - frame_id=depth_image.frame_id, - ts=depth_image.ts, - ).voxel_downsample(voxel_downsample) - - pcd_filtered, _ = pc0.pointcloud.remove_statistical_outlier( + initial = pointcloud_from_xyz_rgb( + np.asarray(pcd.points), + (np.asarray(pcd.colors) * 255).astype(np.uint8), + header=depth_image.header, + ) + pc0 = voxel_downsample_cloud(initial, voxel_downsample) + pcd_filtered, _ = pointcloud_to_open3d(pc0).remove_statistical_outlier( nb_neighbors=statistical_nb_neighbors, std_ratio=statistical_std_ratio, ) - if len(pcd_filtered.points) < 10: continue - - pc = PointCloud2( - pcd_filtered, - frame_id=depth_image.frame_id, - ts=depth_image.ts, - ) - - # Transform pointcloud to world frame if camera_transform is provided + header = depth_image.header if camera_transform is not None: - pc = pc.transform(camera_transform) - frame_id = camera_transform.frame_id - else: - frame_id = depth_image.frame_id + if camera_transform.child_frame_id != header.frame_id: + raise ValueError("Camera transform source frame does not match depth image") + pcd_filtered.transform(transform_matrix(camera_transform.transform)) + header = Header( + stamp=depth_image.header.stamp, frame_id=camera_transform.header.frame_id + ) + pc = pointcloud_from_xyz_rgb( + np.asarray(pcd_filtered.points), + (np.asarray(pcd_filtered.colors) * 255).astype(np.uint8), + header=header, + ) + frame_id = header.frame_id # Compute bounding box: AABB for stable upright obstacles, OBB for tighter fit if use_aabb: - aabb = pc.pointcloud.get_axis_aligned_bounding_box() + aabb = pcd_filtered.get_axis_aligned_bounding_box() aabb_center = (aabb.min_bound + aabb.max_bound) / 2.0 aabb_extent = aabb.max_bound - aabb.min_bound - center = Vector3(aabb_center[0], aabb_center[1], aabb_center[2]) + center = Vector3(x=aabb_center[0], y=aabb_center[1], z=aabb_center[2]) sx, sy, sz = float(aabb_extent[0]), float(aabb_extent[1]), float(aabb_extent[2]) - orientation = Quaternion(0.0, 0.0, 0.0, 1.0) + orientation = Quaternion(w=1.0, x=0.0, y=0.0, z=0.0) else: - obb = pc.pointcloud.get_oriented_bounding_box() - center = Vector3(obb.center[0], obb.center[1], obb.center[2]) + obb = pcd_filtered.get_oriented_bounding_box() + center = Vector3(x=obb.center[0], y=obb.center[1], z=obb.center[2]) sx, sy, sz = float(obb.extent[0]), float(obb.extent[1]), float(obb.extent[2]) - orientation = Quaternion.from_rotation_matrix(obb.R) + orientation = quaternion_from_matrix(np.asarray(obb.R)) if max_obstacle_width > 0: sx = min(sx, max_obstacle_width) sy = min(sy, max_obstacle_width) - size = Vector3(sx, sy, sz) + size = Vector3(x=sx, y=sy, z=sz) pose = PoseStamped( - ts=det.ts, - frame_id=frame_id, - position=center, - orientation=orientation, + header=Header(stamp=det.image.header.stamp, frame_id=frame_id), + pose=Pose( + position=Point(x=center.x, y=center.y, z=center.z), orientation=orientation + ), ) # Skip objects too far from origin (background detections) @@ -380,52 +416,38 @@ def aggregate_pointclouds(objects: list[Object]) -> PointCloud2: Returns: Combined PointCloud2 with all points colored by object (empty if no points). """ - import open3d as o3d # type: ignore[import-untyped] - - if not objects: - return PointCloud2(pointcloud=o3d.geometry.PointCloud(), frame_id="", ts=0.0) - + header = ( + Header(stamp=Time(sec=0, nanosec=0), frame_id="") + if not objects + else Header(frame_id=objects[0].frame_id, stamp=objects[0].pointcloud.header.stamp) + ) all_points = [] all_colors = [] - for obj in objects: - points, colors = obj.pointcloud.as_numpy() + if obj.frame_id != header.frame_id: + raise ValueError("Cannot aggregate object clouds in different frames") + points = pointcloud_xyz(obj.pointcloud) if len(points) == 0: continue - + colors = pointcloud_rgb(obj.pointcloud) try: seed = int(obj.object_id, 16) except (ValueError, TypeError): seed = abs(hash(obj.object_id)) rng = np.random.default_rng(abs(seed)) track_color = rng.integers(50, 255, 3) / 255.0 - - if colors is not None: - blended = np.clip(0.6 * colors + 0.4 * track_color, 0.0, 1.0) - else: - blended = np.tile(track_color, (len(points), 1)) - + blended = ( + np.clip(0.6 * colors.astype(np.float64) / 255.0 + 0.4 * track_color, 0.0, 1.0) + if colors is not None + else np.tile(track_color, (len(points), 1)) + ) all_points.append(points) all_colors.append(blended) - if not all_points: - return PointCloud2( - pointcloud=o3d.geometry.PointCloud(), frame_id=objects[0].frame_id, ts=objects[0].ts - ) - - combined_points = np.vstack(all_points) - combined_colors = np.vstack(all_colors) - - pc = PointCloud2.from_numpy( - combined_points, - frame_id=objects[0].frame_id, - timestamp=objects[0].ts, + return pointcloud_from_xyz(np.empty((0, 3), dtype=np.float32), header=header) + return pointcloud_from_xyz_rgb( + np.vstack(all_points), (np.vstack(all_colors) * 255).astype(np.uint8), header=header ) - pcd = pc.pointcloud - pcd.colors = o3d.utility.Vector3dVector(combined_colors) - pc.pointcloud = pcd - - return pc def to_detection3d_array( @@ -448,11 +470,14 @@ def to_detection3d_array( resolved_frame_id = frame_id if resolved_frame_id is None: resolved_frame_id = objects[0].frame_id if objects else "" - resolved_ts = ts - if resolved_ts is None: - resolved_ts = objects[0].ts if objects else 0.0 + stamp = ( + time_from_seconds(ts) + if ts is not None + else objects[0].pose.header.stamp + if objects + else Time(sec=0, nanosec=0) + ) return Detection3DArray( - detections_length=len(detections), - header=Header(resolved_ts, resolved_frame_id), + header=Header(stamp=stamp, frame_id=resolved_frame_id), detections=detections, ) diff --git a/dimos/perception/experimental/objectDB.py b/dimos/perception/experimental/objectDB.py index d2d8e7e1bea..5df1fc432cc 100644 --- a/dimos/perception/experimental/objectDB.py +++ b/dimos/perception/experimental/objectDB.py @@ -14,15 +14,19 @@ from __future__ import annotations +import math import threading import time from typing import TYPE_CHECKING, Any -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +import numpy as np + +from dimos.message_utils.pointcloud import pointcloud_from_xyz +from dimos.message_utils.time import to_nanoseconds from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: - from dimos.msgs.geometry_msgs.Vector3 import Vector3 + from dimos.messages.geometry_msgs.msg import Vector3 from dimos.perception.experimental.object import Object logger = setup_logger() @@ -176,20 +180,19 @@ def find_nearest( if not candidates: return None - return min(candidates, key=lambda obj: position.distance(obj.center)) + return min( + candidates, + key=lambda obj: math.dist( + (position.x, position.y, position.z), (obj.center.x, obj.center.y, obj.center.z) + ), + ) def clear(self) -> None: """Clear all objects from the database.""" - import open3d as o3d # type: ignore[import-untyped] - with self._lock: - # Drop Open3D pointcloud references before clearing to reduce shutdown warnings. + # Release large generated point buffers, retaining metadata on held objects. for obj in list(self._pending_objects.values()) + list(self._objects.values()): - obj.pointcloud = PointCloud2( - pointcloud=o3d.geometry.PointCloud(), - frame_id=obj.pointcloud.frame_id, - ts=obj.pointcloud.ts, - ) + obj.pointcloud = pointcloud_from_xyz(np.empty((0, 3)), header=obj.pointcloud.header) self._pending_objects.clear() self._objects.clear() self._track_id_map.clear() @@ -220,8 +223,6 @@ def _match(self, obj: Object, now: float) -> tuple[Object | None, str | None]: return None, None def _insert_pending(self, obj: Object, now: float) -> Object: - if not obj.ts: - obj.ts = now obj.last_seen_ts = now self._pending_objects[obj.object_id] = obj if obj.track_id >= 0: @@ -234,11 +235,13 @@ def _update_existing(self, existing: Object, obj: Object, now: float) -> bool: self._track_id_map[obj.track_id] = existing.object_id # Multiple prompts or repeated scans may produce the same object from # one camera frame. Only distinct source observations advance memory. - if existing.ts == obj.ts: + if (existing.image.header.frame_id, to_nanoseconds(existing.image.header.stamp)) == ( + obj.image.header.frame_id, + to_nanoseconds(obj.image.header.stamp), + ): return False existing.update_object(obj) - existing.ts = obj.ts or now existing.last_seen_ts = now return True @@ -283,13 +286,22 @@ def _match_by_distance(self, obj: Object) -> Object | None: candidates = [ o for o in all_objects - if o.center is not None and obj.center.distance(o.center) < self._distance_threshold + if o.center is not None + and math.dist( + (obj.center.x, obj.center.y, obj.center.z), (o.center.x, o.center.y, o.center.z) + ) + < self._distance_threshold ] if not candidates: return None - return min(candidates, key=lambda o: obj.center.distance(o.center)) + return min( + candidates, + key=lambda o: math.dist( + (obj.center.x, obj.center.y, obj.center.z), (o.center.x, o.center.y, o.center.z) + ), + ) def _prune_stale_pending(self, now: float) -> None: if self._pending_ttl_s <= 0: diff --git a/dimos/perception/experimental/object_scene_registration.py b/dimos/perception/experimental/object_scene_registration.py index 5e43cb490d0..a02894856df 100644 --- a/dimos/perception/experimental/object_scene_registration.py +++ b/dimos/perception/experimental/object_scene_registration.py @@ -21,21 +21,21 @@ import numpy as np from numpy.typing import NDArray +from reactivex import operators as ops from dimos.agents.annotation import skill from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out +from dimos.message_utils.image import image_from_array, image_view +from dimos.message_utils.pointcloud import pointcloud_from_xyz, transform_cloud +from dimos.message_utils.time import to_seconds +from dimos.messages.geometry_msgs.msg import TransformStamped +from dimos.messages.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.messages.vision_msgs.msg import Detection2DArray, Detection3DArray from dimos.models.segmentation.edge_tam import BoxPromptImageSegmenter from dimos.models.segmentation.yoloe import YoloeBoxSegmenter -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.std_msgs.Header import Header -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray from dimos.perception.detection.detectors.yoloe import Yoloe2DDetector, YoloePromptMode from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.experimental.object import ( @@ -45,13 +45,23 @@ to_detection3d_array, ) from dimos.perception.experimental.objectDB import ObjectDB -from dimos.types.timestamped import align_timestamped +from dimos.types.timestamped import TimestampedData, align_timestamped from dimos.utils.logging_config import setup_logger from dimos.utils.reactive import backpressure logger = setup_logger() +def _timed_image(image: Image) -> TimestampedData[Image]: + return TimestampedData(image, to_seconds(image.header.stamp)) + + +def _aligned_images( + pair: tuple[TimestampedData[Image], TimestampedData[Image]], +) -> tuple[Image, Image]: + return pair[0].value, pair[1].value + + class ObjectSceneRegistrationConfig(ModuleConfig): target_frame: str = "base_link" prompt_mode: YoloePromptMode = YoloePromptMode.LRPC @@ -89,7 +99,7 @@ class ObjectSceneRegistrationModule(Module): _processing_lock: threading.RLock # A tuple assignment/read is atomic, so depth and its transform cannot be # observed from different frames by get_full_scene_pointcloud(). - _latest_scene_snapshot: tuple[Image, Transform | None] | None = None + _latest_scene_snapshot: tuple[Image, TransformStamped | None] | None = None config: ObjectSceneRegistrationConfig @@ -155,11 +165,11 @@ def start(self) -> None: self.camera_info.subscribe(lambda msg: setattr(self, "_camera_info", msg)) aligned_frames = align_timestamped( - self.color_image.observable(), - self.depth_image.observable(), + self.color_image.observable().pipe(ops.map(_timed_image)), + self.depth_image.observable().pipe(ops.map(_timed_image)), buffer_size=2.0, match_tolerance=0.1, - ) + ).pipe(ops.map(_aligned_images)) backpressure(aligned_frames).subscribe(self._on_aligned_frames) def _create_segmenter(self) -> BoxPromptImageSegmenter | None: @@ -229,11 +239,12 @@ def scan_scene(self, text: list[str] | None = None) -> Detection3DArray: if frames is None: return to_detection3d_array([], frame_id=self._target_frame) objects = self._scan_scene_objects(frames) - return to_detection3d_array( + result = to_detection3d_array( objects, frame_id=self._target_frame, - ts=frames[0].ts, ) + result.header.stamp = frames[0].header.stamp + return result def _scan_scene_objects(self, frames: tuple[Image, Image] | None = None) -> list[DetObject]: """Process one frame and return only objects observed during this scan.""" @@ -275,7 +286,7 @@ def get_object_pointcloud_by_object_id(self, object_id: str) -> PointCloud2 | No logger.warning(f"No object found with object_id='{object_id}'") return None pc = obj.pointcloud - num_points = len(pc.pointcloud.points) if pc else 0 + num_points = pc.width * pc.height logger.info(f"Found object '{object_id}' ({obj.name}) with {num_points} points") return pc @@ -313,7 +324,7 @@ def get_full_scene_pointcloud( return None depth_image, camera_transform = scene_snapshot - depth_cv = depth_image.to_opencv() + depth_cv = image_view(depth_image) h, w = depth_cv.shape[:2] # Zero out excluded object's depth @@ -324,8 +335,8 @@ def get_full_scene_pointcloud( depth_cv[exclude_mask > 0] = 0 # Build pointcloud from depth - fx, fy = self._camera_info.K[0], self._camera_info.K[4] - cx, cy = self._camera_info.K[2], self._camera_info.K[5] + fx, fy = self._camera_info.k[0], self._camera_info.k[4] + cx, cy = self._camera_info.k[2], self._camera_info.k[5] intrinsic = o3d.camera.PinholeCameraIntrinsic(w, h, fx, fy, cx, cy) depth_o3d = o3d.geometry.Image(depth_cv.astype(np.float32)) @@ -338,14 +349,9 @@ def get_full_scene_pointcloud( pcd = pcd.voxel_down_sample(voxel_size) - pc = PointCloud2( - pcd, - frame_id=depth_image.frame_id, - ts=depth_image.ts, - ) - + pc = pointcloud_from_xyz(np.asarray(pcd.points), header=depth_image.header) if camera_transform is not None: - pc = pc.transform(camera_transform) + pc = transform_cloud(pc, camera_transform) return pc @@ -412,21 +418,19 @@ def _process_images(self, color_msg: Image, depth_msg: Image) -> list[DetObject] color_image = color_msg # Convert depth to meters (float32) - depth_cv = depth_msg.to_opencv() - if depth_msg.format == ImageFormat.DEPTH16: + depth_cv = image_view(depth_msg) + if depth_msg.encoding == "16UC1": depth_cv = depth_cv.astype(np.float32) / 1000.0 elif depth_cv.dtype != np.float32: depth_cv = depth_cv.astype(np.float32) - depth_image = Image( - data=depth_cv, format=ImageFormat.DEPTH, frame_id=depth_msg.frame_id, ts=depth_msg.ts - ) + depth_image = image_from_array(depth_cv, encoding="32FC1", header=depth_msg.header) camera_transform = None - if self._target_frame != color_image.frame_id: + if self._target_frame != color_image.header.frame_id: camera_transform = self.tfbuffer.get( self._target_frame, - color_image.frame_id, - color_image.ts, + color_image.header.frame_id, + to_seconds(color_image.header.stamp), 0.1, forward_tolerance=0.2, ) @@ -457,8 +461,7 @@ def _process_images(self, color_msg: Image, depth_msg: Image) -> list[DetObject] detections_2d = self._segmenter.segment(detections_2d) detections_2d_msg = Detection2DArray( - detections_length=len(detections_2d.detections), - header=Header(color_image.ts, color_image.frame_id or ""), + header=color_image.header, detections=[det.to_ros_detection2d() for det in detections_2d.detections], ) self.detections_2d.publish(detections_2d_msg) @@ -473,13 +476,13 @@ def _process_3d_detections( detections_2d: ImageDetections2D[Any], color_image: Image, depth_image: Image, - camera_transform: Transform | None, + camera_transform: TransformStamped | None, ) -> list[DetObject]: """Convert 2D detections to 3D and publish.""" if self._camera_info is None: return [] - if self._target_frame != color_image.frame_id and camera_transform is None: + if self._target_frame != color_image.header.frame_id and camera_transform is None: logger.warning("Failed to lookup transform from camera frame to target frame") return [] diff --git a/dimos/perception/experimental/object_scene_registration_spec.py b/dimos/perception/experimental/object_scene_registration_spec.py index 5f0cbbf8e1b..a082529e424 100644 --- a/dimos/perception/experimental/object_scene_registration_spec.py +++ b/dimos/perception/experimental/object_scene_registration_spec.py @@ -14,8 +14,8 @@ from typing import Protocol -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.messages.sensor_msgs.msg import PointCloud2 +from dimos.messages.vision_msgs.msg import Detection3DArray from dimos.spec.utils import Spec diff --git a/dimos/perception/experimental/object_tracker.py b/dimos/perception/experimental/object_tracker.py index 2331dbfd8ca..dceb5ebbbb7 100644 --- a/dimos/perception/experimental/object_tracker.py +++ b/dimos/perception/experimental/object_tracker.py @@ -12,41 +12,57 @@ # See the License for the specific language governing permissions and # limitations under the License. +import math import threading import time from typing import Any -# Import LCM messages -from dimos_lcm.vision_msgs import ( - Detection2D, - Detection3D, - ObjectHypothesisWithPose, -) import numpy as np from numpy.typing import NDArray +from reactivex import operators as ops from reactivex.disposable import Disposable from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.Pose import Pose -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.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.std_msgs.Header import Header -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray -from dimos.types.timestamped import align_timestamped -from dimos.utils.logging_config import setup_logger -from dimos.utils.transform_utils import ( - euler_to_quaternion, - optical_to_robot_frame, - yaw_towards_point, +from dimos.message_utils.geometry import quaternion_from_euler +from dimos.message_utils.image import image_from_array, image_to_rgb, image_view +from dimos.message_utils.time import time_from_seconds, to_seconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import ( + Point, + Pose, + PoseWithCovariance, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos.messages.sensor_msgs.msg import CameraInfo, Image +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage + +# Generated detection values +from dimos.messages.vision_msgs.msg import ( + BoundingBox2D, + BoundingBox3D, + Detection2D, + Detection2DArray, + Detection3D, + Detection3DArray, + ObjectHypothesis, + ObjectHypothesisWithPose, + Point2D, + Pose2D, ) +from dimos.types.timestamped import TimestampedData, align_timestamped +from dimos.utils.logging_config import setup_logger + + +def _timed_image(image: Image) -> TimestampedData[Image]: + return TimestampedData(image, to_seconds(image.header.stamp)) + logger = setup_logger() @@ -128,20 +144,20 @@ def start(self) -> None: # Subscribe to aligned rgb and depth streams def on_aligned_frames(frames_tuple) -> None: # type: ignore[no-untyped-def] - rgb_msg, depth_msg = frames_tuple + rgb_msg, depth_msg = (item.value for item in frames_tuple) with self._frame_lock: - self._latest_rgb_frame = rgb_msg.data + self._latest_rgb_frame = image_to_rgb(rgb_msg) - depth_data = depth_msg.data + depth_data = image_view(depth_msg) # Convert from millimeters to meters if depth is DEPTH16 format - if depth_msg.format == ImageFormat.DEPTH16: + if depth_msg.encoding in ("16UC1", "mono16"): depth_data = depth_data.astype(np.float32) / 1000.0 self._latest_depth_frame = depth_data # Create aligned observable for RGB and depth aligned_frames = align_timestamped( - self.color_image.observable(), - self.depth.observable(), + self.color_image.pure_observable().pipe(ops.map(_timed_image)), + self.depth.pure_observable().pipe(ops.map(_timed_image)), buffer_size=2.0, # 2 second buffer match_tolerance=0.5, # 500ms tolerance ) @@ -154,10 +170,10 @@ def on_camera_info(camera_info_msg: CameraInfo) -> None: # Extract intrinsics from camera info K matrix # K is a 3x3 matrix in row-major order: [fx, 0, cx, 0, fy, cy, 0, 0, 1] self.camera_intrinsics = [ # type: ignore[assignment] - camera_info_msg.K[0], - camera_info_msg.K[4], - camera_info_msg.K[2], - camera_info_msg.K[5], + camera_info_msg.k[0], + camera_info_msg.k[4], + camera_info_msg.k[2], + camera_info_msg.k[5], ] unsub = self.camera_info.subscribe(on_camera_info) # type: ignore[assignment] @@ -309,8 +325,12 @@ def _reset_tracking_state(self) -> None: self.tracking_frame_count = 0 # Reset frame counter # Publish empty detections to clear any visualizations - empty_2d = Detection2DArray(detections_length=0, header=Header(), detections=[]) - empty_3d = Detection3DArray(detections_length=0, header=Header(), detections=[]) + empty_2d = Detection2DArray( + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), detections=[] + ) + empty_3d = Detection3DArray( + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), detections=[] + ) self._latest_detection2d = empty_2d self._latest_detection3d = empty_3d self._detection_event.clear() @@ -409,9 +429,9 @@ def _process_tracking(self) -> None: return # Create detections if tracking succeeded - header = Header(self.frame_id) - detection2darray = Detection2DArray(detections_length=0, header=header, detections=[]) - detection3darray = Detection3DArray(detections_length=0, header=header, detections=[]) + header = Header(stamp=time_from_seconds(time.time()), frame_id=self.config.frame_id) + detection2darray = Detection2DArray(header=header, detections=[]) + detection3darray = Detection3DArray(header=header, detections=[]) if final_success and current_bbox_x1y1x2y2 is not None: x1, y1, x2, y2 = current_bbox_x1y1x2y2 @@ -421,13 +441,28 @@ def _process_tracking(self) -> None: height = float(y2 - y1) # Create Detection2D - detection_2d = Detection2D() + detection_2d = Detection2D( + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + results=[], + bbox=BoundingBox2D( + center=Pose2D(position=Point2D(x=0.0, y=0.0), theta=0.0), size_x=0.0, size_y=0.0 + ), + id="", + ) detection_2d.id = "0" - detection_2d.results_length = 1 detection_2d.header = header # Create hypothesis - hypothesis = ObjectHypothesisWithPose() + hypothesis = ObjectHypothesisWithPose( + hypothesis=ObjectHypothesis(class_id="", score=0.0), + pose=PoseWithCovariance( + pose=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + covariance=np.zeros(36, dtype=np.float64), + ), + ) hypothesis.hypothesis.class_id = "tracked_object" hypothesis.hypothesis.score = 1.0 detection_2d.results = [hypothesis] @@ -439,8 +474,9 @@ def _process_tracking(self) -> None: detection_2d.bbox.size_x = width detection_2d.bbox.size_y = height - detection2darray = Detection2DArray() - detection2darray.detections_length = 1 + detection2darray = Detection2DArray( + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), detections=[] + ) detection2darray.header = header detection2darray.detections = [detection_2d] @@ -460,18 +496,12 @@ def _process_tracking(self) -> None: x_optical = (center_x - cx) * z_optical / fx y_optical = (center_y - cy) * z_optical / fy - # Create pose in optical frame - optical_pose = Pose() - optical_pose.position = Vector3(x_optical, y_optical, z_optical) - optical_pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0) # Identity for now - - # Convert to robot frame - robot_pose = optical_to_robot_frame(optical_pose) - - # Calculate orientation: object facing towards camera (origin) - yaw = yaw_towards_point(robot_pose.position) - euler = Vector3(0.0, 0.0, yaw) # Only yaw, no roll/pitch - robot_pose.orientation = euler_to_quaternion(euler) + # Optical (right, down, forward) -> robot (forward, left, up). + position = Point(x=z_optical, y=-x_optical, z=-y_optical) + robot_pose = Pose( + position=position, + orientation=quaternion_from_euler(0, 0, math.atan2(position.y, position.x)), + ) # Estimate object size in meters size_x = width * z_optical / fx @@ -479,42 +509,61 @@ def _process_tracking(self) -> None: size_z = 0.1 # Default depth size # Create Detection3D - detection_3d = Detection3D() + detection_3d = Detection3D( + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + results=[], + bbox=BoundingBox3D( + center=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + size=Vector3(x=0.0, y=0.0, z=0.0), + ), + id="", + ) detection_3d.id = "0" - detection_3d.results_length = 1 detection_3d.header = header # Reuse hypothesis from 2D detection_3d.results = [hypothesis] # Create 3D bounding box with robot frame pose - detection_3d.bbox.center = Pose() + detection_3d.bbox.center = Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ) detection_3d.bbox.center.position = robot_pose.position detection_3d.bbox.center.orientation = robot_pose.orientation - detection_3d.bbox.size = Vector3(size_x, size_y, size_z) + detection_3d.bbox.size = Vector3(x=size_x, y=size_y, z=size_z) - detection3darray = Detection3DArray() - detection3darray.detections_length = 1 + detection3darray = Detection3DArray( + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), detections=[] + ) detection3darray.header = header detection3darray.detections = [detection_3d] # Publish transform for tracked object # The optical pose is in camera optical frame, so publish it relative to the camera frame - tracked_object_tf = Transform( - translation=robot_pose.position, - rotation=robot_pose.orientation, - frame_id=self.frame_id, # Use configured camera frame + tracked_object_tf = TransformStamped( + header=header, child_frame_id="tracked_object", - ts=header.ts, + transform=Transform( + translation=Vector3( + x=robot_pose.position.x, + y=robot_pose.position.y, + z=robot_pose.position.z, + ), + rotation=robot_pose.orientation, + ), ) - self.tf.publish(TFMessage(tracked_object_tf)) + self.tf.publish(TFMessage(transforms=[tracked_object_tf])) # Store latest detections for RPC access self._latest_detection2d = detection2darray self._latest_detection3d = detection3darray # Signal that new detections are available - if detection2darray.detections_length > 0 or detection3darray.detections_length > 0: + if bool(detection2darray.detections) or bool(detection3darray.detections): self._detection_event.set() # Publish detections @@ -524,12 +573,8 @@ def _process_tracking(self) -> None: # Create and publish visualization if tracking is active if self.tracking_initialized: # Convert single detection to list for visualization - detections_3d = ( - detection3darray.detections if detection3darray.detections_length > 0 else [] - ) - detections_2d = ( - detection2darray.detections if detection2darray.detections_length > 0 else [] - ) + detections_3d = detection3darray.detections if bool(detection3darray.detections) else [] + detections_2d = detection2darray.detections if bool(detection2darray.detections) else [] if detections_3d and detections_2d: # Extract 2D bbox for visualization @@ -555,7 +600,9 @@ def _process_tracking(self) -> None: viz_image = self._draw_reid_matches(viz_image) # Convert to Image message and publish - viz_msg = Image.from_numpy(viz_image) + viz_msg = image_from_array( + viz_image, encoding="rgb8", header=self._latest_detection2d.header + ) self.tracked_overlay.publish(viz_msg) def _draw_reid_matches(self, image: NDArray[np.uint8]) -> NDArray[np.uint8]: diff --git a/dimos/perception/experimental/object_tracker_2d.py b/dimos/perception/experimental/object_tracker_2d.py index 385d0ff98d8..86d1aa20fa0 100644 --- a/dimos/perception/experimental/object_tracker_2d.py +++ b/dimos/perception/experimental/object_tracker_2d.py @@ -17,15 +17,6 @@ import time from typing import Any -# Import LCM messages -from dimos_lcm.vision_msgs import ( - BoundingBox2D, - Detection2D, - ObjectHypothesis, - ObjectHypothesisWithPose, - Point2D, - Pose2D, -) import numpy as np from numpy.typing import NDArray from reactivex.disposable import Disposable @@ -34,9 +25,22 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.std_msgs.Header import Header -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray +from dimos.message_utils.image import image_from_array, image_to_rgb +from dimos.message_utils.time import time_from_seconds +from dimos.messages.geometry_msgs.msg import Point, Pose, PoseWithCovariance, Quaternion +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.std_msgs.msg import Header + +# Generated detection values +from dimos.messages.vision_msgs.msg import ( + BoundingBox2D, + Detection2D, + Detection2DArray, + ObjectHypothesis, + ObjectHypothesisWithPose, + Point2D, + Pose2D, +) from dimos.utils.logging_config import setup_logger logger = setup_logger(level=logging.INFO) @@ -91,7 +95,7 @@ def start(self) -> None: def on_frame(frame_msg: Image) -> None: arrival_time = time.perf_counter() with self._frame_lock: - self._latest_rgb_frame = frame_msg.data + self._latest_rgb_frame = image_to_rgb(frame_msg) self._frame_arrival_time = arrival_time unsub = self.color_image.subscribe(on_frame) @@ -176,7 +180,8 @@ def _reset_tracking_state(self) -> None: # Publish empty detection empty_2d = Detection2DArray( - detections_length=0, header=Header(time.time(), self.frame_id), detections=[] + header=Header(stamp=time_from_seconds(time.time()), frame_id=self.config.frame_id), + detections=[], ) self._latest_detection2d = empty_2d self.detection2darray.publish(empty_2d) @@ -260,12 +265,11 @@ def _process_tracking(self) -> None: height = float(y2 - y1) # Create 2D detection header - header = Header(time.time(), self.frame_id) + header = Header(stamp=time_from_seconds(time.time()), frame_id=self.config.frame_id) # Create Detection2D with all fields in constructors detection_2d = Detection2D( id="0", - results_length=1, header=header, bbox=BoundingBox2D( center=Pose2D(position=Point2D(x=center_x, y=center_y), theta=0.0), @@ -274,14 +278,19 @@ def _process_tracking(self) -> None: ), results=[ ObjectHypothesisWithPose( - hypothesis=ObjectHypothesis(class_id="tracked_object", score=1.0) + hypothesis=ObjectHypothesis(class_id="tracked_object", score=1.0), + pose=PoseWithCovariance( + pose=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + covariance=np.zeros(36, dtype=np.float64), + ), ) ], ) - detection2darray = Detection2DArray( - detections_length=1, header=header, detections=[detection_2d] - ) + detection2darray = Detection2DArray(header=header, detections=[detection_2d]) # Store and publish self._latest_detection2d = detection2darray @@ -290,7 +299,7 @@ def _process_tracking(self) -> None: # Create visualization viz_image = self._draw_visualization(frame, current_bbox_x1y1x2y2) viz_copy = viz_image.copy() # Force copy needed to prevent frame reuse - viz_msg = Image.from_numpy(viz_copy, format=ImageFormat.RGB) + viz_msg = image_from_array(viz_copy, encoding="rgb8", header=header) self.tracked_overlay.publish(viz_msg) def _draw_visualization(self, image: NDArray[np.uint8], bbox: list[int]) -> NDArray[np.uint8]: diff --git a/dimos/perception/experimental/object_tracker_3d.py b/dimos/perception/experimental/object_tracker_3d.py index f2f5a116b10..78ec1968eb8 100644 --- a/dimos/perception/experimental/object_tracker_3d.py +++ b/dimos/perception/experimental/object_tracker_3d.py @@ -13,33 +13,47 @@ # limitations under the License. -# Import LCM messages -from dimos_lcm.sensor_msgs import CameraInfo -from dimos_lcm.vision_msgs import ( - Detection3D, - ObjectHypothesisWithPose, -) +import math + import numpy as np +from reactivex import operators as ops from dimos.core.core import rpc from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.Pose import Pose -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.Image import Image, ImageFormat -from dimos.msgs.std_msgs.Header import Header -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.message_utils.geometry import quaternion_from_euler +from dimos.message_utils.image import image_from_array, image_to_rgb, image_view +from dimos.message_utils.time import to_seconds + +# Generated detection values +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import ( + Point, + Pose, + PoseWithCovariance, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos.messages.sensor_msgs.msg import CameraInfo, Image +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.messages.vision_msgs.msg import ( + BoundingBox3D, + Detection2DArray, + Detection3D, + Detection3DArray, + ObjectHypothesis, + ObjectHypothesisWithPose, +) from dimos.perception.experimental.object_tracker_2d import ObjectTracker2D -from dimos.types.timestamped import align_timestamped +from dimos.types.timestamped import TimestampedData, align_timestamped from dimos.utils.logging_config import setup_logger -from dimos.utils.transform_utils import ( - euler_to_quaternion, - optical_to_robot_frame, - yaw_towards_point, -) + + +def _timed_image(image: Image) -> TimestampedData[Image]: + return TimestampedData(image, to_seconds(image.header.stamp)) + logger = setup_logger() @@ -78,20 +92,20 @@ def start(self) -> None: # Subscribe to aligned RGB and depth streams def on_aligned_frames(frames_tuple) -> None: # type: ignore[no-untyped-def] - rgb_msg, depth_msg = frames_tuple + rgb_msg, depth_msg = (item.value for item in frames_tuple) with self._frame_lock: - self._latest_rgb_frame = rgb_msg.data + self._latest_rgb_frame = image_to_rgb(rgb_msg) - depth_data = depth_msg.data + depth_data = image_view(depth_msg) # Convert from millimeters to meters if depth is DEPTH16 format - if depth_msg.format == ImageFormat.DEPTH16: + if depth_msg.encoding in ("16UC1", "mono16"): depth_data = depth_data.astype(np.float32) / 1000.0 self._latest_depth_frame = depth_data # Create aligned observable for RGB and depth aligned_frames = align_timestamped( - self.color_image.observable(), - self.depth.observable(), + self.color_image.pure_observable().pipe(ops.map(_timed_image)), + self.depth.pure_observable().pipe(ops.map(_timed_image)), buffer_size=2.0, # 2 second buffer match_tolerance=0.5, # 500ms tolerance ) @@ -103,10 +117,10 @@ def on_camera_info(camera_info_msg: CameraInfo) -> None: self._latest_camera_info = camera_info_msg # Extract intrinsics: K is [fx, 0, cx, 0, fy, cy, 0, 0, 1] self.camera_intrinsics = [ # type: ignore[assignment] - camera_info_msg.K[0], - camera_info_msg.K[4], - camera_info_msg.K[2], - camera_info_msg.K[5], + camera_info_msg.k[0], + camera_info_msg.k[4], + camera_info_msg.k[2], + camera_info_msg.k[5], ] self.camera_info.subscribe(on_camera_info) @@ -127,7 +141,7 @@ def _process_tracking(self) -> None: # Enhance with 3D if we have depth and a valid 2D detection if ( self._latest_detection2d - and self._latest_detection2d.detections_length > 0 + and bool(self._latest_detection2d.detections) and self._latest_depth_frame is not None and self.camera_intrinsics is not None ): @@ -161,12 +175,14 @@ def _process_tracking(self) -> None: if self.last_good_matches and self.last_roi_kps and self.last_roi_bbox: viz_image = self._draw_reid_overlay(viz_image) - viz_msg = Image.from_numpy(viz_image) + viz_msg = image_from_array( + viz_image, encoding="rgb8", header=self._latest_detection2d.header + ) self.tracked_overlay.publish(viz_msg) def _create_detection3d_from_2d(self, detection2d: Detection2DArray) -> Detection3DArray | None: """Create 3D detection from 2D detection using depth.""" - if detection2d.detections_length == 0: + if not detection2d.detections: return None det_2d = detection2d.detections[0] @@ -196,18 +212,12 @@ def _create_detection3d_from_2d(self, detection2d: Detection2DArray) -> Detectio x_optical = (center_x - cx) * z_optical / fx # type: ignore[has-type] y_optical = (center_y - cy) * z_optical / fy # type: ignore[has-type] - # Create pose in optical frame - optical_pose = Pose() - optical_pose.position = Vector3(x_optical, y_optical, z_optical) - optical_pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0) - - # Convert to robot frame - robot_pose = optical_to_robot_frame(optical_pose) - - # Calculate orientation: object facing towards camera - yaw = yaw_towards_point(robot_pose.position) - euler = Vector3(0.0, 0.0, yaw) - robot_pose.orientation = euler_to_quaternion(euler) + # Optical (right, down, forward) -> robot (forward, left, up). + position = Point(x=z_optical, y=-x_optical, z=-y_optical) + robot_pose = Pose( + position=position, + orientation=quaternion_from_euler(0, 0, math.atan2(position.y, position.x)), + ) # Estimate object size in meters size_x = width * z_optical / fx # type: ignore[has-type] @@ -215,38 +225,63 @@ def _create_detection3d_from_2d(self, detection2d: Detection2DArray) -> Detectio size_z = 0.1 # Default depth size # Create Detection3D - header = Header(self.frame_id) - detection_3d = Detection3D() + header = Header(stamp=detection2d.header.stamp, frame_id=self.config.frame_id) + detection_3d = Detection3D( + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + results=[], + bbox=BoundingBox3D( + center=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + size=Vector3(x=0.0, y=0.0, z=0.0), + ), + id="", + ) detection_3d.id = "0" - detection_3d.results_length = 1 detection_3d.header = header # Create hypothesis - hypothesis = ObjectHypothesisWithPose() + hypothesis = ObjectHypothesisWithPose( + hypothesis=ObjectHypothesis(class_id="", score=0.0), + pose=PoseWithCovariance( + pose=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + covariance=np.zeros(36, dtype=np.float64), + ), + ) hypothesis.hypothesis.class_id = "tracked_object" hypothesis.hypothesis.score = 1.0 detection_3d.results = [hypothesis] # Create 3D bounding box - detection_3d.bbox.center = Pose() + detection_3d.bbox.center = Pose( + position=Point(x=0.0, y=0.0, z=0.0), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ) detection_3d.bbox.center.position = robot_pose.position detection_3d.bbox.center.orientation = robot_pose.orientation - detection_3d.bbox.size = Vector3(size_x, size_y, size_z) + detection_3d.bbox.size = Vector3(x=size_x, y=size_y, z=size_z) - detection3darray = Detection3DArray() - detection3darray.detections_length = 1 + detection3darray = Detection3DArray( + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), detections=[] + ) detection3darray.header = header detection3darray.detections = [detection_3d] # Publish TF for tracked object - tracked_object_tf = Transform( - translation=robot_pose.position, - rotation=robot_pose.orientation, - frame_id=self.frame_id, + tracked_object_tf = TransformStamped( + header=header, child_frame_id="tracked_object", - ts=header.ts, + transform=Transform( + translation=Vector3( + x=robot_pose.position.x, y=robot_pose.position.y, z=robot_pose.position.z + ), + rotation=robot_pose.orientation, + ), ) - self.tf.publish(TFMessage(tracked_object_tf)) + self.tf.publish(TFMessage(transforms=[tracked_object_tf])) return detection3darray diff --git a/dimos/perception/experimental/perceive_loop_skill.py b/dimos/perception/experimental/perceive_loop_skill.py index 532555d9943..ced12861ba1 100644 --- a/dimos/perception/experimental/perceive_loop_skill.py +++ b/dimos/perception/experimental/perceive_loop_skill.py @@ -14,20 +14,28 @@ from __future__ import annotations +import base64 from datetime import datetime, timezone import json import os from threading import RLock from typing import TYPE_CHECKING, Any +import reactivex as rx +from reactivex import Observable, operators as ops +from reactivex.abc import SchedulerBase + from dimos.agents.agent_spec import AgentSpec from dimos.agents.annotation import skill from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In +from dimos.message_utils.image import image_sharpness, image_to_bgr, image_to_jpeg +from dimos.message_utils.time import to_seconds +from dimos.messages.sensor_msgs.msg import Image from dimos.models.vl.create import create from dimos.models.vl.types import VlModelName -from dimos.msgs.sensor_msgs.Image import Image, sharpness_window +from dimos.types.timestamped import TimestampedBufferCollection, TimestampedData from dimos.utils.logging_config import setup_logger from dimos.utils.reactive import backpressure from dimos.utils.threadpool import run_in_thread @@ -42,6 +50,25 @@ logger = setup_logger() +def _sharpness_window( + frequency: float, source: Observable[Image], *, scheduler: SchedulerBase | None = None +) -> Observable[Image]: + """Keep the existing source-time rolling selection and repeated timer emissions.""" + if frequency <= 0: + raise ValueError("frequency must be positive") + window: TimestampedBufferCollection[TimestampedData[Image]] = TimestampedBufferCollection( + 1 / frequency + ) + + def remember(image: Image) -> None: + window.add(TimestampedData(image, to_seconds(image.header.stamp))) + + return rx.interval(1 / frequency, scheduler=scheduler).pipe( + ops.with_latest_from(source.pipe(ops.do_action(remember))), + ops.map(lambda _: max(window, key=lambda item: image_sharpness(item.value)).value), + ) + + class PerceiveLoopSkillConfig(ModuleConfig): detection_model: VlModelName = "moondream" @@ -120,7 +147,7 @@ def look_out_for( model = self._vl_model.result() sharpest = backpressure( - sharpness_window(1.0 / self._period, self.color_image.pure_observable()) + _sharpness_window(1.0 / self._period, self.color_image.pure_observable()) ) model.start() self._model_started = True @@ -187,7 +214,7 @@ def _on_image(self, image: Image) -> None: continuation_context: dict[str, Any] = { "bbox": list(best.bbox), "label": best.name, - "image": image.to_base64(quality=70), + "image": base64.b64encode(image_to_jpeg(image, quality=70)).decode("ascii"), } logger.info( "Lookout matched, dispatching continuation", @@ -214,7 +241,7 @@ def _write_debug_image(image: Image, detections: ImageDetections2D[Detection2DBB import cv2 try: - debug_img = image.to_opencv().copy() + debug_img = image_to_bgr(image).copy() for det in detections.detections: x1, y1, x2, y2 = (int(v) for v in det.bbox) cv2.rectangle(debug_img, (x1, y1), (x2, y2), (0, 255, 0), 2) diff --git a/dimos/perception/experimental/spatial_perception.py b/dimos/perception/experimental/spatial_perception.py index 54f15cbd7b4..21849b83717 100644 --- a/dimos/perception/experimental/spatial_perception.py +++ b/dimos/perception/experimental/spatial_perception.py @@ -19,7 +19,7 @@ from datetime import datetime import os import time -from typing import TYPE_CHECKING, Any +from typing import Any import uuid import numpy as np @@ -30,17 +30,17 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.message_utils.geometry import pose_from_transform, quaternion_euler +from dimos.message_utils.image import image_to_bgr +from dimos.messages.geometry_msgs.msg import Point +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.tf2_msgs.msg import TFMessage from dimos.perception.experimental.image_embedding import ImageEmbeddingProvider from dimos.perception.experimental.spatial_vector_db import SpatialVectorDB from dimos.perception.experimental.visual_memory import VisualMemory from dimos.types.robot_location import RobotLocation from dimos.utils.logging_config import setup_logger -if TYPE_CHECKING: - from dimos.msgs.geometry_msgs.Vector3 import Vector3 - _OUTPUT_DIR = DIMOS_PROJECT_ROOT / "assets" / "output" _MEMORY_DIR = _OUTPUT_DIR / "memory" _SPATIAL_MEMORY_DIR = _MEMORY_DIR / "spatial_memory" @@ -154,7 +154,7 @@ def __init__(self, **kwargs: Any) -> None: embedding_provider=self.embedding_provider, ) - self.last_position: Vector3 | None = None + self.last_position: Point | None = None self.last_record_time: float | None = None self.frame_count: int = 0 @@ -167,23 +167,14 @@ def __init__(self, **kwargs: Any) -> None: self._latest_video_frame: np.ndarray | None = None self._process_interval = 1 + def _receive_frame(self, image_msg: Image) -> None: + self._latest_video_frame = image_to_bgr(image_msg) + @rpc def start(self) -> None: - import cv2 - super().start() - # Subscribe to LCM streams - def set_video(image_msg: Image) -> None: - # Convert Image message to numpy array - if hasattr(image_msg, "data"): - frame = image_msg.data - frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR) - self._latest_video_frame = frame - else: - logger.warning("Received image message without data attribute") - - self.register_disposable(Disposable(self.color_image.subscribe(set_video))) + self.register_disposable(Disposable(self.color_image.subscribe(self._receive_frame))) # Start periodic processing using interval self.register_disposable( @@ -211,7 +202,7 @@ def _process_frame(self) -> None: return # Create Pose object with position and orientation - current_pose = tf.to_pose() + current_pose = pose_from_transform(tf).pose # Process the frame directly try: @@ -242,16 +233,16 @@ def _process_frame(self) -> None: frame_id = f"frame_{datetime.now().strftime('%Y%m%d_%H%M%S')}_{uuid.uuid4().hex[:8]}" # Get euler angles from quaternion orientation for metadata - euler = tf.rotation.to_euler() + roll, pitch, yaw = quaternion_euler(tf.transform.rotation) # Create metadata dictionary with primitive types only metadata = { "pos_x": float(current_pose.position.x), "pos_y": float(current_pose.position.y), "pos_z": float(current_pose.position.z), - "rot_x": float(euler.x), - "rot_y": float(euler.y), - "rot_z": float(euler.z), + "rot_x": roll, + "rot_y": pitch, + "rot_z": yaw, "timestamp": current_time, "frame_id": frame_id, } @@ -271,7 +262,7 @@ def _process_frame(self) -> None: logger.info( f"Stored frame at position ({current_pose.position.x:.2f}, {current_pose.position.y:.2f}, {current_pose.position.z:.2f}), " - f"rotation ({euler.x:.2f}, {euler.y:.2f}, {euler.z:.2f}) " + f"rotation ({roll:.2f}, {pitch:.2f}, {yaw:.2f}) " f"stored {self.stored_frame_count}/{self.frame_count} frames" ) @@ -374,7 +365,7 @@ def process_combined_data(data): # type: ignore[no-untyped-def] vector_id=frame_id, image=frame, embedding=frame_embedding, metadata=metadata ) - self.last_position = position_v3 + self.last_position = Point(x=position_v3.x, y=position_v3.y, z=position_v3.z) self.last_record_time = current_time self.stored_frame_count += 1 @@ -477,11 +468,12 @@ def add_named_location( return False # Create RobotLocation object - location = RobotLocation( # type: ignore[call-arg] + translation = tf.transform.translation + location = RobotLocation( name=name, - position=tf.translation, - rotation=tf.rotation.to_euler(), - description=description or f"Location: {name}", + position=(translation.x, translation.y, translation.z), + rotation=quaternion_euler(tf.transform.rotation), + metadata={"description": description or f"Location: {name}"}, timestamp=time.time(), ) diff --git a/dimos/perception/experimental/temporal_memory/clip_filter.py b/dimos/perception/experimental/temporal_memory/clip_filter.py index cb89c31c55d..5f6401f13f3 100644 --- a/dimos/perception/experimental/temporal_memory/clip_filter.py +++ b/dimos/perception/experimental/temporal_memory/clip_filter.py @@ -18,7 +18,8 @@ import numpy as np -from dimos.msgs.sensor_msgs.Image import Image +from dimos.message_utils.image import image_to_rgb +from dimos.messages.sensor_msgs.msg import Image from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -36,9 +37,7 @@ def _get_image_data(image: Image) -> np.ndarray[Any, Any]: """Extract numpy array from Image.""" - if not hasattr(image, "data"): - raise AttributeError(f"Image missing .data attribute: {type(image)}") - return image.data + return image_to_rgb(image) if CLIP_AVAILABLE: diff --git a/dimos/perception/experimental/temporal_memory/entity_graph_db.py b/dimos/perception/experimental/temporal_memory/entity_graph_db.py index 54f188034a4..7dc8ab8890f 100644 --- a/dimos/perception/experimental/temporal_memory/entity_graph_db.py +++ b/dimos/perception/experimental/temporal_memory/entity_graph_db.py @@ -37,8 +37,8 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: + from dimos.messages.sensor_msgs.msg import Image from dimos.models.vl.base import VlModel - from dimos.msgs.sensor_msgs.Image import Image logger = setup_logger() diff --git a/dimos/perception/experimental/temporal_memory/frame_window_accumulator.py b/dimos/perception/experimental/temporal_memory/frame_window_accumulator.py index 4c910a1b88a..6493853fca7 100644 --- a/dimos/perception/experimental/temporal_memory/frame_window_accumulator.py +++ b/dimos/perception/experimental/temporal_memory/frame_window_accumulator.py @@ -24,8 +24,10 @@ import threading from typing import TYPE_CHECKING +from dimos.message_utils.time import to_seconds + if TYPE_CHECKING: - from dimos.msgs.sensor_msgs.Image import Image + from dimos.messages.sensor_msgs.msg import Image @dataclass @@ -87,10 +89,7 @@ def add_frame(self, image: Image, wall_time: float) -> None: with self._lock: if self._video_start_wall_time is None: return - if image.ts is not None: - timestamp_s = image.ts - self._video_start_wall_time - else: - timestamp_s = wall_time - self._video_start_wall_time + timestamp_s = to_seconds(image.header.stamp) - self._video_start_wall_time frame = Frame( frame_index=self._frame_count, timestamp_s=timestamp_s, diff --git a/dimos/perception/experimental/temporal_memory/temporal_memory.py b/dimos/perception/experimental/temporal_memory/temporal_memory.py index dd8c10084a2..55570331004 100644 --- a/dimos/perception/experimental/temporal_memory/temporal_memory.py +++ b/dimos/perception/experimental/temporal_memory/temporal_memory.py @@ -37,10 +37,12 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out +from dimos.message_utils.image import image_sharpness +from dimos.message_utils.time import header_now +from dimos.messages.dimos_msgs.msg import EntityMarker, EntityMarkers +from dimos.messages.geometry_msgs.msg import Point, PoseStamped +from dimos.messages.sensor_msgs.msg import Image from dimos.models.vl.base import VlModel -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.Image import Image, sharpness_barrier -from dimos.msgs.visualization_msgs.EntityMarkers import EntityMarkers, Marker from dimos.perception.experimental.temporal_memory.clip_filter import ( CLIP_AVAILABLE, adaptive_keyframes, @@ -57,6 +59,7 @@ from dimos.perception.experimental.temporal_memory.temporal_utils.helpers import is_scene_stale from dimos.perception.experimental.temporal_memory.window_analyzer import WindowAnalyzer from dimos.utils.logging_config import get_run_log_dir, setup_logger +from dimos.utils.reactive import quality_barrier try: from dimos.perception.experimental.temporal_memory.clip_filter import CLIPFrameFilter @@ -252,7 +255,7 @@ def _publish_entity_markers(self) -> None: if not all_entities: return - markers: list[Marker] = [] + markers: list[EntityMarker] = [] for e in all_entities: meta = e.get("metadata") or {} x = meta.get("world_x") @@ -261,18 +264,18 @@ def _publish_entity_markers(self) -> None: if x is None or y is None: continue markers.append( - Marker( + EntityMarker( entity_id=e["entity_id"], label=(e.get("descriptor") or "")[:40], entity_type=e.get("entity_type", "object"), - x=x, - y=y, - z=(z or 0.0) + 0.3, # Offset up so labels float above ground + position=Point(x=x, y=y, z=(z or 0.0) + 0.3), # Offset labels above ground ) ) if markers: - self.entity_markers.publish(EntityMarkers(markers=markers)) + self.entity_markers.publish( + EntityMarkers(header=header_now("world"), markers=markers) + ) logger.info(f"[temporal-memory] published {len(markers)} entity markers to Rerun") except Exception as e: logger.debug(f"entity marker publish error: {e}") @@ -299,7 +302,9 @@ def _on_frame(img: Image) -> None: ) self.register_disposable( - frame_subject.pipe(sharpness_barrier(self.config.fps)).subscribe(_on_frame) + frame_subject.pipe(quality_barrier(image_sharpness, self.config.fps)).subscribe( + _on_frame + ) ) unsub_image = self.color_image.subscribe(frame_subject.on_next) self.register_disposable(Disposable(unsub_image)) @@ -307,9 +312,9 @@ def _on_frame(img: Image) -> None: # Odometry tracking for entity world positioning (optional — # module works without it, entities just won't have world positions) def _on_odom(msg: PoseStamped) -> None: - self._robot_x = msg.position.x - self._robot_y = msg.position.y - self._robot_z = msg.position.z + self._robot_x = msg.pose.position.x + self._robot_y = msg.pose.position.y + self._robot_z = msg.pose.position.z self._odom_count += 1 if self.odom.transport is not None: diff --git a/dimos/perception/experimental/temporal_memory/temporal_utils/helpers.py b/dimos/perception/experimental/temporal_memory/temporal_utils/helpers.py index 683c88376de..88900424d33 100644 --- a/dimos/perception/experimental/temporal_memory/temporal_utils/helpers.py +++ b/dimos/perception/experimental/temporal_memory/temporal_utils/helpers.py @@ -18,6 +18,8 @@ import numpy as np +from dimos.message_utils.image import image_to_rgb + if TYPE_CHECKING: from dimos.perception.experimental.temporal_memory.frame_window_accumulator import Frame @@ -68,7 +70,9 @@ def is_scene_stale(frames: list["Frame"], stale_threshold: float = 5.0) -> bool: last_img = frames[-1].image if first_img is None or last_img is None: return False - if not hasattr(first_img, "data") or not hasattr(last_img, "data"): + first = image_to_rgb(first_img) + last = image_to_rgb(last_img) + if first.shape != last.shape: return False - diff = np.abs(first_img.data.astype(float) - last_img.data.astype(float)) + diff = np.abs(first.astype(float) - last.astype(float)) return bool(diff.mean() < stale_threshold) diff --git a/dimos/perception/experimental/temporal_memory/test_temporal_memory_module.py b/dimos/perception/experimental/temporal_memory/test_temporal_memory_module.py index 79265931d1d..95542fa0cc2 100644 --- a/dimos/perception/experimental/temporal_memory/test_temporal_memory_module.py +++ b/dimos/perception/experimental/temporal_memory/test_temporal_memory_module.py @@ -16,16 +16,25 @@ from __future__ import annotations +from contextlib import ExitStack import json import threading from typing import TYPE_CHECKING -from unittest.mock import MagicMock, create_autospec, patch +from unittest.mock import create_autospec, patch +from dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode import numpy as np import pytest +from dimos.core.transport import LCMTransport +from dimos.message_utils.image import image_from_array +from dimos.message_utils.time import time_from_seconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.dimos_msgs.msg import EntityMarkers +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.std_msgs.msg import Header from dimos.models.vl.base import VlModel -from dimos.msgs.sensor_msgs.Image import Image +from dimos.perception.experimental.temporal_memory.clip_filter import _get_image_data from dimos.perception.experimental.temporal_memory.entity_graph_db import EntityGraphDB from dimos.perception.experimental.temporal_memory.frame_window_accumulator import ( Frame, @@ -38,6 +47,8 @@ from dimos.perception.experimental.temporal_memory.temporal_utils.graph_utils import ( extract_time_window, ) +from dimos.perception.experimental.temporal_memory.temporal_utils.helpers import is_scene_stale +from dimos.perception.experimental.temporal_memory.window_analyzer import WindowAnalyzer from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -52,7 +63,7 @@ def _make_image(value: int = 128, shape: tuple[int, ...] = (64, 64, 3)) -> Image: data = np.full(shape, value, dtype=np.uint8) - return Image.from_numpy(data) + return image_from_array(data, encoding="rgb8") class TestFrameWindowAccumulator: @@ -61,7 +72,7 @@ def test_bounded_buffer(self) -> None: acc.set_start_time(0.0) for i in range(10): img = _make_image(i * 25) - img.ts = float(i) + img.header.stamp = time_from_seconds(float(i)) acc.add_frame(img, float(i)) assert acc.buffer_size == 5 assert acc.frame_count == 10 @@ -74,7 +85,7 @@ def test_window_extraction(self) -> None: # Add 3 frames for i in range(3): img = _make_image() - img.ts = float(i) + img.header.stamp = time_from_seconds(float(i)) acc.add_frame(img, float(i)) frames = acc.try_extract_window() assert frames is not None @@ -85,7 +96,7 @@ def test_stride_guard(self) -> None: acc.set_start_time(0.0) for i in range(3): img = _make_image() - img.ts = float(i) + img.header.stamp = time_from_seconds(float(i)) acc.add_frame(img, float(i)) # First extraction should succeed frames = acc.try_extract_window() @@ -104,7 +115,7 @@ def test_clear(self) -> None: acc = FrameWindowAccumulator(max_buffer_frames=50, window_s=1.0, stride_s=1.0, fps=1.0) acc.set_start_time(0.0) img = _make_image() - img.ts = 0.0 + img.header.stamp = time_from_seconds(0.0) acc.add_frame(img, 0.0) assert acc.buffer_size == 1 acc.clear() @@ -304,6 +315,7 @@ def test_new_memory_clears_db(self, tmp_path: Path) -> None: return_value=None, ): tm = TemporalMemory( + use_clip_filtering=False, vlm=create_autospec(VlModel, spec_set=True, instance=True), db_dir=str(db_dir), new_memory=True, @@ -329,6 +341,7 @@ def test_persistent_memory_survives(self, tmp_path: Path) -> None: return_value=None, ): tm = TemporalMemory( + use_clip_filtering=False, vlm=create_autospec(VlModel, spec_set=True, instance=True), db_dir=str(db_dir), new_memory=False, @@ -350,6 +363,7 @@ def test_log_entries(self, tmp_path: Path) -> None: return_value=log_dir, ): tm = TemporalMemory( + use_clip_filtering=False, vlm=create_autospec(VlModel, spec_set=True, instance=True), db_dir=str(db_dir), ) @@ -386,62 +400,51 @@ def test_publish_entity_markers(self, tmp_path: Path) -> None: return_value=None, ): tm = TemporalMemory( + use_clip_filtering=False, vlm=create_autospec(VlModel, spec_set=True, instance=True), db_dir=str(db_dir), visualize=True, ) - # Populate DB with world positions - tm._graph_db.upsert_entity( - "E1", - "person", - "walking person", - 1.0, - metadata={"world_x": 1.0, "world_y": 2.0, "world_z": 0.0}, - ) - tm._graph_db.upsert_entity( - "E2", - "object", - "table", - 1.0, - metadata={"world_x": 3.0, "world_y": 4.0, "world_z": 0.0}, - ) - - # Mock the output stream publish - tm.entity_markers = MagicMock() - tm._publish_entity_markers() - - # Should have published EntityMarkers - tm.entity_markers.publish.assert_called_once() - msg = tm.entity_markers.publish.call_args[0][0] - assert len(msg.markers) == 2 - ids = {m.entity_id for m in msg.markers} - assert ids == {"E1", "E2"} - e1 = next(m for m in msg.markers if m.entity_id == "E1") - assert e1.x == 1.0 - assert e1.y == 2.0 - tm.stop() - - def test_markers_to_rerun(self) -> None: - from dimos.msgs.visualization_msgs.EntityMarkers import EntityMarkers, Marker + with ExitStack() as cleanup: + wire = LCMTransport("entity_markers", EntityMarkers, url="memq://") + cleanup.callback(wire.stop) + cleanup.callback(tm.stop) + tm.entity_markers.transport = wire + tm._graph_db.upsert_entity( + "E1", + "person", + "walking person", + 1, + metadata={"world_x": 1.0, "world_y": 2.0, "world_z": 0.0}, + ) + tm._graph_db.upsert_entity( + "E2", + "object", + "table", + 1, + metadata={"world_x": 3.0, "world_y": 4.0, "world_z": 0.0}, + ) + received = [] + ready = threading.Event() - markers = EntityMarkers( - markers=[ - Marker("E1", "person walking", "person", 1.0, 2.0, 0.3), - Marker("E2", "wooden table", "object", 3.0, 4.0, 0.3), - ] - ) - archetype = markers.to_rerun() - # Should return rr.Points3D - import rerun as rr + def receive(value): + received.append(value) + ready.set() - assert isinstance(archetype, rr.Points3D) + cleanup.callback(wire.subscribe(receive)) + tm._publish_entity_markers() + assert ready.wait(2) + assert len(received) == 1 + msg = received[0] + assert msg.header.frame_id == "world" + assert {m.entity_id for m in msg.markers} == {"E1", "E2"} + e1 = next(m for m in msg.markers if m.entity_id == "E1") + assert (e1.position.x, e1.position.y, e1.position.z) == (1, 2, 0.3) class TestWindowAnalyzer: def test_analyze_window_calls_vlm(self) -> None: - from dimos.perception.experimental.temporal_memory.window_analyzer import WindowAnalyzer - mock_vlm = create_autospec(VlModel, spec_set=True, instance=True) mock_vlm.query.return_value = json.dumps( { @@ -455,7 +458,7 @@ def test_analyze_window_calls_vlm(self) -> None: analyzer = WindowAnalyzer(mock_vlm) img = _make_image() - img.ts = 0.0 + img.header.stamp = time_from_seconds(0.0) frame = Frame(frame_index=0, timestamp_s=0.0, image=img) state_dict = {"entity_roster": [], "rolling_summary": ""} @@ -466,22 +469,18 @@ def test_analyze_window_calls_vlm(self) -> None: mock_vlm.query.assert_called_once() def test_analyze_window_vlm_error(self) -> None: - from dimos.perception.experimental.temporal_memory.window_analyzer import WindowAnalyzer - mock_vlm = create_autospec(VlModel, spec_set=True, instance=True) mock_vlm.query.side_effect = RuntimeError("VLM error") analyzer = WindowAnalyzer(mock_vlm) img = _make_image() - img.ts = 0.0 + img.header.stamp = time_from_seconds(0.0) frame = Frame(frame_index=0, timestamp_s=0.0, image=img) result = analyzer.analyze_window([frame], {}, 0.0, 2.0) assert result is None def test_update_summary(self) -> None: - from dimos.perception.experimental.temporal_memory.window_analyzer import WindowAnalyzer - mock_vlm = create_autospec(VlModel, spec_set=True, instance=True) mock_vlm.query.return_value = "Updated summary text" @@ -493,8 +492,6 @@ def test_update_summary(self) -> None: assert result.summary_text == "Updated summary text" def test_answer_query(self) -> None: - from dimos.perception.experimental.temporal_memory.window_analyzer import WindowAnalyzer - mock_vlm = create_autospec(VlModel, spec_set=True, instance=True) mock_vlm.query.return_value = "The answer is 42" @@ -504,3 +501,30 @@ def test_answer_query(self) -> None: result = analyzer.answer_query("What is the answer?", {}, img) assert result is not None assert result.answer == "The answer is 42" + + +def test_scene_staleness_compares_pixels_not_padding(): + first = Image( + width=2, + height=2, + encoding="rgb8", + step=8, + data=np.asarray([10] * 6 + [0, 0] + [10] * 6 + [0, 0], dtype=np.uint8), + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + is_bigendian=0, + ) + last = cdr_decode(cdr_encode(first), Image) + last.data = np.asarray([10] * 6 + [255, 255] + [10] * 6 + [255, 255], dtype=np.uint8) + assert is_scene_stale([Frame(0, 0, first), Frame(1, 1, last)], stale_threshold=1) + last.data = np.asarray([30] * 6 + [255, 255] + [30] * 6 + [255, 255], dtype=np.uint8) + assert not is_scene_stale([Frame(0, 0, first), Frame(1, 1, last)], stale_threshold=1) + + +def test_clip_frame_comparison_uses_rgb_colors_for_bgr_wire_images(): + pixels = np.zeros((4, 4, 3), dtype=np.uint8) + pixels[:, :, 0] = 255 + rgb = image_from_array(pixels, encoding="rgb8") + bgr = image_from_array(pixels[:, :, ::-1], encoding="bgr8") + bgr = cdr_decode(cdr_encode(bgr), Image) + np.testing.assert_array_equal(_get_image_data(rgb), _get_image_data(bgr)) + np.testing.assert_array_equal(_get_image_data(bgr)[0, 0], [255, 0, 0]) diff --git a/dimos/perception/experimental/temporal_memory/window_analyzer.py b/dimos/perception/experimental/temporal_memory/window_analyzer.py index afb21c3ca48..c5d617c5000 100644 --- a/dimos/perception/experimental/temporal_memory/window_analyzer.py +++ b/dimos/perception/experimental/temporal_memory/window_analyzer.py @@ -35,8 +35,8 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: + from dimos.messages.sensor_msgs.msg import Image from dimos.models.vl.base import VlModel - from dimos.msgs.sensor_msgs.Image import Image from dimos.perception.experimental.temporal_memory.frame_window_accumulator import Frame logger = setup_logger() diff --git a/dimos/perception/experimental/test_generated_tracker.py b/dimos/perception/experimental/test_generated_tracker.py new file mode 100644 index 00000000000..fa7e37e7319 --- /dev/null +++ b/dimos/perception/experimental/test_generated_tracker.py @@ -0,0 +1,143 @@ +# 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 contextlib import ExitStack +from threading import Event + +from dimos_cdrgen.registry import decode, encode +import numpy as np +import pytest + +from dimos.core.transport import LCMTransport +from dimos.message_utils.image import image_view +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.std_msgs.msg import Header +from dimos.messages.vision_msgs.msg import ( + BoundingBox2D, + Detection2D, + Detection2DArray, + Point2D, + Pose2D, +) +from dimos.perception.experimental.object_tracker_2d import ObjectTracker2D +from dimos.perception.experimental.object_tracker_3d import ObjectTracker3D + + +@pytest.fixture +def tracker_factory(): + with ExitStack() as cleanup: + + def create(cls): + transports = cleanup.enter_context(ExitStack()) + module = cls(frame_id="camera") + cleanup.callback(module.stop) + for name, output in module.outputs.items(): + wire = LCMTransport("tracker_" + name, output.type, url="memq://") + transports.callback(wire.stop) + output.transport = wire + return module + + yield create, cleanup + + +@pytest.mark.parametrize("succeeded", [True, False]) +def test_tracker_publishes_generated_detection_and_clears_failed_track( + tracker_factory, mocker, succeeded +): + create, cleanup = tracker_factory + module = create(ObjectTracker2D) + tracker = mocker.Mock() + tracker.update.return_value = (succeeded, (10, 12, 20, 16)) + mocker.patch.object(module, "tracker", tracker) + module.tracking_initialized = True + module._latest_rgb_frame = np.zeros((64, 64, 3), dtype=np.uint8) + detections, overlays = [], [] + detection_ready, overlay_ready = Event(), Event() + + def receive_detection(value): + detections.append(value) + detection_ready.set() + + def receive_overlay(value): + overlays.append(value) + overlay_ready.set() + + cleanup.callback(module.detection2darray.transport.subscribe(receive_detection)) + cleanup.callback(module.tracked_overlay.transport.subscribe(receive_overlay)) + module._process_tracking() + assert detection_ready.wait(2) + assert len(detections) == 1 + result = detections[0] + assert result.header.frame_id == "camera" + if not succeeded: + assert not result.detections + assert not module.tracking_initialized + assert not overlays + return + assert overlay_ready.wait(2) + (detection,) = result.detections + assert (detection.bbox.center.position.x, detection.bbox.center.position.y) == (20, 20) + assert (detection.bbox.size_x, detection.bbox.size_y) == (20, 16) + assert detection.results[0].hypothesis.class_id == "tracked_object" + assert len(overlays) == 1 + assert overlays[0].header == result.header + assert image_view(overlays[0]).shape == (64, 64, 3) + assert overlays[0].encoding == "rgb8" + + +@pytest.mark.parametrize("depth", [2.0, 0.0, float("nan")]) +def test_tracker3d_projection_preserves_depth_filter_and_source_stamp(tracker_factory, depth): + create, cleanup = tracker_factory + module = create(ObjectTracker3D) + module.camera_intrinsics = [100.0, 100.0, 32.0, 32.0] + module._latest_depth_frame = np.full((64, 64), depth, dtype=np.float32) + header = Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="camera") + detection = Detection2DArray( + header=header, + detections=[ + Detection2D( + header=header, + bbox=BoundingBox2D( + center=Pose2D(position=Point2D(x=32, y=32), theta=0), size_x=20, size_y=16 + ), + results=[], + id="", + ) + ], + ) + received = [] + ready = Event() + + def receive(value): + received.append(value) + ready.set() + + cleanup.callback(module.tf.transport.subscribe(receive)) + output = module._create_detection3d_from_2d(detection) + if not np.isfinite(depth) or depth <= 0: + assert output is None + assert not received + return + result = decode(encode(output), type(output)) + assert result.header == header + box = result.detections[0].bbox + assert (box.center.position.x, box.center.position.y, box.center.position.z) == (2, 0, 0) + np.testing.assert_allclose([box.size.x, box.size.y, box.size.z], [0.4, 0.32, 0.1]) + assert ready.wait(2) + assert len(received) == 1 + (transform,) = received[0].transforms + assert transform.header == header + assert transform.child_frame_id == "tracked_object" + assert transform.transform.translation.x == 2 + assert transform.transform.rotation.w == 1 diff --git a/dimos/perception/experimental/test_object_generated.py b/dimos/perception/experimental/test_object_generated.py new file mode 100644 index 00000000000..faa2287abd9 --- /dev/null +++ b/dimos/perception/experimental/test_object_generated.py @@ -0,0 +1,202 @@ +# 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. + +"""Object geometry and deduplication use generated ROS values.""" + +from dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode +import numpy as np + +from dimos.message_utils.camera_info import camera_info_from_intrinsics +from dimos.message_utils.image import image_from_array +from dimos.message_utils.pointcloud import pointcloud_from_xyz_rgb, pointcloud_rgb, pointcloud_xyz +from dimos.message_utils.time import time_from_seconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos.messages.sensor_msgs.msg import Image, PointCloud2 +from dimos.messages.std_msgs.msg import Header +from dimos.messages.vision_msgs.msg import Detection3D +from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D +from dimos.perception.detection.type.detection2d.seg import Detection2DSeg +from dimos.perception.experimental.object import Object, aggregate_pointclouds, to_detection3d_array +from dimos.perception.experimental.objectDB import ObjectDB + + +def _object(object_id, x, timestamp, source_stamp=None): + stamp = time_from_seconds(timestamp) if source_stamp is None else source_stamp + return Object( + object_id=object_id, + name="cup", + center=Vector3(x=x, y=0.0, z=0.0), + size=Vector3(x=0.1, y=0.2, z=0.3), + pose=PoseStamped( + header=Header(frame_id="world", stamp=stamp), + pose=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ), + pointcloud=PointCloud2( + header=Header(stamp=stamp, frame_id="world"), + height=0, + width=0, + fields=[], + is_bigendian=False, + point_step=0, + row_step=0, + data=np.array([], dtype=np.uint8), + is_dense=False, + ), + bbox=(0.0, 0.0, 1.0, 1.0), + track_id=-1, + class_id=0, + confidence=0.9, + ts=timestamp, + frame_id="world", + image=Image( + header=Header(stamp=stamp, frame_id="world"), + height=0, + width=0, + encoding="", + is_bigendian=0, + step=0, + data=np.array([], dtype=np.uint8), + ), + ) + + +def test_detection_cdr_preserves_source_pose_header_and_current_center(): + obj = _object("cup-1", 0.2, 1.0, source_stamp=Time(sec=1700000000, nanosec=123456789)) + obj.set_center(Vector3(x=0.4, y=0.5, z=0.6)) + decoded = cdr_decode(cdr_encode(obj.to_detection3d_msg()), Detection3D) + assert decoded.header == obj.pose.header + assert decoded.bbox.center == obj.pose.pose + assert decoded.bbox.size == obj.size + assert decoded.results[0].hypothesis.class_id == "cup" + assert decoded.results[0].hypothesis.score == 0.9 + array = to_detection3d_array([obj]) + assert array.header == obj.pose.header + decoded.bbox.center.position.x = 99 + assert obj.center.x == 0.4 + assert obj.pose.pose.position.x == 0.4 + + +def test_spatial_deduplication_and_nearest_lookup_use_generated_centers(): + database = ObjectDB(distance_threshold=0.2, min_detections_for_permanent=1) + first = _object("first", 0.0, 1.0) + nearby = _object("nearby", 0.1, 2.0) + distant = _object("distant", 2.0, 3.0) + assert database.add_objects([first]) == [first] + assert database.add_objects([nearby]) == [first] + assert first.center.x == 0.1 + assert database.add_objects([distant]) == [distant] + assert database.find_nearest(Vector3(x=1.8, y=0.0, z=0.0)) is distant + assert database.find_nearest(Vector3(x=0.2, y=0.0, z=0.0)) is first + + +def test_generated_object_cloud_aggregation_and_dictionary_preserve_values(): + obj = _object("abc", 0.2, 1.0) + stamp = Time(sec=1700000000, nanosec=987654321) + obj.pointcloud = pointcloud_from_xyz_rgb( + np.array([[1.0, 2.0, 3.0]], dtype=np.float32), + np.array([[255, 128, 0]], dtype=np.uint8), + header=Header(frame_id="world", stamp=stamp), + ) + pixels = np.zeros((2, 3, 3), dtype=np.uint8) + obj.image = image_from_array(pixels, encoding="rgb8", header=obj.pointcloud.header) + exported = obj.to_dict() + np.testing.assert_array_equal(exported["pointcloud"][0], [[1.0, 2.0, 3.0]]) + np.testing.assert_allclose(exported["pointcloud"][1], [[1.0, 128 / 255.0, 0.0]]) + np.testing.assert_array_equal(exported["image"], pixels) + merged = aggregate_pointclouds([obj]) + decoded = cdr_decode(cdr_encode(merged), PointCloud2) + assert decoded.header.stamp == stamp and decoded.header.frame_id == "world" + np.testing.assert_array_equal(pointcloud_xyz(decoded), [[1.0, 2.0, 3.0]]) + assert pointcloud_rgb(decoded) is not None + assert aggregate_pointclouds([]).width == 0 + + +def test_zero_and_adjacent_source_stamps_are_distinct_observations(): + for sec in [0, 1700000000]: + database = ObjectDB(min_detections_for_permanent=2) + stamp = Time(sec=sec, nanosec=0) + first = _object("first", 0, float(sec), source_stamp=stamp) + duplicate = _object("duplicate", 0, float(sec), source_stamp=stamp) + newer = _object("newer", 0, float(sec), source_stamp=Time(sec=sec, nanosec=1)) + database.add_objects([first, duplicate]) + assert first.detections_count == 1 + assert first.ts == float(sec) + database.add_objects([newer]) + assert first.detections_count == 2 + assert first.pose.header.stamp == newer.pose.header.stamp + assert database.get_objects() == [first] + + +def test_actual_rgbd_object_projection_keeps_color_and_depth_source_headers(): + color_header = Header(frame_id="camera", stamp=Time(sec=1700000000, nanosec=123456788)) + depth_header = Header(frame_id="camera", stamp=Time(sec=1700000000, nanosec=123456789)) + pixels = np.zeros((32, 32, 3), dtype=np.uint8) + pixels[:, :, 0] = 255 + color = image_from_array(pixels, encoding="rgb8", header=color_header) + depth = image_from_array( + np.full((32, 32), 2, dtype=np.float32), encoding="32FC1", header=depth_header + ) + mask = np.zeros((32, 32), dtype=np.uint8) + mask[8:24, 8:24] = 255 + detection = Detection2DSeg( + bbox=(8, 8, 24, 24), + track_id=7, + class_id=0, + confidence=0.9, + name="cup", + ts=1700000000.1234567, + image=color, + mask=mask, + ) + camera = camera_info_from_intrinsics(100, 100, 16, 16, 32, 32, header=color_header) + mount = TransformStamped( + header=Header(frame_id="world", stamp=Time(sec=1, nanosec=0)), + child_frame_id="camera", + transform=Transform( + translation=Vector3(x=1, y=0, z=0), rotation=Quaternion(x=0, y=0, z=0, w=1) + ), + ) + objects = Object.from_2d_to_list( + ImageDetections2D(color, [detection]), + color, + depth, + camera, + mount, + use_aabb=True, + mask_erode_pixels=0, + statistical_std_ratio=10, + ) + assert len(objects) == 1 + obj = objects[0] + assert obj.pose.header.stamp == color_header.stamp + assert obj.pointcloud.header.stamp == depth_header.stamp + assert obj.pose.header.frame_id == obj.pointcloud.header.frame_id == "world" + np.testing.assert_allclose(pointcloud_xyz(obj.pointcloud)[:, 2], 2) + np.testing.assert_array_equal(pointcloud_rgb(obj.pointcloud)[:, 0], 255) + assert 0.9 < obj.center.x < 1.1 + wire = cdr_decode(cdr_encode(obj.to_detection3d_msg()), Detection3D) + assert wire.header == obj.pose.header + assert wire.id == obj.object_id diff --git a/dimos/perception/experimental/test_object_scene_registration_temporal.py b/dimos/perception/experimental/test_object_scene_registration_temporal.py index 82f14417f9c..07719256379 100644 --- a/dimos/perception/experimental/test_object_scene_registration_temporal.py +++ b/dimos/perception/experimental/test_object_scene_registration_temporal.py @@ -14,8 +14,7 @@ from __future__ import annotations -from collections.abc import Iterator -import sys +from contextlib import ExitStack from threading import Event, Thread from typing import Any from unittest.mock import ANY, MagicMock, call @@ -23,66 +22,78 @@ import numpy as np import pytest -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.core.transport import LCMTransport +from dimos.message_utils.camera_info import camera_info_from_intrinsics +from dimos.message_utils.image import image_from_array +from dimos.message_utils.pointcloud import pointcloud_xyz +from dimos.message_utils.time import time_from_seconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.messages.vision_msgs.msg import Detection3DArray +from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.experimental.object_scene_registration import ObjectSceneRegistrationModule from dimos.perception.experimental.objectDB import ObjectDB +from dimos.perception.experimental.test_object_generated import _object -class _FakeTF: - def __init__(self, result: Any) -> None: - self.result = result - self.calls: list[tuple[Any, ...]] = [] +def _image(timestamp: float) -> Image: + return image_from_array( + np.ones((2, 2), dtype=np.float32), + encoding="32FC1", + header=Header(frame_id="camera", stamp=time_from_seconds(timestamp)), + ) - def get(self, *args: Any, **kwargs: Any) -> Any: - self.calls.append((args, kwargs)) - return self.result - def dispose(self) -> None: - pass +@pytest.fixture +def module_factory(): + with ExitStack() as cleanup: + def create(**kwargs): + module = ObjectSceneRegistrationModule(**kwargs) + cleanup.callback(module.stop) + module._camera_info = camera_info_from_intrinsics( + 1, 1, 0, 0, 2, 2, header=Header(frame_id="camera", stamp=Time(sec=0, nanosec=0)) + ) + wire = LCMTransport("object_scene_tf", TFMessage, url="memq://") + cleanup.callback(wire.stop) + module.tf.transport = wire + return module -def _image(timestamp: float) -> Image: - return Image( - data=np.ones((2, 2), dtype=np.float32), - format=ImageFormat.DEPTH, - frame_id="camera", - ts=timestamp, - ) + yield create @pytest.fixture -def module() -> Iterator[ObjectSceneRegistrationModule]: - module = ObjectSceneRegistrationModule(target_frame="map") - module._camera_info = MagicMock(K=[1.0, 0.0, 0.0, 0.0, 1.0, 0.0]) - module._latest_scene_snapshot = None - yield module - module.stop() +def module(module_factory): + return module_factory(target_frame="map") def test_transform_is_captured_before_slow_detector( - monkeypatch: Any, module: ObjectSceneRegistrationModule + monkeypatch: Any, module: ObjectSceneRegistrationModule, module_factory, mocker ) -> None: now = [0.0] - transform = MagicMock(name="fresh_transform") - - class _ExpiringTF(_FakeTF): - def get(self, *args: Any, **kwargs: Any) -> Any: - super().get(*args, **kwargs) - return transform if now[0] <= 10.0 else None - - tf = _ExpiringTF(None) - module._tf = tf # type: ignore[assignment] + transform = TransformStamped( + header=Header(frame_id="map", stamp=time_from_seconds(12.5)), + child_frame_id="camera", + transform=Transform( + translation=Vector3(x=1, y=0, z=0), rotation=Quaternion(x=0, y=0, z=0, w=1) + ), + ) + lookup = mocker.patch.object( + module.tfbuffer, + "get", + side_effect=lambda *args, **kwargs: transform if now[0] <= 10 else None, + ) module._detector_backend = "owlv2" module._detector = MagicMock() module._text_prompts = ["cup"] - module.detections_2d = MagicMock() - color = Image( - data=np.zeros((2, 2, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera", - ts=12.5, + color = image_from_array( + np.zeros((2, 2, 3), dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera", stamp=time_from_seconds(12.5)), ) detections = ImageDetections2D(color, []) @@ -96,14 +107,20 @@ def slow_detection(*_args: Any, **_kwargs: Any) -> ImageDetections2D: module._process_images(color, _image(12.5)) - assert tf.calls == [(("map", "camera", 12.5, 0.1), {"forward_tolerance": 0.2})] + lookup.assert_called_once_with("map", "camera", 12.5, 0.1, forward_tolerance=0.2) process_3d.assert_called_once_with(detections, color, ANY, transform) def test_failed_lookup_does_not_retry_without_time_or_replace_coherent_cache( - monkeypatch: Any, module: ObjectSceneRegistrationModule + monkeypatch: Any, module: ObjectSceneRegistrationModule, module_factory, mocker ) -> None: - old_transform = MagicMock(name="old_transform") + old_transform = TransformStamped( + header=Header(frame_id="map", stamp=time_from_seconds(1)), + child_frame_id="camera", + transform=Transform( + translation=Vector3(x=0, y=0, z=0), rotation=Quaternion(x=0, y=0, z=0, w=1) + ), + ) warning = MagicMock() monkeypatch.setattr( "dimos.perception.experimental.object_scene_registration.logger.warning", warning @@ -116,7 +133,7 @@ def test_failed_lookup_does_not_retry_without_time_or_replace_coherent_cache( old_depth = _image(1.0) ObjectSceneRegistrationModule._process_3d_detections( module, - MagicMock(spec=ImageDetections2D), + ImageDetections2D(_image(1), []), old_depth, old_depth, old_transform, @@ -124,7 +141,7 @@ def test_failed_lookup_does_not_retry_without_time_or_replace_coherent_cache( new_depth = _image(2.0) ObjectSceneRegistrationModule._process_3d_detections( module, - MagicMock(spec=ImageDetections2D), + ImageDetections2D(_image(1), []), new_depth, new_depth, None, @@ -135,53 +152,44 @@ def test_failed_lookup_does_not_retry_without_time_or_replace_coherent_cache( def test_full_scene_pointcloud_uses_one_coherent_scene_snapshot( - monkeypatch: Any, module: ObjectSceneRegistrationModule + monkeypatch: Any, module: ObjectSceneRegistrationModule, module_factory, mocker ) -> None: depth = _image(3.0) - transform = MagicMock(name="transform") - module._tf = _FakeTF(transform) # type: ignore[assignment] - module._latest_scene_snapshot = (depth, transform) - - class _PointCloud: - points = list(range(100)) - - def voxel_down_sample(self, voxel_size: float) -> _PointCloud: - return self - - pointcloud = _PointCloud() - fake_o3d = MagicMock() - fake_o3d.camera.PinholeCameraIntrinsic.return_value = MagicMock() - fake_o3d.geometry.Image.return_value = MagicMock() - fake_o3d.geometry.PointCloud.create_from_depth_image.return_value = pointcloud - # open3d is imported inside the method under test, so swap the module itself - monkeypatch.setitem(sys.modules, "open3d", fake_o3d) - - result = MagicMock() - result.transform.side_effect = lambda used_transform: ( - result if used_transform is transform else pytest.fail("mixed scene snapshot") + transform = TransformStamped( + header=Header(frame_id="map", stamp=Time(sec=0, nanosec=0)), + child_frame_id="camera", + transform=Transform( + translation=Vector3(x=1, y=0.0, z=0.0), rotation=Quaternion(w=1, x=0.0, y=0.0, z=0.0) + ), ) - monkeypatch.setattr( - "dimos.perception.experimental.object_scene_registration.PointCloud2", - lambda *_args, **_kwargs: result, + depth = image_from_array( + np.ones((20, 20), dtype=np.float32), encoding="32FC1", header=depth.header ) - - module.get_full_scene_pointcloud() - result.transform.assert_called_once_with(transform) - - -def test_owlv2_queries_configured_prompts(monkeypatch: Any) -> None: - module = ObjectSceneRegistrationModule( + module._camera_info = camera_info_from_intrinsics(100, 100, 10, 10, 20, 20, header=depth.header) + module._latest_scene_snapshot = (depth, transform) + lookup = mocker.spy(module.tfbuffer, "get") + result = module.get_full_scene_pointcloud(voxel_size=0.001) + assert result is not None + assert result.header.frame_id == "map" + assert result.header.stamp == depth.header.stamp + points = pointcloud_xyz(result) + assert len(points) == 400 + np.testing.assert_allclose(points[:, 2], 1) + assert points[:, 0].min() == pytest.approx(0.9) + assert points[:, 0].max() == pytest.approx(1.09) + lookup.assert_not_called() + + +def test_owlv2_queries_configured_prompts(monkeypatch: Any, module_factory, mocker) -> None: + module = module_factory( target_frame="camera", detector_backend="owlv2", detector_confidence=0.07 ) - module._camera_info = MagicMock() module._detector = MagicMock() module._text_prompts = ["mug"] - module.detections_2d = MagicMock() - color = Image( - data=np.zeros((2, 2, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera", - ts=4.0, + color = image_from_array( + np.zeros((2, 2, 3), dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera", stamp=time_from_seconds(4.0)), ) detections = ImageDetections2D(color, []) module._detector.query_detections.return_value = detections @@ -195,8 +203,8 @@ def test_owlv2_queries_configured_prompts(monkeypatch: Any) -> None: module.stop() -def test_owlv2_yolo_constructs_box_prompt_segmenter(mocker: Any) -> None: - module = ObjectSceneRegistrationModule( +def test_owlv2_yolo_constructs_box_prompt_segmenter(mocker: Any, module_factory) -> None: + module = module_factory( detector_backend="owlv2", segmentation_backend="yolo", segmentation_confidence=0.04, @@ -216,30 +224,31 @@ def test_owlv2_yolo_constructs_box_prompt_segmenter(mocker: Any) -> None: segmenter_factory.assert_called_once_with(confidence=0.04) -def test_moondream_queries_each_configured_prompt(monkeypatch: Any) -> None: - module = ObjectSceneRegistrationModule(target_frame="camera", detector_backend="moondream") - module._camera_info = MagicMock() - module._detector = MagicMock() +def test_moondream_queries_each_configured_prompt(monkeypatch: Any, module_factory, mocker) -> None: + module = module_factory(target_frame="camera", detector_backend="moondream") + detector = MagicMock() + module._detector = detector module._text_prompts = ["cup", "bottle"] - module.detections_2d = MagicMock() - color = Image( - data=np.zeros((2, 2, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera", - ts=4.0, + color = image_from_array( + np.zeros((2, 2, 3), dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera", stamp=time_from_seconds(4.0)), ) - cup = MagicMock(track_id=0, class_id=-1) - bottle = MagicMock(track_id=0, class_id=-1) - module._detector.query_detections.side_effect = [ + cup = Detection2DBBox((0, 0, 1, 1), 0, -1, 0.9, "cup", 4.0, color) + bottle = Detection2DBBox((1, 1, 2, 2), 0, -1, 0.8, "bottle", 4.0, color) + detector.query_detections.side_effect = [ ImageDetections2D(color, [cup]), ImageDetections2D(color, [bottle]), ] process_3d = MagicMock() monkeypatch.setattr(module, "_process_3d_detections", process_3d) - module._process_images(color, _image(4.0)) + try: + module._process_images(color, _image(4.0)) + finally: + module.stop() - assert module._detector.query_detections.call_args_list == [ + assert detector.query_detections.call_args_list == [ call(color, "cup"), call(color, "bottle"), ] @@ -247,17 +256,14 @@ def test_moondream_queries_each_configured_prompt(monkeypatch: Any) -> None: assert combined.detections == [cup, bottle] assert (cup.track_id, cup.class_id) == (-1, 0) assert (bottle.track_id, bottle.class_id) == (-1, 1) - module.stop() -def test_edgetam_refines_detector_output(monkeypatch: Any) -> None: - module = ObjectSceneRegistrationModule(target_frame="camera", segmentation_backend="edgetam") - module._camera_info = MagicMock() - color = Image( - data=np.zeros((2, 2, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera", - ts=4.0, +def test_edgetam_refines_detector_output(monkeypatch: Any, module_factory, mocker) -> None: + module = module_factory(target_frame="camera", segmentation_backend="edgetam") + color = image_from_array( + np.zeros((2, 2, 3), dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera", stamp=time_from_seconds(4.0)), ) raw_detections = ImageDetections2D(color, []) segmented_detections = ImageDetections2D(color, []) @@ -265,7 +271,6 @@ def test_edgetam_refines_detector_output(monkeypatch: Any) -> None: module._segmenter.segment.return_value = segmented_detections module._detector = MagicMock() module._detector.process_image.return_value = raw_detections - module.detections_2d = MagicMock() process_3d = MagicMock() monkeypatch.setattr(module, "_process_3d_detections", process_3d) @@ -276,21 +281,22 @@ def test_edgetam_refines_detector_output(monkeypatch: Any) -> None: module.stop() -def test_request_driven_scan_processes_latest_aligned_frame(monkeypatch: Any) -> None: - module = ObjectSceneRegistrationModule( - target_frame="camera", detector_backend="owlv2", detect_on_request=True - ) +def test_request_driven_scan_processes_latest_aligned_frame( + monkeypatch: Any, module_factory, mocker +) -> None: + module = module_factory(target_frame="camera", detector_backend="owlv2", detect_on_request=True) module._object_db = MagicMock() - color = Image( - data=np.zeros((2, 2, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera", - ts=4.0, + color = image_from_array( + np.zeros((2, 2, 3), dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera", stamp=time_from_seconds(4.0)), ) depth = _image(4.0) module._on_aligned_frames((color, depth)) output = MagicMock() - result = MagicMock(spec=Detection3DArray) + result = Detection3DArray( + header=Header(frame_id="camera", stamp=time_from_seconds(4)), detections=[] + ) def process_images(got_color: Image, got_depth: Image) -> list[MagicMock]: assert (got_color, got_depth) == (color, depth) @@ -307,8 +313,8 @@ def process_images(got_color: Image, got_depth: Image) -> list[MagicMock]: module.stop() -def test_request_driven_detect_triggers_scan(monkeypatch: Any) -> None: - module = ObjectSceneRegistrationModule(detector_backend="owlv2", detect_on_request=True) +def test_request_driven_detect_triggers_scan(monkeypatch: Any, module_factory, mocker) -> None: + module = module_factory(detector_backend="owlv2", detect_on_request=True) module._detector = MagicMock() current_object = MagicMock() current_object.agent_encode.return_value = {"name": "mug", "object_id": "current"} @@ -322,18 +328,14 @@ def test_request_driven_detect_triggers_scan(monkeypatch: Any) -> None: module.stop() -def test_scan_output_includes_pending_objects(monkeypatch: Any) -> None: - module = ObjectSceneRegistrationModule(target_frame="camera") - module._camera_info = MagicMock() +def test_scan_output_includes_pending_objects(monkeypatch: Any, module_factory, mocker) -> None: + module = module_factory(target_frame="camera") module._object_db = MagicMock() pending = MagicMock() permanent = MagicMock() module._object_db.get_all_objects.return_value = [pending, permanent] module._object_db.get_objects.return_value = [permanent] module._object_db.add_objects.return_value = [pending] - module.detections_3d = MagicMock() - module.objects = MagicMock() - module.pointcloud = MagicMock() converted_objects = [pending] monkeypatch.setattr( "dimos.perception.experimental.object_scene_registration.Object.from_2d_to_list", @@ -350,7 +352,7 @@ def test_scan_output_includes_pending_objects(monkeypatch: Any) -> None: observed = ObjectSceneRegistrationModule._process_3d_detections( module, - MagicMock(spec=ImageDetections2D), + ImageDetections2D(_image(1), []), _image(4.0), _image(4.0), None, @@ -362,7 +364,7 @@ def test_scan_output_includes_pending_objects(monkeypatch: Any) -> None: module._object_db.add_objects.reset_mock() observed = ObjectSceneRegistrationModule._process_3d_detections( module, - MagicMock(spec=ImageDetections2D), + ImageDetections2D(_image(1), []), _image(5.0), _image(5.0), None, @@ -373,16 +375,13 @@ def test_scan_output_includes_pending_objects(monkeypatch: Any) -> None: module.stop() -def test_concurrent_request_scans_do_not_overlap(monkeypatch: Any) -> None: - module = ObjectSceneRegistrationModule( - target_frame="camera", detector_backend="owlv2", detect_on_request=True - ) +def test_concurrent_request_scans_do_not_overlap(monkeypatch: Any, module_factory, mocker) -> None: + module = module_factory(target_frame="camera", detector_backend="owlv2", detect_on_request=True) module._object_db = MagicMock() - color = Image( - data=np.zeros((2, 2, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera", - ts=4.0, + color = image_from_array( + np.zeros((2, 2, 3), dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera", stamp=time_from_seconds(4.0)), ) module._on_aligned_frames((color, _image(4.0))) first_started = Event() @@ -407,12 +406,15 @@ def process_images(_: Image, __: Image) -> list[MagicMock]: first = Thread(target=lambda: module.scan_scene(text=["cup"])) second = Thread(target=lambda: module.scan_scene(text=["mug"])) first.start() - assert first_started.wait(timeout=1.0) - second.start() - assert not second_started.wait(timeout=0.05) - release_first.set() - first.join(timeout=1.0) - second.join(timeout=1.0) + try: + assert first_started.wait(timeout=1.0) + second.start() + assert not second_started.wait(timeout=0.05) + finally: + release_first.set() + first.join(timeout=1.0) + if second.ident is not None: + second.join(timeout=1.0) assert not first.is_alive() assert not second.is_alive() @@ -420,104 +422,53 @@ def process_images(_: Image, __: Image) -> list[MagicMock]: module.stop() -def test_object_db_uses_wall_clock_for_pending_ttl(monkeypatch: Any) -> None: - object_db = ObjectDB(pending_ttl_s=5.0) - detected = MagicMock() - detected.object_id = "stable-id" - detected.track_id = -1 - detected.ts = 4.0 # Hardware timestamps are relative to camera boot. - detected.last_seen_ts = None - detected.center = None - detected.detections_count = 1 - now = [1000.0] +def test_object_db_uses_wall_clock_for_pending_ttl(monkeypatch): + database = ObjectDB(pending_ttl_s=5) + detected = _object("stable-id", 0, 4) + now = [1000] monkeypatch.setattr("dimos.perception.experimental.objectDB.time.time", lambda: now[0]) - - object_db.add_objects([detected]) - assert detected.last_seen_ts == 1000.0 - now[0] = 1004.0 - object_db.add_objects([]) - assert object_db.find_by_object_id("stable-id") is detected - now[0] = 1006.0 - object_db.add_objects([]) - assert object_db.find_by_object_id("stable-id") is None - - -def test_object_db_counts_each_source_frame_once(monkeypatch: Any) -> None: - object_db = ObjectDB(min_detections_for_permanent=10) - now = [1000.0] + database.add_objects([detected]) + assert detected.last_seen_ts == 1000 + now[0] = 1004 + database.add_objects([]) + assert database.find_by_object_id("stable-id") is detected + now[0] = 1006 + database.add_objects([]) + assert database.find_by_object_id("stable-id") is None + + +def test_object_db_counts_each_source_frame_once(monkeypatch): + database = ObjectDB(min_detections_for_permanent=10) + now = [1000] monkeypatch.setattr("dimos.perception.experimental.objectDB.time.time", lambda: now[0]) - - first = MagicMock( - object_id="first-id", - track_id=-1, - ts=4.0, - last_seen_ts=None, - detections_count=1, - ) - first.center = MagicMock() - duplicate = MagicMock(object_id="duplicate-id", track_id=-1, ts=4.0) - duplicate.center = MagicMock() - duplicate.center.distance.return_value = 0.0 - - observed = object_db.add_objects([first, duplicate]) - - assert observed == [first] - first.update_object.assert_not_called() - assert first.last_seen_ts == 1000.0 - - newer = MagicMock(object_id="newer-id", track_id=-1, ts=5.0) - newer.center = MagicMock() - newer.center.distance.return_value = 0.0 - first.update_object.side_effect = lambda _: setattr(first, "detections_count", 2) - now[0] = 1001.0 - - assert object_db.add_objects([newer]) == [first] - first.update_object.assert_called_once_with(newer) - assert first.last_seen_ts == 1001.0 - - -def test_object_db_promotes_a_first_sighting_when_threshold_is_one() -> None: - object_db = ObjectDB(min_detections_for_permanent=1) - detected = MagicMock( - object_id="first-id", - track_id=-1, - ts=1.0, - last_seen_ts=None, - detections_count=1, - name="cup", - ) - detected.center = None - - object_db.add_objects([detected]) - - assert object_db.get_objects() == [detected] - assert object_db.get_stats() == { - "pending_count": 0, - "permanent_count": 1, - "total_count": 1, - } - - -def test_object_db_keeps_first_sighting_pending_above_threshold() -> None: - object_db = ObjectDB(min_detections_for_permanent=2) - detected = MagicMock( - object_id="first-id", - track_id=7, - ts=1.0, - last_seen_ts=None, - detections_count=1, - name="cup", - ) - detected.center = None - newer = MagicMock(object_id="newer-id", track_id=7, ts=2.0) - newer.center = None - detected.update_object.side_effect = lambda _: setattr(detected, "detections_count", 2) - - object_db.add_objects([detected]) - - assert object_db.get_objects() == [] - assert object_db.find_by_object_id("first-id") is detected - - object_db.add_objects([newer]) - - assert object_db.get_objects() == [detected] + first = _object("first-id", 0, 4) + duplicate = _object("duplicate-id", 0, 4) + assert database.add_objects([first, duplicate]) == [first] + assert first.detections_count == 1 + assert first.last_seen_ts == 1000 + newer = _object("newer-id", 0, 5) + now[0] = 1001 + assert database.add_objects([newer]) == [first] + assert first.detections_count == 2 + assert first.last_seen_ts == 1001 + + +def test_object_db_promotes_a_first_sighting_when_threshold_is_one(): + database = ObjectDB(min_detections_for_permanent=1) + detected = _object("first-id", 0, 1) + database.add_objects([detected]) + assert database.get_objects() == [detected] + assert database.get_stats() == {"pending_count": 0, "permanent_count": 1, "total_count": 1} + + +def test_object_db_keeps_first_sighting_pending_above_threshold(): + database = ObjectDB(min_detections_for_permanent=2) + detected = _object("first-id", 0, 1) + detected.track_id = 7 + newer = _object("newer-id", 0, 2) + newer.track_id = 7 + database.add_objects([detected]) + assert database.get_objects() == [] + assert database.find_by_object_id("first-id") is detected + database.add_objects([newer]) + assert database.get_objects() == [detected] diff --git a/dimos/perception/experimental/test_perceive_loop_skill.py b/dimos/perception/experimental/test_perceive_loop_skill.py index 272669a1da0..be0a8553705 100644 --- a/dimos/perception/experimental/test_perceive_loop_skill.py +++ b/dimos/perception/experimental/test_perceive_loop_skill.py @@ -18,8 +18,13 @@ from threading import Event +import numpy as np import pytest +from reactivex.testing import ReactiveTest, TestScheduler +from dimos.message_utils.image import image_from_array +from dimos.message_utils.time import time_from_seconds +from dimos.messages.std_msgs.msg import Header from dimos.perception.experimental import perceive_loop_skill as skill_module from dimos.perception.experimental.perceive_loop_skill import PerceiveLoopSkill @@ -61,3 +66,41 @@ def failing_create(name: str) -> object: assert module._lookout_subscription is None finally: module.stop() + + +def test_generated_sharpness_window_keeps_rolling_and_repeated_selection_and_disposes(): + scheduler = TestScheduler() + flat = image_from_array( + np.full((32, 32, 3), 128, dtype=np.uint8), + encoding="rgb8", + header=Header(frame_id="camera", stamp=time_from_seconds(0)), + ) + pixels = np.tile((np.indices((32, 32)).sum(axis=0) % 2 * 255)[:, :, None], (1, 1, 3)).astype( + np.uint8 + ) + sharp = image_from_array( + pixels, encoding="rgb8", header=Header(frame_id="camera", stamp=time_from_seconds(0.1)) + ) + latest = image_from_array( + np.zeros((32, 32, 3), dtype=np.uint8), + encoding="rgb8", + header=Header(frame_id="camera", stamp=time_from_seconds(0.3)), + ) + source = scheduler.create_hot_observable( + ReactiveTest.on_next(0.05, flat), + ReactiveTest.on_next(0.15, sharp), + ReactiveTest.on_next(0.35, latest), + ) + observer = scheduler.create_observer() + subscription = skill_module._sharpness_window(10, source, scheduler=scheduler).subscribe( + observer + ) + try: + scheduler.advance_to(0.5) + values = [message.value.value for message in observer.messages] + assert values == [flat, sharp, sharp, latest, latest] + finally: + subscription.dispose() + scheduler.advance_to(1) + assert len(observer.messages) == 5 + assert source.subscriptions[0].unsubscribe == 0.5 diff --git a/dimos/perception/experimental/test_spatial_memory.py b/dimos/perception/experimental/test_spatial_memory.py index 211644847cf..94aa3bab684 100644 --- a/dimos/perception/experimental/test_spatial_memory.py +++ b/dimos/perception/experimental/test_spatial_memory.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +from contextlib import ExitStack import os import shutil import tempfile @@ -22,9 +23,15 @@ from reactivex import of, operators as ops from reactivex.scheduler import ThreadPoolScheduler -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.core.transport import LCMTransport +from dimos.message_utils.image import image_from_array +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.perception.experimental import spatial_perception from dimos.perception.experimental.spatial_perception import SpatialMemory +from dimos.perception.experimental.visual_memory import VisualMemory from dimos.stream.video_provider import VideoProvider @@ -109,7 +116,7 @@ def process_frame(frame): nonlocal frame_counter # Generate a unique position for this frame to ensure minimum distance threshold is met - pos = Pose(frame_counter * 0.5, frame_counter * 0.5, 0) + pos = Vector3(x=frame_counter * 0.5, y=frame_counter * 0.5, z=0.0) transform = {"position": pos, "timestamp": time.time()} frame_counter += 1 @@ -221,11 +228,63 @@ def flaky_get_embedding(image): spatial_memory.process_stream( of( - {"frame": frame, "position": Vector3(1000, 0, 0), "rotation": Vector3()}, - {"frame": frame, "position": Vector3(1001, 0, 0), "rotation": Vector3()}, + { + "frame": frame, + "position": Vector3(x=1000.0, y=0.0, z=0.0), + "rotation": Vector3(x=0.0, y=0.0, z=0.0), + }, + { + "frame": frame, + "position": Vector3(x=1001.0, y=0.0, z=0.0), + "rotation": Vector3(x=0.0, y=0.0, z=0.0), + }, ) ).subscribe(on_next=results.append, on_error=errors.append) assert errors == [] assert [r["position"] for r in results] == [(1001, 0, 0)] assert spatial_memory.stored_frame_count == stored_before + 1 + + +def test_generated_image_and_tf_reach_spatial_backend_with_correct_colors_and_pose( + tmp_path, mocker +): + embedding = mocker.Mock() + embedding.get_embedding.return_value = np.ones(512, dtype=np.float32) / np.sqrt(512) + database = mocker.Mock() + mocker.patch.object(spatial_perception, "ImageEmbeddingProvider", return_value=embedding) + mocker.patch.object(spatial_perception, "SpatialVectorDB", return_value=database) + with ExitStack() as cleanup: + wire = LCMTransport("spatial_tf", TFMessage, url="memq://") + cleanup.callback(wire.stop) + memory = SpatialMemory( + chroma_client=mocker.Mock(), + db_path=None, + visual_memory_path=None, + visual_memory=VisualMemory(output_dir=str(tmp_path)), + min_time_threshold=0, + ) + cleanup.callback(memory.stop) + memory.tf.transport = wire + memory.tfbuffer.receive_transform( + TransformStamped( + header=Header(frame_id="world", stamp=Time(sec=1, nanosec=0)), + child_frame_id="base_link", + transform=Transform( + translation=Vector3(x=1, y=2, z=3), rotation=Quaternion(x=0, y=0, z=0, w=1) + ), + ) + ) + pixels = np.zeros((4, 4, 3), dtype=np.uint8) + pixels[:, :, 0] = 255 + image = image_from_array( + pixels, encoding="rgb8", header=Header(frame_id="camera", stamp=Time(sec=0, nanosec=1)) + ) + memory._receive_frame(image) + memory._process_frame() + database.add_image_vector.assert_called_once() + stored = database.add_image_vector.call_args.kwargs + np.testing.assert_array_equal(stored["image"][0, 0], [0, 0, 255]) + assert [stored["metadata"][key] for key in ["pos_x", "pos_y", "pos_z"]] == [1, 2, 3] + assert [stored["metadata"][key] for key in ["rot_x", "rot_y", "rot_z"]] == [0, 0, 0] + assert memory.stored_frame_count == 1 diff --git a/dimos/perception/fiducial/blueprints/desk_marker_tf.py b/dimos/perception/fiducial/blueprints/desk_marker_tf.py index 0a63f90ca57..68277c7dd31 100644 --- a/dimos/perception/fiducial/blueprints/desk_marker_tf.py +++ b/dimos/perception/fiducial/blueprints/desk_marker_tf.py @@ -27,12 +27,15 @@ from dimos.core.transport import LCMTransport 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.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.message_utils.camera_info import camera_info_from_yaml +from dimos.message_utils.geometry import quaternion_from_euler +from dimos.message_utils.time import time_from_nanoseconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.sensor_msgs.msg import CameraInfo +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.messages.vision_msgs.msg import Detection3DArray from dimos.perception.fiducial.marker_detection_stream_module import MarkerDetectionStreamModule from dimos.perception.fiducial.marker_tf_module import MarkerTfModule @@ -61,8 +64,9 @@ def create_desk_webcam( def create_desk_camera_info( camera_info_yaml: str | Path = DEFAULT_DESK_CAMERA_INFO_YAML, ) -> CameraInfo: - camera_info = CameraInfo.from_yaml(str(camera_info_yaml)) - camera_info.frame_id = DESK_CAMERA_FRAME_ID + camera_info = camera_info_from_yaml( + camera_info_yaml, header=Header(frame_id=DESK_CAMERA_FRAME_ID, stamp=Time(sec=0, nanosec=0)) + ) return camera_info @@ -124,28 +128,30 @@ def stop(self) -> None: super().stop() def publish_static_chain(self) -> None: - ts = time.time() - self._last_publish_ts = ts + stamp = time_from_nanoseconds(time.time_ns()) + self._last_publish_ts = stamp.sec + stamp.nanosec / 1e9 roll, pitch, yaw = self.config.camera_rotation_rpy_rad x, y, z = self.config.camera_translation_m - self.tf.publish( TFMessage( - 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, - ), + transforms=[ + TransformStamped( + header=Header(stamp=stamp, frame_id=self.config.world_frame), + child_frame_id=self.config.base_frame, + transform=Transform( + rotation=Quaternion(w=1, x=0.0, y=0.0, z=0.0), + translation=Vector3(x=0.0, y=0.0, z=0.0), + ), + ), + TransformStamped( + header=Header(stamp=stamp, frame_id=self.config.base_frame), + child_frame_id=self.config.camera_optical_frame, + transform=Transform( + translation=Vector3(x=x, y=y, z=z), + rotation=quaternion_from_euler(roll, pitch, yaw), + ), + ), + ] ) ) @@ -167,8 +173,7 @@ def publish_static_chain(self) -> None: ).transports( { ("detections", MarkerDetectionStreamModule): LCMTransport( - "/marker_detection/detections", - Detection3DArray, + "/marker_detection/detections", Detection3DArray ), } ) diff --git a/dimos/perception/fiducial/blueprints/test_desk_marker_tf.py b/dimos/perception/fiducial/blueprints/test_desk_marker_tf.py index fc7e4ecf639..ae76017ba9a 100644 --- a/dimos/perception/fiducial/blueprints/test_desk_marker_tf.py +++ b/dimos/perception/fiducial/blueprints/test_desk_marker_tf.py @@ -41,7 +41,9 @@ def test_desk_marker_tf_blueprint_declares_static_tf_module() -> None: assert desk_marker_tf.blueprints[2].module is MarkerDetectionStreamModule 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["camera_info"].frame_id == DESK_CAMERA_FRAME_ID + assert ( + desk_marker_tf.blueprints[2].kwargs["camera_info"].header.frame_id == DESK_CAMERA_FRAME_ID + ) assert desk_marker_tf.blueprints[3].module is MarkerTfModule assert ( desk_marker_tf.blueprints[3].kwargs["marker_namespace_prefix"] @@ -49,7 +51,7 @@ def test_desk_marker_tf_blueprint_declares_static_tf_module() -> None: ) assert ( desk_marker_tf.transport_map[("detections", MarkerDetectionStreamModule)].topic.topic - == "/marker_detection/detections" + == "marker_detection/detections/cdr/vision_msgs/msg/Detection3DArray" ) @@ -87,10 +89,10 @@ def test_create_desk_webcam_loads_camera_info_yaml(tmp_path: Path) -> None: 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 + assert camera.config.camera_info.header.frame_id == DESK_CAMERA_FRAME_ID camera_info = create_desk_camera_info(camera_info_yaml) - assert camera_info.frame_id == DESK_CAMERA_FRAME_ID + assert camera_info.header.frame_id == DESK_CAMERA_FRAME_ID assert camera_info.width == 1920 @@ -106,10 +108,11 @@ def test_desk_static_tf_module_publishes_world_to_camera_optical_chain() -> None world_camera = view.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.header.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 + assert world_camera.transform.translation.x == 0.3 + assert world_camera.transform.translation.y == 0.0 + assert world_camera.transform.translation.z == 0.2 finally: + view.dispose() mod.stop() diff --git a/dimos/perception/fiducial/fixture_verification.py b/dimos/perception/fiducial/fixture_verification.py index b5b6b25e373..15e78f73ca5 100644 --- a/dimos/perception/fiducial/fixture_verification.py +++ b/dimos/perception/fiducial/fixture_verification.py @@ -20,6 +20,7 @@ from pathlib import Path from typing import Any +import cv2 import numpy as np from reportlab.lib.pagesizes import A4, LETTER from reportlab.lib.units import mm @@ -178,8 +179,6 @@ def generated_apriltag_board_layout( def detect_apriltag_frame(image_path: Path, dictionary_name: str) -> DetectionResult: """Run OpenCV AprilTag detection on a fixture image.""" - import cv2 - if not hasattr(cv2.aruco, dictionary_name): raise ValueError(f"Unknown ArUco dictionary {dictionary_name!r}") image = cv2.imread(str(image_path), cv2.IMREAD_GRAYSCALE) @@ -193,7 +192,9 @@ def detect_apriltag_frame(image_path: Path, dictionary_name: str) -> DetectionRe corners_by_id: dict[int, np.ndarray] = {} if ids is not None: for corner_set, id_arr in zip(corners, ids, strict=True): - corners_by_id[int(id_arr[0])] = np.asarray(corner_set, dtype=np.float32).reshape(4, 2) + corners_by_id[int(np.asarray(id_arr).reshape(-1)[0])] = np.asarray( + corner_set, dtype=np.float32 + ).reshape(4, 2) height, width = image.shape[:2] return DetectionResult(corners_by_id, width, height) @@ -311,8 +312,6 @@ def verify_board_layout_geometry( This is a 2D PDF-layout check: it fits a homography from generated board corners to detected image corners and measures residuals. """ - import cv2 - source_points: list[np.ndarray] = [] image_points: list[np.ndarray] = [] edges_px: list[float] = [] @@ -419,8 +418,6 @@ def verify_fixture_frame( def _convex_hull_area(points: np.ndarray) -> float: - import cv2 - if len(points) < 3: return 0.0 return float(cv2.contourArea(cv2.convexHull(points.reshape(-1, 1, 2)))) diff --git a/dimos/perception/fiducial/marker_detect.py b/dimos/perception/fiducial/marker_detect.py index 9abb07ff334..b89d4c65145 100644 --- a/dimos/perception/fiducial/marker_detect.py +++ b/dimos/perception/fiducial/marker_detect.py @@ -18,12 +18,16 @@ from typing import Any +import cv2 +import cv2.aruco import numpy as np -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.msgs.sensor_msgs.Image import Image +from dimos.message_utils.geometry import compose_transforms +from dimos.message_utils.image import image_to_bgr +from dimos.message_utils.time import to_seconds +from dimos.messages.geometry_msgs.msg import TransformStamped, Vector3 +from dimos.messages.sensor_msgs.msg import CameraInfo, Image +from dimos.messages.std_msgs.msg import Header from dimos.perception.detection.type.detection3d.marker import Detection3DMarker from dimos.perception.fiducial.marker_pose import ( camera_info_to_cv_matrices, @@ -40,14 +44,14 @@ def detect_markers_in_image( image: Image, *, camera_info: CameraInfo, - world_T_optical: Transform, + world_T_optical: TransformStamped, marker_length_m: float, aruco_dictionary: str, world_frame: str = "world", detect_inverted: bool = False, detector: Any | None = None, - camera_matrix: np.ndarray | None = None, - dist_coeffs: np.ndarray | None = None, + camera_matrix: np.ndarray[Any, np.dtype[Any]] | None = None, + dist_coeffs: np.ndarray[Any, np.dtype[Any]] | None = None, ) -> list[Detection3DMarker]: """Detect markers in one image and return rich world-frame 3D detections.""" if marker_length_m <= 0: @@ -66,24 +70,22 @@ def detect_markers_in_image( if camera_matrix is None or dist_coeffs is None: camera_matrix, dist_coeffs = camera_info_to_cv_matrices(camera_info) - gray = image.to_grayscale().as_numpy() + gray = cv2.cvtColor(image_to_bgr(image), cv2.COLOR_BGR2GRAY) corners, ids, _ = detector.detectMarkers(gray) if ids is None or len(ids) == 0: return [] optical_frame = camera_optical_frame_id(image, camera_info) - t_world_optical = Transform( - translation=world_T_optical.translation, - rotation=world_T_optical.rotation, - frame_id=world_frame, + t_world_optical = TransformStamped( + header=Header(stamp=image.header.stamp, frame_id=world_frame), child_frame_id=optical_frame, - ts=image.ts, + transform=world_T_optical.transform, ) - marker_size = Vector3(marker_length_m, marker_length_m, 0.0) + marker_size = Vector3(x=marker_length_m, y=marker_length_m, z=0.0) detections: list[Detection3DMarker] = [] for corner_set, mid_arr in zip(corners, ids, strict=True): - mid = int(mid_arr[0]) + mid = int(np.asarray(mid_arr).reshape(-1)[0]) pose = estimate_marker_pose( corner_set, marker_length_m, @@ -98,11 +100,10 @@ def detect_markers_in_image( t_optical_marker = rvec_tvec_to_transform( rvec, tvec, - frame_id=optical_frame, + header=Header(stamp=image.header.stamp, frame_id=optical_frame), child_frame_id=f"marker_{mid}", - ts=image.ts, ) - t_world_marker = t_world_optical + t_optical_marker + t_world_marker = compose_transforms(t_world_optical, t_optical_marker) corners_2d = corner_set.reshape(4, 2).astype(np.float32) bbox = marker_corners_to_bbox(corners_2d) @@ -123,13 +124,13 @@ def detect_markers_in_image( class_id=mid, confidence=1.0, name="", - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, - center=t_world_marker.translation, + center=t_world_marker.transform.translation, size=marker_size, transform=t_world_optical, frame_id=world_frame, - orientation=t_world_marker.rotation, + orientation=t_world_marker.transform.rotation, marker_id=mid, corners_px=corners_2d, dictionary=aruco_dictionary, diff --git a/dimos/perception/fiducial/marker_detection_stream_module.py b/dimos/perception/fiducial/marker_detection_stream_module.py index e200326c75a..5623e91c724 100644 --- a/dimos/perception/fiducial/marker_detection_stream_module.py +++ b/dimos/perception/fiducial/marker_detection_stream_module.py @@ -21,10 +21,9 @@ from __future__ import annotations -import time from typing import Any, cast -from pydantic import Field +from pydantic import Field, InstanceOf from reactivex.disposable import Disposable from dimos.core.core import rpc @@ -34,10 +33,11 @@ from dimos.memory.store.null import NullStore from dimos.memory.stream import Stream from dimos.memory.transform import QualityWindow, SpeedLimit -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.message_utils.image import image_sharpness +from dimos.message_utils.time import to_seconds +from dimos.messages.sensor_msgs.msg import CameraInfo, Image +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.messages.vision_msgs.msg import Detection3DArray from dimos.perception.detection.type.detection3d.marker import Detection3DMarker from dimos.perception.fiducial.marker_pose import camera_optical_frame_id, is_fisheye_model from dimos.perception.fiducial.marker_transformer import DetectMarkers, MarkersPerFrame @@ -60,7 +60,7 @@ class MarkerDetectionStreamModuleConfig(ModuleConfig): speed_limit_max_mps: float = Field(0.05, gt=0.0) speed_limit_max_dps: float = Field(15.0, gt=0.0) tf_lookup_tolerance: float = Field(0.5, ge=0.0) - camera_info: CameraInfo | None = None + camera_info: InstanceOf[CameraInfo] | None = None class MarkerDetectionStreamModule(StreamModule[Image, Detection3DArray]): @@ -78,7 +78,7 @@ def __init__(self, **kwargs: Any) -> None: def pipeline(self, stream: Stream[Image]) -> Stream[Detection3DArray]: result: Stream[Any] = stream.transform( - QualityWindow(lambda img: img.sharpness, window=self.config.quality_window_s) + QualityWindow(image_sharpness, window=self.config.quality_window_s) ) if self.config.speed_limit_enabled: result = result.transform( @@ -121,7 +121,7 @@ def _append_image_with_pose(self, stream: Stream[Image], image: Image) -> None: logger.debug("MarkerDetectionStreamModule: no CameraInfo yet; skipping frame") return - ts = getattr(image, "ts", None) or time.time() + ts = to_seconds(image.header.stamp) optical = camera_optical_frame_id(image, info) t_world_optical = self.tfbuffer.get( self.config.world_frame, @@ -142,13 +142,13 @@ def _append_image_with_pose(self, stream: Stream[Image], image: Image) -> None: image, ts=ts, pose=( - t_world_optical.translation.x, - t_world_optical.translation.y, - t_world_optical.translation.z, - t_world_optical.rotation.x, - t_world_optical.rotation.y, - t_world_optical.rotation.z, - t_world_optical.rotation.w, + t_world_optical.transform.translation.x, + t_world_optical.transform.translation.y, + t_world_optical.transform.translation.z, + t_world_optical.transform.rotation.x, + t_world_optical.transform.rotation.y, + t_world_optical.transform.rotation.z, + t_world_optical.transform.rotation.w, ), ) diff --git a/dimos/perception/fiducial/marker_pose.py b/dimos/perception/fiducial/marker_pose.py index bfe7f115e18..049292dd181 100644 --- a/dimos/perception/fiducial/marker_pose.py +++ b/dimos/perception/fiducial/marker_pose.py @@ -16,19 +16,16 @@ from __future__ import annotations -from typing import TYPE_CHECKING +from typing import Any +import cv2 +import cv2.aruco import numpy as np -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.msgs.sensor_msgs.Image import Image - -if TYPE_CHECKING: - import cv2 - import cv2.aruco +from dimos.message_utils.geometry import quaternion_from_matrix +from dimos.messages.geometry_msgs.msg import Transform, TransformStamped, Vector3 +from dimos.messages.sensor_msgs.msg import CameraInfo, Image +from dimos.messages.std_msgs.msg import Header _FISHEYE_MODELS = frozenset({"equidistant", "fisheye", "kannala_brandt"}) @@ -38,26 +35,28 @@ def is_fisheye_model(distortion_model: str | None) -> bool: return (distortion_model or "").strip().lower() in _FISHEYE_MODELS -def camera_info_to_cv_matrices(camera_info: CameraInfo) -> tuple[np.ndarray, np.ndarray]: +def camera_info_to_cv_matrices( + camera_info: CameraInfo, +) -> tuple[np.ndarray[Any, np.dtype[Any]], np.ndarray[Any, np.dtype[Any]]]: """Build OpenCV ``cameraMatrix`` and ``distCoeffs`` from ``CameraInfo``.""" - k = np.array(camera_info.K, dtype=np.float64).reshape(3, 3) - d = np.array(camera_info.D if camera_info.D else [], dtype=np.float64).reshape(-1, 1) + k = np.array(camera_info.k, dtype=np.float64).reshape(3, 3) + d = np.array(camera_info.d, dtype=np.float64).reshape(-1, 1) return k, d def camera_optical_frame_id(image: Image, camera_info: CameraInfo) -> str: """Frame in which image pixels and intrinsics apply (optical convention in ROS). - Prefer ``Image.frame_id`` so TF lookups match the stream that produced the - pixels. Fall back to ``CameraInfo.frame_id``, then a conventional default. + Prefer ``Image.header.frame_id`` so TF lookups match the stream that produced the + pixels. Fall back to ``CameraInfo.header.frame_id``, then a conventional default. """ - for fid in (image.frame_id, camera_info.frame_id): + for fid in (image.header.frame_id, camera_info.header.frame_id): if fid and fid.strip(): return fid.strip() return "camera_optical" -def _aruco_marker_object_points(marker_length_m: float) -> np.ndarray: +def _aruco_marker_object_points(marker_length_m: float) -> np.ndarray[Any, np.dtype[Any]]: """Corner order matches OpenCV ArUco / solvePnP convention (planar square, Z=0).""" h = marker_length_m / 2.0 return np.array( @@ -72,33 +71,31 @@ def _aruco_marker_object_points(marker_length_m: float) -> np.ndarray: def estimate_marker_pose( - corners_px: np.ndarray, + corners_px: np.ndarray[Any, np.dtype[Any]], marker_length_m: float, - camera_matrix: np.ndarray, - dist_coeffs: np.ndarray, + camera_matrix: np.ndarray[Any, np.dtype[Any]], + dist_coeffs: np.ndarray[Any, np.dtype[Any]], *, distortion_model: str | None = None, -) -> tuple[np.ndarray, np.ndarray] | None: +) -> tuple[np.ndarray[Any, np.dtype[Any]], np.ndarray[Any, np.dtype[Any]]] | None: """Return ``(rvec, tvec)`` for camera optical <- marker from undistorted solvePnP. For fisheye/equidistant intrinsics, corners are first undistorted into the same pinhole ``K`` so the radtan-only ``solvePnP`` sees pinhole-equivalent pixels. Otherwise the radtan ``dist_coeffs`` are passed straight through. """ - import cv2 - obj = _aruco_marker_object_points(marker_length_m) - img: np.ndarray = corners_px.reshape(4, 1, 2).astype(np.float32) + img: np.ndarray[Any, np.dtype[Any]] = corners_px.reshape(4, 1, 2).astype(np.float32) if is_fisheye_model(distortion_model): d_flat = np.asarray(dist_coeffs, dtype=np.float64).reshape(-1) if d_flat.size < 4: raise ValueError( f"Fisheye/equidistant distortion model requires at least 4 coefficients; " - f"got {d_flat.size}. Check CameraInfo.D." + f"got {d_flat.size}. Check CameraInfo.d." ) d_fisheye = d_flat[:4].reshape(4, 1) img = cv2.fisheye.undistortPoints(img, camera_matrix, d_fisheye, P=camera_matrix) - solve_dist: np.ndarray = np.zeros((0, 1), dtype=np.float64) + solve_dist: np.ndarray[Any, np.dtype[Any]] = np.zeros((0, 1), dtype=np.float64) else: solve_dist = dist_coeffs ok, rvec, tvec = cv2.solvePnP( @@ -114,25 +111,22 @@ def estimate_marker_pose( def rvec_tvec_to_transform( - rvec: np.ndarray, - tvec: np.ndarray, + rvec: np.ndarray[Any, np.dtype[Any]], + tvec: np.ndarray[Any, np.dtype[Any]], *, - frame_id: str, + header: Header, child_frame_id: str, - ts: float, -) -> Transform: +) -> TransformStamped: """Build ``Transform`` for ``frame_id`` <- ``child_frame_id`` (camera <- marker).""" - import cv2 - rot_mat, _ = cv2.Rodrigues(rvec) - quat = Quaternion.from_rotation_matrix(rot_mat) + quat = quaternion_from_matrix(np.asarray(rot_mat, dtype=np.float64)) tx, ty, tz = tvec.reshape(3) - return Transform( - translation=Vector3(float(tx), float(ty), float(tz)), - rotation=quat, - frame_id=frame_id, + return TransformStamped( + header=header, child_frame_id=child_frame_id, - ts=ts, + transform=Transform( + translation=Vector3(x=float(tx), y=float(ty), z=float(tz)), rotation=quat + ), ) @@ -140,8 +134,6 @@ def create_aruco_detector( dictionary_name: str, *, detect_inverted: bool = False ) -> cv2.aruco.ArucoDetector: """Build a detector; `detect_inverted` also accepts light-on-dark (negative) markers.""" - import cv2.aruco - if not hasattr(cv2.aruco, dictionary_name): raise ValueError(f"Unknown ArUco dictionary {dictionary_name!r}") dictionary = cv2.aruco.getPredefinedDictionary(getattr(cv2.aruco, dictionary_name)) @@ -150,7 +142,9 @@ def create_aruco_detector( return cv2.aruco.ArucoDetector(dictionary, parameters) -def marker_corners_to_bbox(corners_px: np.ndarray) -> tuple[float, float, float, float]: +def marker_corners_to_bbox( + corners_px: np.ndarray[Any, np.dtype[Any]], +) -> tuple[float, float, float, float]: """Return the axis-aligned image bbox around a marker's four pixel corners.""" corners_2d = np.asarray(corners_px, dtype=np.float32).reshape(4, 2) xy_min = corners_2d.min(axis=0) @@ -159,12 +153,12 @@ def marker_corners_to_bbox(corners_px: np.ndarray) -> tuple[float, float, float, def marker_reprojection_error( - corners_px: np.ndarray, + corners_px: np.ndarray[Any, np.dtype[Any]], marker_length_m: float, - camera_matrix: np.ndarray, - dist_coeffs: np.ndarray, - rvec: np.ndarray, - tvec: np.ndarray, + camera_matrix: np.ndarray[Any, np.dtype[Any]], + dist_coeffs: np.ndarray[Any, np.dtype[Any]], + rvec: np.ndarray[Any, np.dtype[Any]], + tvec: np.ndarray[Any, np.dtype[Any]], *, distortion_model: str | None = None, ) -> float: @@ -173,9 +167,9 @@ def marker_reprojection_error( Fisheye/equidistant inputs are compared in the same undistorted pinhole pixel space used by :func:`estimate_marker_pose`. """ - import cv2 - - observed: np.ndarray = np.asarray(corners_px, dtype=np.float32).reshape(4, 1, 2) + observed: np.ndarray[Any, np.dtype[Any]] = np.asarray(corners_px, dtype=np.float32).reshape( + 4, 1, 2 + ) project_dist = dist_coeffs if is_fisheye_model(distortion_model): @@ -183,7 +177,7 @@ def marker_reprojection_error( if d_flat.size < 4: raise ValueError( f"Fisheye/equidistant distortion model requires at least 4 coefficients; " - f"got {d_flat.size}. Check CameraInfo.D." + f"got {d_flat.size}. Check CameraInfo.d." ) d_fisheye = d_flat[:4].reshape(4, 1) observed = cv2.fisheye.undistortPoints( diff --git a/dimos/perception/fiducial/marker_tf_module.py b/dimos/perception/fiducial/marker_tf_module.py index f5184ab9430..45db2507933 100644 --- a/dimos/perception/fiducial/marker_tf_module.py +++ b/dimos/perception/fiducial/marker_tf_module.py @@ -45,12 +45,10 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -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.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.vision_msgs.Detection3D import Detection3D -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.messages.vision_msgs.msg import Detection3D, Detection3DArray from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -83,22 +81,19 @@ def _marker_child_frame(self, marker_id: str) -> str: return f"{p}/{name}" if p else name def _process_detections(self, detections: Detection3DArray) -> None: - if detections.detections_length == 0: + if not detections.detections: return - - marker_detections = detections.detections[: detections.detections_length] - if not marker_detections: - return - + marker_detections = detections.detections markers_parent = self._markers_parent_frame() - ts = detections.ts - out: list[Transform] = [ - 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, + stamp = detections.header.stamp + out = [ + TransformStamped( + header=Header(stamp=stamp, frame_id=self.config.world_frame), child_frame_id=markers_parent, - ts=ts, + transform=Transform( + rotation=Quaternion(w=1, x=0.0, y=0.0, z=0.0), + translation=Vector3(x=0.0, y=0.0, z=0.0), + ), ) ] @@ -110,12 +105,15 @@ def _process_detections(self, detections: Detection3DArray) -> None: pose = detection.bbox.center out.append( - Transform( - translation=pose.position, - rotation=pose.orientation, - frame_id=markers_parent, + TransformStamped( + header=Header(stamp=stamp, frame_id=markers_parent), child_frame_id=self._marker_child_frame(marker_id), - ts=ts, + transform=Transform( + translation=Vector3( + x=pose.position.x, y=pose.position.y, z=pose.position.z + ), + rotation=pose.orientation, + ), ) ) @@ -123,7 +121,7 @@ def _process_detections(self, detections: Detection3DArray) -> None: # In that case, skip TF entirely rather than publishing only the # namespace anchor without any marker child frames. if len(out) > 1: - self.tf.publish(TFMessage(*out)) + self.tf.publish(TFMessage(transforms=out)) @staticmethod def _marker_id_from_detection(detection: Detection3D) -> str | None: @@ -131,7 +129,7 @@ def _marker_id_from_detection(detection: Detection3D) -> str | None: if marker_id: return marker_id - for result in detection.results[: detection.results_length]: + for result in detection.results: class_id = str(result.hypothesis.class_id).strip() if ":" not in class_id: continue diff --git a/dimos/perception/fiducial/marker_transformer.py b/dimos/perception/fiducial/marker_transformer.py index 0a0f7bfd036..1a2ba8cc520 100644 --- a/dimos/perception/fiducial/marker_transformer.py +++ b/dimos/perception/fiducial/marker_transformer.py @@ -32,17 +32,16 @@ from collections.abc import Callable import dataclasses import math -from typing import TYPE_CHECKING, Any, cast +from typing import TYPE_CHECKING, Any, TypeAlias, cast import numpy as np from dimos.memory.transform import Transformer -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.msgs.sensor_msgs.Image import Image -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.message_utils.time import to_nanoseconds +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.sensor_msgs.msg import CameraInfo, Image +from dimos.messages.std_msgs.msg import Header +from dimos.messages.vision_msgs.msg import Detection3DArray from dimos.perception.detection.type.detection3d.imageDetections3D import ImageDetections3D from dimos.perception.detection.type.detection3d.marker import Detection3DMarker from dimos.perception.fiducial.marker_detect import ( @@ -64,7 +63,7 @@ logger = setup_logger() -CameraInfoSource = CameraInfo | Callable[[], CameraInfo | None] | None +CameraInfoSource: TypeAlias = CameraInfo | Callable[[], CameraInfo | None] | None def _camera_info_key(info: CameraInfo) -> tuple[Any, ...]: @@ -73,25 +72,24 @@ def _camera_info_key(info: CameraInfo) -> tuple[Any, ...]: info.width, info.height, info.distortion_model, - tuple(info.K), - tuple(info.D), + tuple(info.k), + tuple(info.d), ) def _pose_tuple_to_transform( pose: tuple[float, float, float, float, float, float, float], *, - frame_id: str, + header: Header, child_frame_id: str, - ts: float, -) -> Transform: +) -> TransformStamped: x, y, z, qx, qy, qz, qw = pose - return Transform( - translation=Vector3(x, y, z), - rotation=Quaternion(qx, qy, qz, qw), - frame_id=frame_id, + return TransformStamped( + header=header, child_frame_id=child_frame_id, - ts=ts, + transform=Transform( + translation=Vector3(x=x, y=y, z=z), rotation=Quaternion(x=qx, y=qy, z=qz, w=qw) + ), ) @@ -124,10 +122,10 @@ def _average_marker_pose( norm = math.sqrt(qsx * qsx + qsy * qsy + qsz * qsz + qsw * qsw) if norm < 1e-12: # Signs cancelled exactly — pathological, fall back to the hemisphere ref. - return (Vector3(cx, cy, cz), ref) + return (Vector3(x=cx, y=cy, z=cz), ref) return ( - Vector3(cx, cy, cz), - Quaternion(qsx / norm, qsy / norm, qsz / norm, qsw / norm), + Vector3(x=cx, y=cy, z=cz), + Quaternion(x=qsx / norm, y=qsy / norm, z=qsz / norm, w=qsw / norm), ) @@ -158,8 +156,8 @@ def __init__( self._detector = create_aruco_detector(aruco_dictionary, detect_inverted=detect_inverted) self._camera_info_key: tuple[Any, ...] | None = None self._resolved_camera_info: CameraInfo | None = None - self._cam_mtx: np.ndarray | None = None - self._dist: np.ndarray | None = None + self._cam_mtx: np.ndarray[Any, np.dtype[Any]] | None = None + self._dist: np.ndarray[Any, np.dtype[Any]] | None = None # Per marker_id sliding-window buffer of raw detections, used to emit # smoothed pose updates when ``smoothing_window > 0``. self._buffers: dict[int, TimestampedBufferCollection[Detection3DMarker]] = {} @@ -220,9 +218,8 @@ def __call__( optical_frame = camera_optical_frame_id(image, info) t_world_optical = _pose_tuple_to_transform( pose_tuple, - frame_id=self.world_frame, + header=Header(stamp=image.header.stamp, frame_id=self.world_frame), child_frame_id=optical_frame, - ts=obs.ts, ) detections = _detect_markers_in_image( @@ -264,12 +261,10 @@ def __call__( track_id = -1 det = dataclasses.replace(det, track_id=track_id) - yielded_pose = Transform( - translation=det.center, - rotation=det.orientation, - frame_id=self.world_frame, + yielded_pose = TransformStamped( + header=Header(stamp=image.header.stamp, frame_id=self.world_frame), child_frame_id=f"marker_{mid}", - ts=obs.ts, + transform=Transform(translation=det.center, rotation=det.orientation), ) yielded_det = det @@ -289,15 +284,13 @@ def __call__( yielded_det = dataclasses.replace( det, center=avg_center, orientation=avg_orient, transform=None ) - yielded_pose = Transform( - translation=avg_center, - rotation=avg_orient, - frame_id=self.world_frame, + yielded_pose = TransformStamped( + header=Header(stamp=image.header.stamp, frame_id=self.world_frame), child_frame_id=f"marker_{mid}", - ts=obs.ts, + transform=Transform(translation=avg_center, rotation=avg_orient), ) - yield obs.derive(data=yielded_det, pose=yielded_pose).tag( + yield obs.derive(data=yielded_det, pose=yielded_pose.transform).tag( marker_id=mid, track_id=track_id, marker_frame_image=image, @@ -326,16 +319,16 @@ def __call__( ) -> Iterator[Observation[Detection3DArray]]: pending: list[Detection3DMarker] = [] pending_obs: Observation[Detection3DMarker | None] | None = None - pending_ts: float | None = None + pending_ns: int | None = None def flush() -> Observation[Detection3DArray] | None: - nonlocal pending, pending_obs, pending_ts + nonlocal pending, pending_obs, pending_ns if pending_obs is None: return None result = self._to_array_observation(pending_obs, pending) pending = [] pending_obs = None - pending_ts = None + pending_ns = None return result for obs in upstream: @@ -347,14 +340,15 @@ def flush() -> Observation[Detection3DArray] | None: yield self._to_array_observation(obs, []) continue - if pending_ts is not None and obs.ts != pending_ts: + source_ns = to_nanoseconds(det.image.header.stamp) + if pending_ns is not None and source_ns != pending_ns: flushed = flush() if flushed is not None: yield flushed if pending_obs is None: pending_obs = obs - pending_ts = obs.ts + pending_ns = source_ns pending.append(det) expected_count = obs.tags.get("marker_frame_count") @@ -401,12 +395,12 @@ def _source_pose( if detections and detections[0].transform is not None: transform = detections[0].transform return ( - transform.translation.x, - transform.translation.y, - transform.translation.z, - transform.rotation.x, - transform.rotation.y, - transform.rotation.z, - transform.rotation.w, + transform.transform.translation.x, + transform.transform.translation.y, + transform.transform.translation.z, + transform.transform.rotation.x, + transform.transform.rotation.y, + transform.transform.rotation.z, + transform.transform.rotation.w, ) return obs.pose diff --git a/dimos/perception/fiducial/test_fixture_verification.py b/dimos/perception/fiducial/test_fixture_verification.py index 295fd1ef375..34aa75236f5 100644 --- a/dimos/perception/fiducial/test_fixture_verification.py +++ b/dimos/perception/fiducial/test_fixture_verification.py @@ -18,11 +18,12 @@ import numpy as np import pytest -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.msgs.sensor_msgs.Image import Image +from dimos.message_utils.camera_info import camera_info_from_intrinsics +from dimos.message_utils.geometry import transform_matrix +from dimos.message_utils.image import image_from_array +from dimos.message_utils.time import time_from_seconds +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.std_msgs.msg import Header from dimos.perception.detection.type.detection3d.imageDetections3D import ImageDetections3D from dimos.perception.fiducial.fixture_verification import ( BoardLayout, @@ -146,7 +147,9 @@ def test_synthetic_packed_board_detects_all_twelve_ids_and_layout_homography( corners_by_id: dict[int, np.ndarray] = {} for corner_set, id_arr in zip(corners, ids, strict=True): - corners_by_id[int(id_arr[0])] = np.asarray(corner_set, dtype=np.float32).reshape(4, 2) + corners_by_id[int(np.asarray(id_arr).reshape(-1)[0])] = np.asarray( + corner_set, dtype=np.float32 + ).reshape(4, 2) geom = verify_board_layout_geometry(corners_by_id, layout) assert geom.ok, f"layout homography failed p95={geom.layout_error_px_p95}" @@ -159,24 +162,17 @@ def test_marker_tf_replay_synthetic_packed_board_publishes_twelve_markers( width, height = 1920, 1080 bgr = _synthetic_packed_apriltag_board_bgr(layout, width=width, height=height) ts = 10_000.0 - cam_info = CameraInfo.from_intrinsics( - 1400.0, - 1400.0, - width / 2.0, - height / 2.0, - width, - height, - frame_id="camera_optical", + header = Header(stamp=time_from_seconds(ts), frame_id="camera_optical") + cam_info = camera_info_from_intrinsics( + fx=1400, fy=1400, cx=width / 2, cy=height / 2, width=width, height=height, header=header ) - cam_info.ts = ts - - image = Image.from_opencv(bgr, frame_id="camera_optical", ts=ts) - world_T_optical = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="world", + image = image_from_array(bgr, encoding="bgr8", header=header) + world_T_optical = TransformStamped( + header=Header(stamp=header.stamp, frame_id="world"), child_frame_id="camera_optical", - ts=ts, + transform=Transform( + rotation=Quaternion(w=1, x=0.0, y=0.0, z=0.0), translation=Vector3(x=0.0, y=0.0, z=0.0) + ), ) detections = detect_markers_in_image( image, @@ -199,8 +195,9 @@ def test_marker_tf_replay_synthetic_packed_board_publishes_twelve_markers( for tag_id in range(12): tr = view.get(marker_parent, f"fixture/marker_{tag_id}", ts, 0.1) assert tr is not None, f"missing marker {tag_id}" - assert np.all(np.isfinite(tr.to_matrix())) + assert np.all(np.isfinite(transform_matrix(tr.transform))) finally: + view.dispose() mod.stop() diff --git a/dimos/perception/fiducial/test_generated_marker_detect.py b/dimos/perception/fiducial/test_generated_marker_detect.py new file mode 100644 index 00000000000..45d398f458e --- /dev/null +++ b/dimos/perception/fiducial/test_generated_marker_detect.py @@ -0,0 +1,79 @@ +# Copyright 2025-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 + +import cv2 +from dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode +import numpy as np +import pytest + +from dimos.message_utils.image import image_from_array +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.sensor_msgs.msg import CameraInfo, Image, RegionOfInterest +from dimos.messages.std_msgs.msg import Header +from dimos.messages.vision_msgs.msg import Detection3D +from dimos.perception.fiducial.marker_detect import detect_markers_in_image + + +def test_generated_image_detects_real_aruco_and_composes_pose(): + dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) + pixels = np.full((400, 400), 255, dtype=np.uint8) + pixels[100:300, 100:300] = cv2.aruco.generateImageMarker(dictionary, 7, 200) + header = Header(frame_id="camera", stamp=Time(sec=1700000000, nanosec=123456789)) + image = image_from_array(pixels, encoding="mono8", header=header) + image = cdr_decode(cdr_encode(image), Image) + camera = CameraInfo( + width=400, + height=400, + k=np.array([400, 0, 200, 0, 400, 200, 0, 0, 1], dtype=np.float64), + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + distortion_model="", + d=np.array([], dtype=np.float64), + r=np.zeros(9, dtype=np.float64), + p=np.zeros(12, dtype=np.float64), + binning_x=0, + binning_y=0, + roi=RegionOfInterest(x_offset=0, y_offset=0, height=0, width=0, do_rectify=False), + ) + tf = TransformStamped( + header=Header(frame_id="world", stamp=Time(sec=0, nanosec=0)), + child_frame_id="camera", + transform=Transform( + translation=Vector3(x=2, y=0.0, z=0.0), rotation=Quaternion(w=1, x=0.0, y=0.0, z=0.0) + ), + ) + found = detect_markers_in_image( + image, + camera_info=camera, + world_T_optical=tf, + marker_length_m=0.2, + aruco_dictionary="DICT_4X4_50", + ) + assert len(found) == 1 + wire = cdr_decode(cdr_encode(found[0].to_detection3d_msg()), Detection3D) + assert wire.id == "7" + assert wire.results[0].hypothesis.class_id == "DICT_4X4_50:7" + assert wire.header.stamp == header.stamp + assert wire.header.frame_id == "world" + assert wire.bbox.center.position.x == pytest.approx(2, abs=0.01) + assert wire.bbox.center.position.z == pytest.approx(0.4, abs=0.01) + + +def test_documented_fiducial_example_executes(): + guide = Path(__file__).resolve().parents[3] / "docs/capabilities/perception/index.md" + section = guide.read_text().split("## Fiducial markers\n", 1)[1].split("\n## ", 1)[0] + example = section.split("```python\n", 1)[1].split("```", 1)[0] + exec(compile(example, str(guide), "exec"), {}) diff --git a/dimos/perception/fiducial/test_helpers.py b/dimos/perception/fiducial/test_helpers.py index 12b4f890450..c0d00749d11 100644 --- a/dimos/perception/fiducial/test_helpers.py +++ b/dimos/perception/fiducial/test_helpers.py @@ -19,33 +19,37 @@ import cv2 import numpy as np -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.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.message_utils.image import image_from_array +from dimos.message_utils.time import time_from_seconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.sensor_msgs.msg import CameraInfo, Image, RegionOfInterest +from dimos.messages.std_msgs.msg import Header def camera_info(ts: float = 10.0) -> CameraInfo: - info = CameraInfo.from_intrinsics( - fx=600.0, - fy=600.0, - cx=320.0, - cy=240.0, + info = CameraInfo( width=640, height=480, - frame_id="camera_optical", + k=np.array([600, 0, 320, 0, 600, 240, 0, 0, 1], dtype=np.float64), + header=Header(frame_id="camera_optical", stamp=Time(sec=0, nanosec=0)), + distortion_model="", + d=np.array([], dtype=np.float64), + r=np.zeros(9, dtype=np.float64), + p=np.zeros(12, dtype=np.float64), + binning_x=0, + binning_y=0, + roi=RegionOfInterest(x_offset=0, y_offset=0, height=0, width=0, do_rectify=False), ) - info.ts = ts + info.header.stamp = time_from_seconds(ts) return info def blank_image(ts: float = 10.0) -> Image: - return Image( - data=np.full((480, 640, 3), 255, dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera_optical", - ts=ts, + return image_from_array( + np.full((480, 640, 3), 255, dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera_optical", stamp=time_from_seconds(ts)), ) @@ -70,19 +74,19 @@ def synthetic_marker_image(marker_id: int = 7, ts: float = 10.0, inverted: bool y0 = (canvas.shape[0] - side_px) // 2 x0 = (canvas.shape[1] - side_px) // 2 canvas[y0 : y0 + side_px, x0 : x0 + side_px] = tile - return Image( - data=cv2.cvtColor(canvas, cv2.COLOR_GRAY2BGR), - format=ImageFormat.BGR, - frame_id="camera_optical", - ts=ts, + return image_from_array( + cv2.cvtColor(canvas, cv2.COLOR_GRAY2BGR), + encoding="bgr8", + header=Header(frame_id="camera_optical", stamp=time_from_seconds(ts)), ) -def world_T_optical(ts: float = 10.0) -> Transform: - return Transform( - translation=Vector3(1.0, 2.0, 3.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="world", +def world_T_optical(ts: float = 10.0) -> TransformStamped: + return TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(ts)), child_frame_id="camera_optical", - ts=ts, + transform=Transform( + translation=Vector3(x=1.0, y=2.0, z=3.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) diff --git a/dimos/perception/fiducial/test_marker_detection_stream_module.py b/dimos/perception/fiducial/test_marker_detection_stream_module.py index 59779c5fc24..53038019987 100644 --- a/dimos/perception/fiducial/test_marker_detection_stream_module.py +++ b/dimos/perception/fiducial/test_marker_detection_stream_module.py @@ -14,18 +14,23 @@ from __future__ import annotations +from contextlib import ExitStack from typing import Any import numpy as np import pytest +from dimos.core.transport import LCMTransport from dimos.memory.store.memory import MemoryStore +from dimos.memory.transform import SpeedLimit from dimos.memory.type.observation import Observation -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.Image import Image -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.message_utils.time import time_from_seconds, to_seconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.messages.vision_msgs.msg import Detection3DArray from dimos.perception.detection.type.detection3d.marker import Detection3DMarker from dimos.perception.fiducial.marker_detection_stream_module import MarkerDetectionStreamModule from dimos.perception.fiducial.marker_transformer import MarkersPerFrame @@ -36,6 +41,21 @@ ) +@pytest.fixture +def tf_module_factory(): + with ExitStack() as cleanup: + + def create(**kwargs): + module = MarkerDetectionStreamModule(marker_length_m=0.18, **kwargs) + cleanup.callback(module.stop) + transport = LCMTransport("fiducial_tf", TFMessage, url="memq://") + cleanup.callback(transport.stop) + module.tf.transport = transport + return module + + yield create + + def _marker(image: Image, marker_id: int) -> Detection3DMarker: return Detection3DMarker( bbox=(10.0 + marker_id, 20.0, 40.0 + marker_id, 50.0), @@ -43,19 +63,22 @@ def _marker(image: Image, marker_id: int) -> Detection3DMarker: class_id=marker_id, confidence=1.0, name="", - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, - center=Vector3(float(marker_id), 2.0, 3.0), - size=Vector3(0.18, 0.18, 0.0), - transform=Transform( - translation=Vector3(1.0, 2.0, 3.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="world", + center=Vector3(x=float(marker_id), y=2.0, z=3.0), + size=Vector3(x=0.18, y=0.18, z=0.0), + transform=TransformStamped( + header=Header( + frame_id="world", stamp=time_from_seconds(to_seconds(image.header.stamp)) + ), child_frame_id="camera_optical", - ts=image.ts, + transform=Transform( + translation=Vector3(x=1.0, y=2.0, z=3.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ), frame_id="world", - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), marker_id=marker_id, corners_px=np.array( [[10.0, 20.0], [40.0, 20.0], [40.0, 50.0], [10.0, 50.0]], @@ -82,7 +105,7 @@ def _marker_obs( tags["marker_frame_index"] = marker_index return Observation( id=obs_id, - ts=image.ts, + ts=to_seconds(image.header.stamp), data_type=Detection3DMarker if marker is not None else type(None), pose=(1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0), tags=tags, @@ -120,15 +143,15 @@ def test_markers_per_frame_groups_markers_and_preserves_empty_frames() -> None: assert len(outputs) == 2 first = outputs[0].data assert first.header.frame_id == "world" - assert first.ts == pytest.approx(image.ts) - assert first.detections_length == 2 + assert to_seconds(first.header.stamp) == pytest.approx(to_seconds(image.header.stamp)) + assert len(first.detections) == 2 assert [det.id for det in first.detections] == ["7", "42"] assert outputs[0].pose_tuple == pytest.approx((1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0)) empty = outputs[1].data assert empty.header.frame_id == "world" - assert empty.ts == pytest.approx(empty_image.ts) - assert empty.detections_length == 0 + assert to_seconds(empty.header.stamp) == pytest.approx(to_seconds(empty_image.header.stamp)) + assert len(empty.detections) == 0 assert empty.detections == [] @@ -140,7 +163,7 @@ def test_marker_detection_stream_pipeline_outputs_arrays_for_marker_and_empty_fr module = MarkerDetectionStreamModule( marker_length_m=marker_length_m, - camera_info=camera_info(marker_image.ts), + camera_info=camera_info(to_seconds(marker_image.header.stamp)), quality_window_s=0.01, ) try: @@ -148,12 +171,12 @@ def test_marker_detection_stream_pipeline_outputs_arrays_for_marker_and_empty_fr stream = store.stream("color_image", Image) stream.append( marker_image, - ts=marker_image.ts, + ts=to_seconds(marker_image.header.stamp), pose=(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0), ) stream.append( empty_image, - ts=empty_image.ts, + ts=to_seconds(empty_image.header.stamp), pose=(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0), ) @@ -162,15 +185,17 @@ def test_marker_detection_stream_pipeline_outputs_arrays_for_marker_and_empty_fr module.stop() assert len(outputs) == 2 - assert outputs[0].detections_length == 1 + assert len(outputs[0].detections) == 1 assert outputs[0].detections[0].id == str(marker_id) assert outputs[0].detections[0].results[0].hypothesis.class_id == ( f"DICT_APRILTAG_36h11:{marker_id}" ) assert outputs[0].detections[0].bbox.size.x == pytest.approx(marker_length_m) - assert outputs[1].ts == pytest.approx(empty_image.ts) - assert outputs[1].detections_length == 0 + assert to_seconds(outputs[1].header.stamp) == pytest.approx( + to_seconds(empty_image.header.stamp) + ) + assert len(outputs[1].detections) == 0 assert outputs[1].detections == [] @@ -199,7 +224,7 @@ def run_pipeline(*, speed_limit_enabled: bool) -> list[Detection3DArray]: with MemoryStore() as store: stream = store.stream("color_image", Image) for image, pose in zip(images, poses, strict=True): - stream.append(image, ts=image.ts, pose=pose) + stream.append(image, ts=to_seconds(image.header.stamp), pose=pose) return [obs.data for obs in module.pipeline(stream).to_list()] finally: module.stop() @@ -207,47 +232,30 @@ def run_pipeline(*, speed_limit_enabled: bool) -> list[Detection3DArray]: disabled = run_pipeline(speed_limit_enabled=False) enabled = run_pipeline(speed_limit_enabled=True) - assert [msg.ts for msg in disabled] == pytest.approx([10.0, 11.0, 12.0]) - assert all(msg.detections_length == 0 for msg in disabled) - assert [msg.ts for msg in enabled] == pytest.approx([12.0]) - assert enabled[0].detections_length == 0 + assert [to_seconds(msg.header.stamp) for msg in disabled] == pytest.approx([10.0, 11.0, 12.0]) + assert all(len(msg.detections) == 0 for msg in disabled) + assert [to_seconds(msg.header.stamp) for msg in enabled] == pytest.approx([12.0]) + assert len(enabled[0].detections) == 0 -def test_append_image_with_pose_uses_camera_optical_tf_without_recomputing_pose() -> None: - image = blank_image(ts=12.0) - info = camera_info(image.ts) - t_world_optical = Transform( - translation=Vector3(4.0, 5.0, 6.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="world", +@pytest.mark.parametrize("timestamp", [0.0, 12.0]) +def test_append_image_with_pose_uses_camera_optical_tf_without_recomputing_pose( + timestamp, mocker, tf_module_factory +) -> None: + image = blank_image(ts=timestamp) + info = camera_info(to_seconds(image.header.stamp)) + t_world_optical = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(to_seconds(image.header.stamp))), child_frame_id="camera_optical", - ts=image.ts, + transform=Transform( + translation=Vector3(x=4.0, y=5.0, z=6.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) - class FakeTf: - def __init__(self) -> None: - self.calls: list[tuple[str, str, float | None, float | None]] = [] - - def get( - self, - parent_frame: str, - child_frame: str, - time_point: float | None = None, - time_tolerance: float | None = None, - ) -> Transform: - self.calls.append((parent_frame, child_frame, time_point, time_tolerance)) - return t_world_optical - - def dispose(self) -> None: - pass - - module = MarkerDetectionStreamModule( - marker_length_m=0.18, - camera_info=info, - tf_lookup_tolerance=0.25, - ) - fake_tf = FakeTf() - module._tf = fake_tf + module = tf_module_factory(camera_info=info, tf_lookup_tolerance=0.25) + module.tfbuffer.receive_transform(t_world_optical) + lookup = mocker.spy(module.tfbuffer, "get") try: with MemoryStore() as store: stream = store.stream("color_image", Image) @@ -258,14 +266,14 @@ def dispose(self) -> None: finally: module.stop() - assert fake_tf.calls == [("world", "camera_optical", image.ts, 0.25)] + lookup.assert_called_once_with("world", "camera_optical", to_seconds(image.header.stamp), 0.25) assert len(observations) == 1 assert observations[0].data is image - assert observations[0].ts == pytest.approx(image.ts) + assert observations[0].ts == pytest.approx(to_seconds(image.header.stamp)) assert observations[0].pose_tuple == pytest.approx((4.0, 5.0, 6.0, 0.0, 0.0, 0.0, 1.0)) -def test_append_image_with_pose_skips_withoutcamera_info_or_tf() -> None: +def test_append_image_with_pose_skips_withoutcamera_info_or_tf(mocker, tf_module_factory) -> None: image = blank_image(ts=13.0) module = MarkerDetectionStreamModule(marker_length_m=0.18) @@ -277,20 +285,8 @@ def test_append_image_with_pose_skips_withoutcamera_info_or_tf() -> None: finally: module.stop() - class MissingTf: - def __init__(self) -> None: - self.calls = 0 - - def get(self, *args: Any, **kwargs: Any) -> None: - self.calls += 1 - return None - - def dispose(self) -> None: - pass - - missing_tf = MissingTf() - module = MarkerDetectionStreamModule(marker_length_m=0.18, camera_info=camera_info(image.ts)) - module._tf = missing_tf + module = tf_module_factory(camera_info=camera_info(to_seconds(image.header.stamp))) + lookup = mocker.spy(module.tfbuffer, "get") try: with MemoryStore() as store: stream = store.stream("color_image", Image) @@ -299,4 +295,36 @@ def dispose(self) -> None: finally: module.stop() - assert missing_tf.calls == 1 + lookup.assert_called_once_with("world", "camera_optical", 13.0, 0.5) + + +def test_generated_speed_limit_handles_quaternion_sign_and_rotation(): + image = blank_image() + poses = [(0, 0, 0, 0, 0, 0, 1), (0, 0, 0, 0, 0, 0, -1), (0, 0, 0, 0, 0, 1, 0)] + observations = [ + Observation(id=i, ts=float(i), data_type=Image, _data=image, pose=pose) + for i, pose in enumerate(poses) + ] + accepted = list(SpeedLimit(max_mps=1, max_dps=15)(iter(observations))) + assert [obs.id for obs in accepted] == [1] + + +def test_markers_per_frame_separates_adjacent_source_nanoseconds_without_count_tags(): + first = blank_image() + second = blank_image() + first.header.stamp = Time(sec=1700000000, nanosec=123456788) + second.header.stamp = Time(sec=1700000000, nanosec=123456789) + observations = [ + _marker_obs(image, _marker(image, marker_id), obs_id=marker_id, marker_count=1) + for image, marker_id in [(first, 7), (second, 42)] + ] + # The Observation API uses seconds; source headers retain the distinct frames. + assert observations[0].ts == observations[1].ts + for observation in observations: + observation.tags.pop("marker_frame_count") + results = list(MarkersPerFrame()(iter(observations))) + assert [result.data.header.stamp for result in results] == [ + first.header.stamp, + second.header.stamp, + ] + assert [[d.id for d in result.data.detections] for result in results] == [["7"], ["42"]] diff --git a/dimos/perception/fiducial/test_marker_pose.py b/dimos/perception/fiducial/test_marker_pose.py index 78c22751805..215825c9a6e 100644 --- a/dimos/perception/fiducial/test_marker_pose.py +++ b/dimos/perception/fiducial/test_marker_pose.py @@ -15,8 +15,10 @@ import cv2 import numpy as np -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.message_utils.image import image_from_array +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.sensor_msgs.msg import CameraInfo, RegionOfInterest +from dimos.messages.std_msgs.msg import Header from dimos.perception.fiducial.marker_pose import ( camera_optical_frame_id, estimate_marker_pose, @@ -26,25 +28,44 @@ def test_camera_optical_frame_id_resolution() -> None: - ts = 1.0 - fx, fy, cx, cy = 600.0, 600.0, 320.0, 240.0 - info_named = CameraInfo.from_intrinsics(fx, fy, cx, cy, 640, 480, frame_id="cam_info_optical") - info_named.ts = ts - info_empty = CameraInfo.from_intrinsics(fx, fy, cx, cy, 640, 480) - info_empty.ts = ts - img_custom = Image( - data=np.zeros((480, 640, 3), dtype=np.uint8), - format=ImageFormat.BGR, - ts=ts, - frame_id="custom_optical", + info_named = CameraInfo( + header=Header(frame_id="cam_info_optical", stamp=Time(sec=0, nanosec=0)), + height=0, + width=0, + distortion_model="", + d=np.array([], dtype=np.float64), + k=np.zeros(9, dtype=np.float64), + r=np.zeros(9, dtype=np.float64), + p=np.zeros(12, dtype=np.float64), + binning_x=0, + binning_y=0, + roi=RegionOfInterest(x_offset=0, y_offset=0, height=0, width=0, do_rectify=False), ) - img_whitespace = Image( - data=np.zeros((480, 640, 3), dtype=np.uint8), - format=ImageFormat.BGR, - ts=ts, - frame_id=" custom_optical ", + info_empty = CameraInfo( + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + height=0, + width=0, + distortion_model="", + d=np.array([], dtype=np.float64), + k=np.zeros(9, dtype=np.float64), + r=np.zeros(9, dtype=np.float64), + p=np.zeros(12, dtype=np.float64), + binning_x=0, + binning_y=0, + roi=RegionOfInterest(x_offset=0, y_offset=0, height=0, width=0, do_rectify=False), ) - img_empty = Image(data=np.zeros((480, 640, 3), dtype=np.uint8), format=ImageFormat.BGR, ts=ts) + pixels = np.zeros((480, 640, 3), dtype=np.uint8) + img_custom = image_from_array( + pixels, + encoding="bgr8", + header=Header(frame_id="custom_optical", stamp=Time(sec=0, nanosec=0)), + ) + img_whitespace = image_from_array( + pixels, + encoding="bgr8", + header=Header(frame_id=" custom_optical ", stamp=Time(sec=0, nanosec=0)), + ) + img_empty = image_from_array(pixels, encoding="bgr8") assert camera_optical_frame_id(img_custom, info_named) == "custom_optical" assert camera_optical_frame_id(img_whitespace, info_named) == "custom_optical" diff --git a/dimos/perception/fiducial/test_marker_tf_integration.py b/dimos/perception/fiducial/test_marker_tf_integration.py index 458edd22e67..3d4f2aaef37 100644 --- a/dimos/perception/fiducial/test_marker_tf_integration.py +++ b/dimos/perception/fiducial/test_marker_tf_integration.py @@ -16,87 +16,110 @@ from __future__ import annotations -import time -import uuid +from contextlib import ExitStack +from threading import Event -from dimos_lcm.vision_msgs import BoundingBox3D, ObjectHypothesis, ObjectHypothesisWithPose +import numpy as np import pytest from dimos.core.transport import LCMTransport -from dimos.core.transport_factory import make_transport -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.std_msgs.Header import Header -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.vision_msgs.Detection3D import Detection3D -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.message_utils.time import time_from_seconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Point, Pose, PoseWithCovariance, Quaternion, Vector3 +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.messages.vision_msgs.msg import ( + BoundingBox3D, + Detection3D, + Detection3DArray, + ObjectHypothesis, + ObjectHypothesisWithPose, +) from dimos.perception.fiducial.marker_tf_module import MarkerTfModule from dimos.protocol.tf.tf import TF def _marker_detection_array(ts: float) -> Detection3DArray: - det = Detection3D() - det.header = Header(ts, "world") + det = Detection3D( + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + results=[], + bbox=BoundingBox3D( + center=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + size=Vector3(x=0.0, y=0.0, z=0.0), + ), + id="", + ) + det.header = Header(stamp=time_from_seconds(ts), frame_id="world") det.id = "11" det.results = [ ObjectHypothesisWithPose( hypothesis=ObjectHypothesis( class_id="DICT_APRILTAG_36h11:11", score=1.0, - ) + ), + pose=PoseWithCovariance( + pose=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + covariance=np.zeros(36, dtype=np.float64), + ), ) ] - det.results_length = len(det.results) det.bbox = BoundingBox3D( center=Pose( - position=Vector3(0.5, -0.2, 1.25), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + position=Point(x=0.5, y=-0.2, z=1.25), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), ), - size=Vector3(0.18, 0.18, 0.0), + size=Vector3(x=0.18, y=0.18, z=0.0), ) return Detection3DArray( - header=Header(ts, "world"), + header=Header(stamp=time_from_seconds(ts), frame_id="world"), detections=[det], - detections_length=1, ) def test_marker_tf_consumes_detection_array_over_lcm() -> None: - ts = time.time() - module = MarkerTfModule(marker_namespace_prefix="marker_tf") - detections_transport = LCMTransport( - f"/mtf/{uuid.uuid4().hex[:8]}", - Detection3DArray, - ) - module.detections.transport = detections_transport - module.tf.transport = make_transport("/tf", TFMessage) - host_transport = make_transport("/tf", TFMessage) - host_tf = TF(host_transport) + stamp = Time(sec=1700000000, nanosec=123456789) + ts = stamp.sec + stamp.nanosec / 1e9 + with ExitStack() as cleanup: + detections_transport = LCMTransport("/mtf/detections", Detection3DArray, url="memq://") + cleanup.callback(detections_transport.stop) + tf_transport = LCMTransport("/mtf/tf", TFMessage, url="memq://") + cleanup.callback(tf_transport.stop) + module = MarkerTfModule(marker_namespace_prefix="marker_tf") + cleanup.callback(module.stop) + module.detections.transport = detections_transport + module.tf.transport = tf_transport + host_tf = TF(tf_transport) + cleanup.callback(host_tf.dispose) + received = [] + ready = Event() - try: + def receive(value): + received.append(value) + ready.set() + + cleanup.callback(tf_transport.subscribe(receive)) module.start() msg = _marker_detection_array(ts) - w_markers = None - w_marker = None - for _ in range(40): - detections_transport.publish(msg) - time.sleep(0.05) - w_markers = host_tf.get("world", "marker_tf/markers", ts, 1.0) - w_marker = host_tf.get("world", "marker_tf/marker_11", ts, 1.0) - if w_markers is not None and w_marker is not None: - break - - assert w_markers is not None, "Timed out waiting for world -> marker_tf/markers" - assert w_marker is not None, "Timed out waiting for world -> marker_tf/marker_11" - assert w_markers.frame_id == "world" + msg.header.stamp = stamp + msg.detections[0].header.stamp = stamp + detections_transport.publish(msg) + assert ready.wait(2), "Timed out waiting for generated CDR marker transforms" + assert len(received) == 1 + assert [tf.header.stamp for tf in received[0].transforms] == [stamp, stamp] + w_markers = host_tf.get("world", "marker_tf/markers", ts, 1.0) + w_marker = host_tf.get("world", "marker_tf/marker_11", ts, 1.0) + assert w_markers is not None + assert w_marker is not None + assert w_markers.header.frame_id == "world" assert w_markers.child_frame_id == "marker_tf/markers" - assert w_marker.frame_id == "world" + assert w_marker.header.frame_id == "world" assert w_marker.child_frame_id == "marker_tf/marker_11" - assert w_marker.translation.x == pytest.approx(0.5) - assert w_marker.translation.y == pytest.approx(-0.2) - assert w_marker.translation.z == pytest.approx(1.25) - finally: - host_tf.dispose() - host_transport.stop() - module.stop() + assert w_marker.transform.translation.x == pytest.approx(0.5) + assert w_marker.transform.translation.y == pytest.approx(-0.2) + assert w_marker.transform.translation.z == pytest.approx(1.25) diff --git a/dimos/perception/fiducial/test_marker_tf_module.py b/dimos/perception/fiducial/test_marker_tf_module.py index 90d2c10ff36..f2178dc7bfc 100644 --- a/dimos/perception/fiducial/test_marker_tf_module.py +++ b/dimos/perception/fiducial/test_marker_tf_module.py @@ -14,15 +14,23 @@ from unittest.mock import patch -from dimos_lcm.vision_msgs import BoundingBox3D, ObjectHypothesis, ObjectHypothesisWithPose +from dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode +import numpy as np import pytest -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.std_msgs.Header import Header -from dimos.msgs.vision_msgs.Detection3D import Detection3D -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.message_utils.geometry import quaternion_from_euler +from dimos.message_utils.time import time_from_seconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Point, Pose, PoseWithCovariance, Quaternion, Vector3 +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.messages.vision_msgs.msg import ( + BoundingBox3D, + Detection3D, + Detection3DArray, + ObjectHypothesis, + ObjectHypothesisWithPose, +) from dimos.perception.fiducial.marker_tf_module import MarkerTfModule from dimos.protocol.tf.tf import TF @@ -32,44 +40,60 @@ def _detection_array( ts: float, marker_id: str = "0", class_id: str = "DICT_APRILTAG_36h11:0", - center: Vector3 | None = None, + center: Point | None = None, orientation: Quaternion | None = None, ) -> Detection3DArray: if center is None: - center = Vector3(1.2, -0.3, 0.8) + center = Point(x=1.2, y=-0.3, z=0.8) if orientation is None: - orientation = Quaternion(0.0, 0.0, 0.0, 1.0) - - det = Detection3D() - det.header = Header(ts, "world") + orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + + det = Detection3D( + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + results=[], + bbox=BoundingBox3D( + center=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + size=Vector3(x=0.0, y=0.0, z=0.0), + ), + id="", + ) + det.header = Header(stamp=time_from_seconds(ts), frame_id="world") det.id = marker_id det.results = [ ObjectHypothesisWithPose( hypothesis=ObjectHypothesis( class_id=class_id, score=1.0, - ) + ), + pose=PoseWithCovariance( + pose=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + covariance=np.zeros(36, dtype=np.float64), + ), ) ] - det.results_length = len(det.results) det.bbox = BoundingBox3D( center=Pose( position=center, orientation=orientation, ), - size=Vector3(0.18, 0.18, 0.0), + size=Vector3(x=0.18, y=0.18, z=0.0), ) return Detection3DArray( - header=Header(ts, "world"), + header=Header(stamp=time_from_seconds(ts), frame_id="world"), detections=[det], - detections_length=1, ) def test_marker_tf_module_publishes_world_markers_chain() -> None: ts = 1_000_000.0 - center = Vector3(1.3, -0.2, 0.4) - orientation = Quaternion(0.1, 0.2, 0.3, 0.9) + center = Point(x=1.3, y=-0.2, z=0.4) + orientation = quaternion_from_euler(0.1, 0.2, 0.3) mod = MarkerTfModule() view = TF(mod.tf) @@ -86,21 +110,22 @@ def test_marker_tf_module_publishes_world_markers_chain() -> None: wm = view.get("world", "markers", ts, 1.0) assert wm is not None - assert abs(wm.translation.x) < 1e-6 - assert abs(wm.translation.y) < 1e-6 - assert abs(wm.translation.z) < 1e-6 + assert abs(wm.transform.translation.x) < 1e-6 + assert abs(wm.transform.translation.y) < 1e-6 + assert abs(wm.transform.translation.z) < 1e-6 w_m7 = view.get("world", "marker_7", ts, 1.0) assert w_m7 is not None - assert w_m7.translation.x == pytest.approx(center.x) - assert w_m7.translation.y == pytest.approx(center.y) - assert w_m7.translation.z == pytest.approx(center.z) - assert w_m7.rotation.x == pytest.approx(orientation.x) - assert w_m7.rotation.y == pytest.approx(orientation.y) - assert w_m7.rotation.z == pytest.approx(orientation.z) - assert w_m7.rotation.w == pytest.approx(orientation.w) + assert w_m7.transform.translation.x == pytest.approx(center.x) + assert w_m7.transform.translation.y == pytest.approx(center.y) + assert w_m7.transform.translation.z == pytest.approx(center.z) + assert w_m7.transform.rotation.x == pytest.approx(orientation.x) + assert w_m7.transform.rotation.y == pytest.approx(orientation.y) + assert w_m7.transform.rotation.z == pytest.approx(orientation.z) + assert w_m7.transform.rotation.w == pytest.approx(orientation.w) assert view.get("world", "marker_99", ts, 0.1) is None finally: + view.dispose() mod.stop() @@ -116,6 +141,7 @@ def test_marker_tf_parses_class_id_when_detection_id_empty() -> None: assert view.get("world", "marker_42", ts, 1.0) is not None finally: + view.dispose() mod.stop() @@ -126,14 +152,14 @@ def test_marker_tf_empty_array_skips_publication() -> None: try: mod._process_detections( Detection3DArray( - header=Header(ts, "world"), + header=Header(stamp=time_from_seconds(ts), frame_id="world"), detections=[], - detections_length=0, ) ) assert view.get("world", "markers", ts, 0.1) is None finally: + view.dispose() mod.stop() @@ -146,6 +172,7 @@ def test_marker_tf_non_empty_array_without_marker_id_skips_publication() -> None assert view.get("world", "markers", ts, 0.1) is None finally: + view.dispose() mod.stop() @@ -160,6 +187,7 @@ def test_marker_tf_does_not_recompute_marker_pose() -> None: mock_estimate.assert_not_called() assert view.get("world", "marker_4", ts, 1.0) is not None finally: + view.dispose() mod.stop() @@ -174,4 +202,24 @@ def test_marker_namespace_prefix_child_frames() -> None: assert view.get("world", "r1/markers", ts, 1.0) is not None assert view.get("world", "r1/marker_0", ts, 1.0) is not None finally: + view.dispose() mod.stop() + + +def test_marker_tf_wire_preserves_exact_timestamp(): + module = MarkerTfModule() + outputs = [] + unsubscribe = module.tf.subscribe( + lambda msg: outputs.append(cdr_decode(cdr_encode(msg), TFMessage)) + ) + detections = _detection_array(ts=0) + detections.header.stamp.sec = 1700000000 + detections.header.stamp.nanosec = 123456789 + try: + module._process_detections(detections) + assert len(outputs) == 1 + assert len(outputs[0].transforms) == 2 + assert all(t.header.stamp == detections.header.stamp for t in outputs[0].transforms) + finally: + unsubscribe() + module.stop() diff --git a/dimos/perception/fiducial/test_marker_transformer.py b/dimos/perception/fiducial/test_marker_transformer.py index 16830e35561..cab69252933 100644 --- a/dimos/perception/fiducial/test_marker_transformer.py +++ b/dimos/perception/fiducial/test_marker_transformer.py @@ -16,8 +16,10 @@ import pytest from dimos.memory.type.observation import Observation -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.message_utils.image import image_from_array +from dimos.message_utils.time import time_from_seconds, to_seconds +from dimos.messages.sensor_msgs.msg import CameraInfo, Image +from dimos.messages.std_msgs.msg import Header from dimos.perception.fiducial.marker_detect import detect_markers_in_image from dimos.perception.fiducial.marker_transformer import DetectMarkers from dimos.perception.fiducial.test_helpers import ( @@ -31,12 +33,12 @@ def test_detect_markers_in_image_builds_rich_marker_detection() -> None: marker_id = 7 marker_length_m = 0.18 image = synthetic_marker_image(marker_id) - info = camera_info(image.ts) + info = camera_info(to_seconds(image.header.stamp)) detections = detect_markers_in_image( image, camera_info=info, - world_T_optical=world_T_optical(image.ts), + world_T_optical=world_T_optical(to_seconds(image.header.stamp)), marker_length_m=marker_length_m, aruco_dictionary="DICT_APRILTAG_36h11", ) @@ -65,11 +67,10 @@ def test_detect_markers_in_image_builds_rich_marker_detection() -> None: def test_detect_markers_in_image_returns_empty_for_no_marker_frame() -> None: ts = 11.0 - image = Image( - data=np.full((480, 640, 3), 255, dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera_optical", - ts=ts, + image = image_from_array( + np.full((480, 640, 3), 255, dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera_optical", stamp=time_from_seconds(ts)), ) detections = detect_markers_in_image( @@ -86,8 +87,8 @@ def test_detect_markers_in_image_returns_empty_for_no_marker_frame() -> None: def test_detect_markers_in_image_needs_detect_inverted_for_negative_tags() -> None: image = synthetic_marker_image(7, inverted=True) kwargs = { - "camera_info": camera_info(image.ts), - "world_T_optical": world_T_optical(image.ts), + "camera_info": camera_info(to_seconds(image.header.stamp)), + "world_T_optical": world_T_optical(to_seconds(image.header.stamp)), "marker_length_m": 0.18, "aruco_dictionary": "DICT_APRILTAG_36h11", } @@ -104,13 +105,13 @@ def test_detect_markers_transformer_preserves_observation_context_and_tags() -> image = synthetic_marker_image(marker_id, ts=12.0) obs = Observation[Image]( id=42, - ts=image.ts, + ts=to_seconds(image.header.stamp), data_type=Image, pose=(1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0), _data=image, ) transformer = DetectMarkers( - camera_info=camera_info(image.ts), + camera_info=camera_info(to_seconds(image.header.stamp)), marker_length_m=0.18, aruco_dictionary="DICT_APRILTAG_36h11", ) @@ -130,21 +131,20 @@ def test_detect_markers_transformer_preserves_observation_context_and_tags() -> def test_detect_markers_transformer_can_emit_empty_frame_sentinel() -> None: - image = Image( - data=np.full((480, 640, 3), 255, dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera_optical", - ts=13.0, + image = image_from_array( + np.full((480, 640, 3), 255, dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera_optical", stamp=time_from_seconds(13.0)), ) obs = Observation[Image]( id=43, - ts=image.ts, + ts=to_seconds(image.header.stamp), data_type=Image, pose=(1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0), _data=image, ) transformer = DetectMarkers( - camera_info=camera_info(image.ts), + camera_info=camera_info(to_seconds(image.header.stamp)), marker_length_m=0.18, aruco_dictionary="DICT_APRILTAG_36h11", emit_empty_frames=True, @@ -165,7 +165,7 @@ def test_detect_markers_transformer_uses_callablecamera_info_source() -> None: image = synthetic_marker_image(marker_id=7, ts=14.0) obs = Observation[Image]( id=44, - ts=image.ts, + ts=to_seconds(image.header.stamp), data_type=Image, pose=(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0), _data=image, @@ -180,7 +180,7 @@ def test_detect_markers_transformer_uses_callablecamera_info_source() -> None: assert list(transformer(iter([obs]))) == [] - latest_info = camera_info(image.ts) + latest_info = camera_info(to_seconds(image.header.stamp)) results = list(transformer(iter([obs]))) assert len(results) == 1 @@ -193,24 +193,23 @@ def test_detect_markers_rebuilds_intrinsics_without_resetting_smoothing_track() marker_length_m = 0.18 image_a = synthetic_marker_image(marker_id=marker_id, ts=15.0) image_b = synthetic_marker_image(marker_id=marker_id, ts=15.2) - info_a = camera_info(image_a.ts) - info_b = camera_info(image_b.ts) - info_b.K = info_b.K.copy() - info_b.P = info_b.P.copy() - info_b.K[0] = info_b.K[4] = 900.0 - info_b.P[0] = info_b.P[5] = 900.0 + info_a = camera_info(to_seconds(image_a.header.stamp)) + info_b = camera_info(to_seconds(image_b.header.stamp)) + + info_b.k[0] = info_b.k[4] = 900.0 + info_b.p[0] = info_b.p[5] = 900.0 latest_info: CameraInfo | None = info_a obs_a = Observation[Image]( id=45, - ts=image_a.ts, + ts=to_seconds(image_a.header.stamp), data_type=Image, pose=(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0), _data=image_a, ) obs_b = Observation[Image]( id=46, - ts=image_b.ts, + ts=to_seconds(image_b.header.stamp), data_type=Image, pose=(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0), _data=image_b, diff --git a/dimos/perception/localize/association.py b/dimos/perception/localize/association.py index 774191ef2d2..935bcb81e66 100644 --- a/dimos/perception/localize/association.py +++ b/dimos/perception/localize/association.py @@ -22,7 +22,11 @@ import numpy as np -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.message_utils.geometry import inverse_transform +from dimos.message_utils.pointcloud import pointcloud_from_xyz, pointcloud_xyz +from dimos.message_utils.time import time_from_nanoseconds, to_nanoseconds +from dimos.messages.sensor_msgs.msg import PointCloud2 +from dimos.messages.std_msgs.msg import Header if TYPE_CHECKING: from dimos.perception.detection.type.detection3d.pointcloud import Detection3DPC @@ -56,9 +60,9 @@ class Groups: def add(self, det: Detection3DPC, radius: float, voxel: float) -> int: """Assign one sighting to its object, folding its cloud in.""" - points = np.asarray(det.pointcloud.pointcloud.points) + points = pointcloud_xyz(det.pointcloud) centroid = points.mean(axis=0) - camera = (-det.transform).translation + camera = inverse_transform(det.transform).transform.translation hit = next( (i for i, c in enumerate(self.centers) if np.linalg.norm(c - centroid) <= radius), -1 @@ -73,7 +77,7 @@ def add(self, det: Detection3DPC, radius: float, voxel: float) -> int: self.trace.append([]) self.ids.append(uuid4().hex[:8]) else: - self._fuse(hit, det.pointcloud.ts, points, centroid, voxel) + self._fuse(hit, to_nanoseconds(det.pointcloud.header.stamp), points, centroid, voxel) self.members[hit].extend( (det.ts, float(det.confidence), *centroid, camera.x, camera.y, camera.z) @@ -81,7 +85,7 @@ def add(self, det: Detection3DPC, radius: float, voxel: float) -> int: return hit def _fuse( - self, i: int, ts: float, points: np.ndarray, centroid: np.ndarray, voxel: float + self, i: int, ts_ns: int, points: np.ndarray, centroid: np.ndarray, voxel: float ) -> None: """Voxel-average a sighting into the group's union. @@ -89,13 +93,13 @@ def _fuse( weighted mean over every viewpoint rather than a mean of means, and the running center follows the same counts. """ - held = np.asarray(self.clouds[i].pointcloud.points) + held = pointcloud_xyz(self.clouds[i]) na, nb = self.raw[i], float(len(points)) self.centers[i] = (na * self.centers[i] + nb * centroid) / (na + nb) self.raw[i] = na + nb merged = np.vstack([held, points]) - ts = max(self.clouds[i].ts, ts) - frame = self.clouds[i].frame_id + ts_ns = max(to_nanoseconds(self.clouds[i].header.stamp), ts_ns) + frame = self.clouds[i].header.frame_id if voxel > 0: w = np.ones(len(merged)) @@ -111,7 +115,9 @@ def _fuse( np.add.at(psum, inverse, merged * w[:, None]) merged = psum / wsum[:, None] self.weights[i] = wsum - self.clouds[i] = PointCloud2.from_numpy(merged, frame_id=frame, timestamp=ts) + self.clouds[i] = pointcloud_from_xyz( + merged, header=Header(frame_id=frame, stamp=time_from_nanoseconds(ts_ns)) + ) def rows(self, i: int) -> np.ndarray: return np.asarray(self.members[i]).reshape(-1, M_WIDTH) diff --git a/dimos/perception/localize/dandetect.py b/dimos/perception/localize/dandetect.py index 284d6b9c380..c07cfce856f 100644 --- a/dimos/perception/localize/dandetect.py +++ b/dimos/perception/localize/dandetect.py @@ -40,8 +40,8 @@ from reactivex.abc import DisposableBase from dimos.memory.stream import Stream + from dimos.messages.sensor_msgs.msg import Image from dimos.models.embedding.base import EmbeddingModel - from dimos.msgs.sensor_msgs.Image import Image from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.localize.association import Groups from dimos.perception.localize.localize import LocalizeTrace diff --git a/dimos/perception/localize/demo_blueprints/xarm_feed.py b/dimos/perception/localize/demo_blueprints/xarm_feed.py index ef05babb4e3..d5a23671738 100644 --- a/dimos/perception/localize/demo_blueprints/xarm_feed.py +++ b/dimos/perception/localize/demo_blueprints/xarm_feed.py @@ -41,9 +41,8 @@ from dimos.core.stream import Out from dimos.memory.replay import ReplayStream from dimos.memory.store.sqlite import SqliteStore -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.messages.sensor_msgs.msg import CameraInfo, Image +from dimos.messages.tf2_msgs.msg import TFMessage from dimos.utils.data import get_data from dimos.utils.logging_config import setup_logger diff --git a/dimos/perception/localize/localize.py b/dimos/perception/localize/localize.py index 8a5a0900fa5..c8d8a028939 100644 --- a/dimos/perception/localize/localize.py +++ b/dimos/perception/localize/localize.py @@ -32,6 +32,11 @@ from dimos.memory.embed import EmbedImages from dimos.memory.transform import QualityWindow, peaks +from dimos.memory.type.observation import Observation +from dimos.message_utils.geometry import inverse_transform, vector_array +from dimos.message_utils.image import image_brightness, image_sharpness +from dimos.message_utils.pointcloud import pointcloud_xyz +from dimos.message_utils.time import to_seconds from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.localize.association import ( @@ -121,15 +126,15 @@ def _lift( detections: ImageDetections2D[Any], rig: Rig, params: LocalizeParams, plane: SupportPlane | None ) -> list[Detection3DPC]: """Lift a frame's detections through its camera's geometry and gate them.""" - transform = rig.world_to_optical(detections.ts, detections.image.frame_id) + transform = rig.world_to_optical(detections.ts, detections.image.header.frame_id) lifted = rig.lift(detections, transform) if transform is not None else None if transform is None or lifted is None: return [] - camera = (-transform).translation.to_numpy() + camera = vector_array(inverse_transform(transform).transform.translation) valid: list[Detection3DPC] = [] for det3d in lifted: - points = np.asarray(det3d.pointcloud.pointcloud.points) + points = pointcloud_xyz(det3d.pointcloud) if len(points) < params.min_points: continue if float((points.max(axis=0) - points.min(axis=0)).max()) > params.max_object_extent_m: @@ -150,16 +155,19 @@ def _lift( def index_frames(rig: Rig, feed: Stream[Any, Any]) -> Stream[Any, Any]: """One camera's frames worth embedding: lit, sharpest per slot, world-posed, camera-tagged.""" - return ( - feed.filter(lambda obs: obs.data.brightness > 0.1) - .transform(QualityWindow(lambda img: img.sharpness, window=1.0 / rig.embed_hz)) - .map( - lambda obs: obs.derive( - data=obs.data, - pose=rig.camera_pose(obs.ts, obs.data.frame_id), - tags={**obs.tags, "camera": obs.data.frame_id}, - ) + + def with_camera_pose(obs: Observation[Any]) -> Observation[Any]: + pose = rig.camera_pose(obs.ts, obs.data.header.frame_id) + return obs.derive( + data=obs.data, + pose=pose.pose if pose is not None else None, + tags={**obs.tags, "camera": obs.data.header.frame_id}, ) + + return ( + feed.filter(lambda obs: image_brightness(obs.data) > 0.1) + .transform(QualityWindow(image_sharpness, window=1.0 / rig.embed_hz)) + .map(with_camera_pose) .filter(lambda obs: obs.pose is not None) ) @@ -183,7 +191,7 @@ def embed_index(rig: Rig, embedder: EmbeddingModel, t0: float, t1: float) -> Str feed.transform(EmbedImages(embedder)) .map( lambda obs: obs.derive( - data=IndexFrame(obs.ts, obs.data.frame_id, float(obs.data.sharpness)) + data=IndexFrame(obs.ts, obs.data.header.frame_id, image_sharpness(obs.data)) ) ) .materialize() @@ -330,7 +338,7 @@ def _frames(upstream: Iterator[Any]) -> Iterator[Any]: class_id=j, confidence=float(score), name=queries[j], - ts=img.ts, + ts=to_seconds(img.header.stamp), image=img, ) if det.is_valid(): diff --git a/dimos/perception/localize/module.py b/dimos/perception/localize/module.py index 07ec1e1be3d..5903d67ca46 100644 --- a/dimos/perception/localize/module.py +++ b/dimos/perception/localize/module.py @@ -41,8 +41,7 @@ import time from typing import Any, Literal -from dimos_lcm.geometry_msgs import Pose -from dimos_lcm.vision_msgs import BoundingBox3D, ObjectHypothesis, ObjectHypothesisWithPose +import numpy as np from dimos.agents.annotation import skill from dimos.core.core import rpc @@ -51,15 +50,19 @@ from dimos.memory.blobstore.memory import MemoryBlobStore from dimos.memory.observationstore.memory import ListObservationStore from dimos.memory.store.memory import MemoryStore -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.std_msgs.Header import Header -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.vision_msgs.Detection3D import Detection3D -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.message_utils.pointcloud import pointcloud_xyz +from dimos.message_utils.time import time_from_seconds, to_seconds +from dimos.messages.geometry_msgs.msg import Point, Pose, PoseWithCovariance, Quaternion, Vector3 +from dimos.messages.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.messages.vision_msgs.msg import ( + BoundingBox3D, + Detection3D, + Detection3DArray, + ObjectHypothesis, + ObjectHypothesisWithPose, +) from dimos.perception.localize.association import Groups from dimos.perception.localize.dandetect import DanDetector from dimos.perception.localize.rig import ( @@ -77,8 +80,8 @@ INDEX_STREAM = "color_image_embedded" GEOMETRY_STREAM = "geometry_memory" -COLOR_CODEC = "jpeg" -GEOMETRY_CODEC = "lz4+lcm" +COLOR_CODEC = "jpegcdr" +GEOMETRY_CODEC = "lz4+cdr" LIDAR_TOLERANCE = 0.2 # s - an embedded frame to its lidar scan @@ -164,11 +167,13 @@ def on_camera_info(info: CameraInfo) -> None: self._camera_seen.set() def on_geometry(msg: Image | PointCloud2) -> None: - self._geometry_feed.append(msg, ts=msg.ts) + self._geometry_feed.append(msg, ts=to_seconds(msg.header.stamp)) geometry = self.depth_image if self.config.geometry == "depth" else self.lidar self._unsubs = [ - self.color_image.subscribe(lambda img: self._color_feed.append(img, ts=img.ts)), + self.color_image.subscribe( + lambda img: self._color_feed.append(img, ts=to_seconds(img.header.stamp)) + ), geometry.subscribe(on_geometry), self.camera_info.subscribe(on_camera_info), ] @@ -213,7 +218,7 @@ def _warm(self) -> None: assert camera is not None depth = self._geometry_memory if self.config.geometry == "depth" else None self.rig = Rig( - cameras={camera.frame_id: RigCamera(info=camera, color=self.index, depth=depth)}, + cameras={camera.header.frame_id: RigCamera(info=camera, color=self.index, depth=depth)}, tf=self._tf_buffer, world_frame=self.config.world_frame, lidar=None if depth is not None else self._geometry_memory, @@ -305,30 +310,43 @@ def as_detection_array( latest = 0.0 for query, hits in zip(queries, results, strict=True): for hit in hits: - points = hit.point_cloud.as_numpy()[0] + points = pointcloud_xyz(hit.point_cloud) low, high = points.min(axis=0), points.max(axis=0) middle, extent = (low + high) / 2, high - low - center = Vector3(*(float(v) for v in middle)) - size = Vector3(*(max(float(v), 1e-3) for v in extent)) + center = Point(x=float(middle[0]), y=float(middle[1]), z=float(middle[2])) + size = Vector3( + x=max(float(extent[0]), 1e-3), + y=max(float(extent[1]), 1e-3), + z=max(float(extent[2]), 1e-3), + ) latest = max(latest, hit.last_seen_timestamp) boxes.append( Detection3D( - header=Header(hit.last_seen_timestamp, frame_id), + header=Header( + stamp=time_from_seconds(hit.last_seen_timestamp), frame_id=frame_id + ), id=hit.instance_id, results=[ ObjectHypothesisWithPose( - hypothesis=ObjectHypothesis(class_id=query, score=hit.semantic_score) + hypothesis=ObjectHypothesis(class_id=query, score=hit.semantic_score), + pose=PoseWithCovariance( + pose=Pose( + position=center, + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + covariance=np.zeros(36, dtype=np.float64), + ), ) ], - results_length=1, bbox=BoundingBox3D( - center=Pose(position=center, orientation=Quaternion(0.0, 0.0, 0.0, 1.0)), + center=Pose( + position=center, orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ), size=size, ), ) ) return Detection3DArray( - detections_length=len(boxes), - header=Header(latest, frame_id), + header=Header(stamp=time_from_seconds(latest), frame_id=frame_id), detections=boxes, ) diff --git a/dimos/perception/localize/rig.py b/dimos/perception/localize/rig.py index 729b6e694c2..2074e8b438a 100644 --- a/dimos/perception/localize/rig.py +++ b/dimos/perception/localize/rig.py @@ -23,6 +23,18 @@ import numpy as np from dimos.memory.tf import StreamTF +from dimos.message_utils.geometry import inverse_transform, pose_from_transform, vector_array +from dimos.message_utils.image import image_view +from dimos.message_utils.pointcloud import ( + pointcloud_from_rgbd, + pointcloud_from_xyz, + pointcloud_xyz, + transform_cloud, +) +from dimos.message_utils.time import time_from_nanoseconds, to_nanoseconds +from dimos.messages.geometry_msgs.msg import PoseStamped, TransformStamped +from dimos.messages.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos.messages.std_msgs.msg import Header from dimos.perception.detection.type.detection3d.imageDetections3DPC import ImageDetections3DPC from dimos.perception.detection.type.detection3d.pointcloud_filters import ( range_cluster, @@ -33,13 +45,7 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: - from dimos_lcm.sensor_msgs import CameraInfo - from dimos.memory.type.observation import Observation - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - from dimos.msgs.geometry_msgs.Transform import Transform - from dimos.msgs.sensor_msgs.Image import Image - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.localize.support_plane import SupportPlane from dimos.protocol.tf.tf import TFLookup @@ -66,7 +72,7 @@ def _tf_root(store: Any, camera: str) -> str: t0, t1 = stream.get_time_range() middle = (t0 + t1) / 2 parent = { - t.child_frame_id: t.frame_id + t.child_frame_id: t.header.frame_id for obs in stream.after(middle - TF_ROOT_WINDOW).before(middle + TF_ROOT_WINDOW) for t in obs.data.transforms } @@ -93,7 +99,13 @@ def _camera_span(cameras: dict[str, RigCamera], tf: StreamTF, world: str) -> flo tf.get(frame, world, t0 + (t1 - t0) * k / 11, TF_TOLERANCE, warn=False) for k in range(12) ] - positions = np.array([(-t).translation.to_numpy() for t in seen if t is not None]) + positions = np.array( + [ + vector_array(inverse_transform(t).transform.translation) + for t in seen + if t is not None + ] + ) if len(positions) > 1: span = max(span, float(np.linalg.norm(np.ptp(positions, axis=0)))) return span @@ -131,37 +143,38 @@ def from_store(cls, store: Any) -> Rig: """Every calibrated optical frame that records colour, with aligned depth or beside the store's one point cloud stream, is a camera, posed in the root of its tf chain.""" - from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo as CameraInfoMsg - from dimos.msgs.sensor_msgs.Image import Image as ImageMsg - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 as PointCloudMsg - names = store.list_streams() types = {name: store.stream(name).data_type for name in names} infos: dict[str, CameraInfo] = {} - for name in sorted(n for n in names if types[n] is CameraInfoMsg): + for name in sorted(n for n in names if types[n] is CameraInfo): if store.stream(name).count(): info = store.stream(name).first().data - infos.setdefault(info.frame_id, info) # sorted: a colour info beats its depth twin + infos.setdefault( + info.header.frame_id, info + ) # sorted: a colour info beats its depth twin images: dict[str, dict[str, str]] = {"color": {}, "depth": {}} for name in names: - if types[name] is not ImageMsg or not store.stream(name).count(): + if types[name] is not Image or not store.stream(name).count(): continue image = store.stream(name).first().data - if image.data.dtype == np.uint16 or image.data.dtype.kind == "f": + pixels = image_view(image) + if ( + pixels.dtype.kind == "u" and pixels.dtype.itemsize == 2 + ) or pixels.dtype.kind == "f": kind = "depth" - elif image.data.ndim == 3: + elif pixels.ndim == 3: kind = "color" else: continue # a derived copy, such as an embedding index, holds at most its feed's frames - held = images[kind].get(image.frame_id) + held = images[kind].get(image.header.frame_id) if held is None or store.stream(name).count() > store.stream(held).count(): - images[kind][image.frame_id] = name + images[kind][image.header.frame_id] = name color, depth = images["color"], images["depth"] - clouds = [n for n in names if types[n] is PointCloudMsg and store.stream(n).count()] + clouds = [n for n in names if types[n] is PointCloud2 and store.stream(n).count()] if len(clouds) > 1 and any(f in color and f not in depth for f in infos): raise ValueError(f"several point cloud streams for a camera without depth: {clouds}") lidar = clouds[0] if clouds else None @@ -207,13 +220,13 @@ def time_range(self) -> tuple[float, float]: ranges = [camera.color.get_time_range() for camera in self.cameras.values()] return min(lo for lo, _ in ranges), max(hi for _, hi in ranges) - def world_to_optical(self, ts: float, frame: str) -> Transform | None: + def world_to_optical(self, ts: float, frame: str) -> TransformStamped | None: return self.tf.get(frame, self.world_frame, ts, TF_TOLERANCE) def camera_pose(self, ts: float, frame: str) -> PoseStamped | None: """World pose of a camera's optical frame at ts.""" transform = self.world_to_optical(ts, frame) - return (-transform).to_pose() if transform is not None else None + return pose_from_transform(inverse_transform(transform)) if transform is not None else None def frame_at(self, obs: Observation[Any]) -> Image: """The colour frame an index observation refers to. @@ -222,7 +235,6 @@ def frame_at(self, obs: Observation[Any]) -> Image: frame selection needs, so its pixels come back from its camera's colour stream. """ - from dimos.msgs.sensor_msgs.Image import Image if isinstance(obs.data, Image): return obs.data @@ -246,38 +258,41 @@ def lidar_map(self) -> PointCloud2 | None: Bound ``lidar`` to the window first; whatever moved during it is in the map wherever it was. """ - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 - if self._lidar_map is not None and self._lidar_map[0] == id(self.lidar): return self._lidar_map[1] parts: list[np.ndarray] = [] - newest = 0.0 + newest_ns: int | None = None for obs in self.lidar: cloud: PointCloud2 = obs.data - if cloud.frame_id != self.world_frame: - transform = self.tf.get(self.world_frame, cloud.frame_id, obs.ts, TF_TOLERANCE) + if cloud.header.frame_id != self.world_frame: + transform = self.tf.get( + self.world_frame, cloud.header.frame_id, obs.ts, TF_TOLERANCE + ) if transform is None: continue - cloud = cloud.transform(transform) - parts.append(cloud.points_f32()) - newest = max(newest, obs.ts) + cloud = transform_cloud(cloud, transform) + parts.append(pointcloud_xyz(cloud)) + source_ns = to_nanoseconds(cloud.header.stamp) + newest_ns = source_ns if newest_ns is None else max(newest_ns, source_ns) merged = None if parts: + assert newest_ns is not None points = np.vstack(parts) cells = np.floor(points / LIDAR_CELL).astype(np.int64) + (1 << 20) keys = (cells[:, 0] << 42) | (cells[:, 1] << 21) | cells[:, 2] _, first = np.unique(keys, return_index=True) - merged = PointCloud2.from_numpy( - points[first], frame_id=self.world_frame, timestamp=newest + merged = pointcloud_from_xyz( + points[first], + header=Header(frame_id=self.world_frame, stamp=time_from_nanoseconds(newest_ns)), ) self._lidar_map = (id(self.lidar), merged) return merged def lift( - self, detections: ImageDetections2D[Any], transform: Transform + self, detections: ImageDetections2D[Any], transform: TransformStamped ) -> ImageDetections3DPC | None: """A frame's detections as world clouds, through its camera's depth or the lidar.""" - frame = detections.image.frame_id + frame = detections.image.header.frame_id camera = self.cameras[frame] if camera.depth is not None: depth = self.depth_at(detections.ts, frame) @@ -292,8 +307,6 @@ def lift( def backdrop(self, ts: float, frame: str, depth_trunc: float = 1.5) -> PointCloud2 | None: """A camera's own RGBD frame at ts as a world-frame cloud, for plane fits and rendering; the lidar map for a camera without depth.""" - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 - if self.cameras[frame].depth is None: return self.lidar_map() depth = self.depth_at(ts, frame) @@ -306,6 +319,9 @@ def backdrop(self, ts: float, frame: str, depth_trunc: float = 1.5) -> PointClou transform = self.world_to_optical(ts, frame) if transform is None: return None - return PointCloud2.from_rgbd( - color, depth, self.cameras[frame].info, depth_scale=0.001, depth_trunc=depth_trunc - ).transform(-transform) + return transform_cloud( + pointcloud_from_rgbd( + color, depth, self.cameras[frame].info, depth_scale=0.001, depth_trunc=depth_trunc + ), + inverse_transform(transform), + ) diff --git a/dimos/perception/localize/support_plane.py b/dimos/perception/localize/support_plane.py index bf28c061393..a747768001d 100644 --- a/dimos/perception/localize/support_plane.py +++ b/dimos/perception/localize/support_plane.py @@ -21,11 +21,12 @@ import numpy as np +from dimos.message_utils.pointcloud import pointcloud_xyz, voxel_downsample_cloud from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: from dimos.memory.type.observation import Observation - from dimos.msgs.sensor_msgs.Image import Image + from dimos.messages.sensor_msgs.msg import Image from dimos.perception.localize.rig import Rig logger = setup_logger() @@ -59,14 +60,11 @@ def fit_support_plane( for obs in keyframes[:: max(1, len(keyframes) // 5)][:5]: cloud = rig.backdrop(obs.ts, frame) if cloud is not None: - clouds.append(cloud.voxel_downsample(0.01)) + clouds.append(pointcloud_xyz(voxel_downsample_cloud(cloud, 0.01))) if not clouds: return None - merged = clouds[0] - for cloud in clouds[1:]: - merged = merged + cloud - points = np.asarray(merged.pointcloud.points) + points = np.concatenate(clouds) if len(points) < 500: return None diff --git a/dimos/perception/localize/test_dandetect.py b/dimos/perception/localize/test_dandetect.py index a7966fd2b81..d1df5b05b10 100644 --- a/dimos/perception/localize/test_dandetect.py +++ b/dimos/perception/localize/test_dandetect.py @@ -22,11 +22,13 @@ import pytest from dimos.memory.store.memory import MemoryStore +from dimos.message_utils.camera_info import camera_info_from_intrinsics +from dimos.message_utils.image import image_from_array +from dimos.message_utils.time import time_from_seconds, to_seconds +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.std_msgs.msg import Header from dimos.models.embedding.base import Embedding -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.msgs.sensor_msgs.Image import Image, ImageFormat from dimos.perception.localize import dandetect from dimos.perception.localize.dandetect import DanDetector, NoSegmenter, Option from dimos.perception.localize.localize import embed_index @@ -57,7 +59,7 @@ def __init__(self, scores: tuple[float, float]) -> None: def query_score_rows( self, image: Image, queries: list[str], threshold: float ) -> tuple[np.ndarray, np.ndarray]: - score = self.scores[image.ts] + score = self.scores[to_seconds(image.header.stamp)] if score < threshold: return np.empty((0, 4)), np.empty((0, len(queries))) boxes = np.array([[10.0, 10.0, 30.0, 30.0]], dtype=np.float32) @@ -74,28 +76,36 @@ def two_view_rig() -> Generator[tuple[Rig, Any], None, None]: color = store.stream("color_image", Image) depth = store.stream("depth_image", Image) tf = MultiTBuffer(buffer_size=math.inf) - info = CameraInfo( - frame_id="cam", - height=40, - width=40, - K=[40.0, 0.0, 20.0, 0.0, 40.0, 20.0, 0.0, 0.0, 1.0], + info = camera_info_from_intrinsics( + 40, 40, 20, 20, 40, 40, header=Header(stamp=time_from_seconds(0), frame_id="cam") ) for k, (ts, x_offset) in enumerate(((1.0, 0.0), (2.0, 0.02))): rgb = np.full((40, 40, 3), 120 + k, dtype=np.uint8) rgb[10:30, 10:30] = 220 - color.append(Image.from_numpy(rgb, format=ImageFormat.RGB, frame_id="cam", ts=ts), ts=ts) + color.append( + image_from_array( + rgb, encoding="rgb8", header=Header(frame_id="cam", stamp=time_from_seconds(ts)) + ), + ts=ts, + ) depth_mm = np.full((40, 40), 500, dtype=np.uint16) depth_mm[10:30, 10:30] = 450 depth.append( - Image.from_numpy(depth_mm, format=ImageFormat.DEPTH16, frame_id="cam", ts=ts), ts=ts + image_from_array( + depth_mm, + encoding="16UC1", + header=Header(frame_id="cam", stamp=time_from_seconds(ts)), + ), + ts=ts, ) tf.receive_transform( - Transform( - translation=Vector3(x_offset, 0.0, 0.0), - frame_id="world", + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(ts)), child_frame_id="cam", - ts=ts, + transform=Transform( + translation=Vector3(x_offset, 0.0, 0.0), rotation=Quaternion(0.0, 0.0, 0.0, 1.0) + ), ) ) diff --git a/dimos/perception/localize/test_rig.py b/dimos/perception/localize/test_rig.py new file mode 100644 index 00000000000..6dd21140078 --- /dev/null +++ b/dimos/perception/localize/test_rig.py @@ -0,0 +1,367 @@ +# 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. + +"""Camera frames, depth joins, and lidar registration used by localization.""" + +from pathlib import Path +import subprocess +import sys + +from dimos_cdrgen.registry import decode, encode +import numpy as np +import pytest + +from dimos.memory.store.memory import MemoryStore +from dimos.memory.type.observation import Observation +from dimos.message_utils.camera_info import camera_info_from_intrinsics +from dimos.message_utils.image import image_from_array +from dimos.message_utils.pointcloud import pointcloud_from_xyz, pointcloud_xyz +from dimos.message_utils.time import time_from_seconds, to_seconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.perception.detection.project import _world_to_optical, sees +from dimos.perception.detection.type.detection3d.pointcloud import Detection3DPC +from dimos.perception.localize.association import Groups +from dimos.perception.localize.localize import index_frames +from dimos.perception.localize.module import as_detection_array +from dimos.perception.localize.rig import Rig, RigCamera, _camera_span +from dimos.perception.localize.types import Localization +from dimos.protocol.tf.tf import MultiTBuffer + + +def _transform(parent: str, child: str, x: float, ts: float = 2.0) -> TransformStamped: + return TransformStamped( + header=Header(stamp=time_from_seconds(ts), frame_id=parent), + child_frame_id=child, + transform=Transform( + translation=Vector3(x=x, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ) + + +@pytest.mark.parametrize("frame, expected_x", [("front", 2.5), ("rear", 1.5)]) +def test_camera_pose_composes_each_cameras_mount(frame: str, expected_x: float) -> None: + tf = MultiTBuffer() + tf.receive_transform( + _transform("world", "base_link", 2.0), + _transform("base_link", "front", 0.5), + _transform("base_link", "rear", -0.5), + ) + rig = Rig(cameras={}, tf=tf) + + pose = rig.camera_pose(2.0, frame) + camera = rig.world_to_optical(2.0, frame) + + assert pose is not None and camera is not None + assert pose.header.frame_id == "world" + assert pose.pose.position.x == expected_x + assert to_seconds(pose.header.stamp) == 2.0 + assert camera.header.frame_id == frame + assert camera.child_frame_id == "world" + assert camera.transform.translation.x == -expected_x + assert rig.camera_pose(3.0, frame) is None + + +@pytest.fixture +def memory(): + with MemoryStore() as store: + yield store + + +@pytest.mark.parametrize("depth_dtype", ["u2"]) +def test_from_store_finds_each_cameras_color_and_aligned_depth(memory, depth_dtype) -> None: + info = camera_info_from_intrinsics( + 100, 100, 1, 1, 2, 2, header=Header(frame_id="front", stamp=time_from_seconds(2)) + ) + for frame in ("front", "rear"): + info = decode(encode(info), CameraInfo) + info.header.frame_id = frame + memory.stream(f"{frame}_info", CameraInfo).append(info, ts=2.0) + # Wire data is flat; classification must inspect decoded pixels. + color = image_from_array( + np.full((2, 2, 3), 100, dtype=np.uint8), + encoding="rgb8", + header=Header(stamp=time_from_seconds(2.0), frame_id=frame), + ) + depth = image_from_array( + np.full((2, 2), 1000, dtype=depth_dtype), + encoding="16UC1", + header=Header(stamp=time_from_seconds(2.0), frame_id=frame), + ) + memory.stream(f"{frame}_color", Image).append(decode(encode(color), Image), ts=2.0) + memory.stream(f"{frame}_depth", Image).append(decode(encode(depth), Image), ts=2.0) + memory.stream("tf", TFMessage).append( + TFMessage(transforms=[_transform("world", "front", 2.5), _transform("world", "rear", 1.5)]), + ts=2.0, + ) + + rig = Rig.from_store(memory) + + assert set(rig.cameras) == {"front", "rear"} + assert rig.world_frame == "world" + assert rig.time_range() == (2.0, 2.0) + assert rig.depth_at(2.0, "rear").encoding == "16UC1" + assert rig.frame_at(Observation(ts=2.0, _data=color)).header.frame_id == "rear" + assert rig.camera_pose(2.0, "front").pose.position.x == 2.5 + + +def test_lidar_map_registers_and_deduplicates_scans_in_world_frame() -> None: + tf = MultiTBuffer() + tf.receive_transform(_transform("world", "lidar", 10.0)) + cloud = pointcloud_from_xyz( + np.array([[0.0, 0.0, 0.0], [0.01, 0.0, 0.0], [0.10, 0.0, 0.0]]), + header=Header(stamp=time_from_seconds(2.0), frame_id="lidar"), + ) + cloud.header.stamp = Time(sec=2, nanosec=123456789) + rig = Rig( + cameras={}, tf=tf, lidar=[Observation(ts=2.0, _data=decode(encode(cloud), PointCloud2))] + ) + + merged = rig.lidar_map() + + assert merged is not None + assert merged.header.frame_id == "world" + assert merged.header.stamp == cloud.header.stamp + np.testing.assert_allclose( + pointcloud_xyz(merged), [[10.0, 0.0, 0.0], [10.1, 0.0, 0.0]], atol=1e-6 + ) + assert rig.lidar_map() is merged + + +def test_frame_index_accepts_generated_camera_pose_without_changing_observation_api(memory): + tf = MultiTBuffer() + tf.receive_transform(_transform("world", "front", 2.5)) + rig = Rig(cameras={}, tf=tf) + image = image_from_array( + np.full((4, 4, 3), 100, dtype=np.uint8), + encoding="rgb8", + header=Header(frame_id="front", stamp=time_from_seconds(2)), + ) + feed = memory.stream("color", Image) + feed.append(image, ts=2) + frames = index_frames(rig, feed).to_list() + assert len(frames) == 1 + assert frames[0].data is image + assert frames[0].tags["camera"] == "front" + assert frames[0].pose_tuple == (2.5, 0, 0, 0, 0, 0, 1) + + +@pytest.mark.parametrize("depth_dtype", ["u2"]) +def test_visibility_uses_generated_mount_and_decoded_depth(depth_dtype): + mount = _transform("base_link", "camera_optical", 0.5) + observation = Observation(ts=2, _data=None, pose=(1, 2, 3)) + camera = camera_info_from_intrinsics( + 100, + 100, + 10, + 10, + 20, + 20, + header=mount.header, + ) + transform = _world_to_optical(observation, "world", None, mount, "camera_optical", 1) + assert transform is not None + assert ( + transform.transform.translation.x, + transform.transform.translation.y, + transform.transform.translation.z, + ) == (-1.5, -2, -3) + assert sees((1.5, 2, 5), camera, base_to_optical=mount)(observation) + depth = image_from_array( + np.full((20, 20), 1000, dtype=depth_dtype), + encoding="16UC1", + header=mount.header, + ) + depth = decode(encode(depth), Image) + assert not sees((1.5, 2, 5), camera, base_to_optical=mount, depth=lambda _: depth)(observation) + + +@pytest.mark.parametrize("voxel", [0.0, 0.1]) +def test_group_union_keeps_exact_newest_cloud_header(voxel): + header = Header(frame_id="world", stamp=Time(sec=1700000000, nanosec=123456788)) + image = image_from_array(np.zeros((2, 2, 3), dtype=np.uint8), encoding="rgb8", header=header) + groups = Groups() + for ns, points in [ + (123456789, [[0, 0, 2], [0.01, 0, 2]]), + (123456788, [[0.02, 0, 2], [0.03, 0, 2]]), + ]: + cloud = pointcloud_from_xyz( + np.array(points), + header=Header(frame_id="world", stamp=Time(sec=1700000000, nanosec=ns)), + ) + detection = Detection3DPC( + image=image, + ts=1700000000.1234567, + bbox=(0, 0, 2, 2), + track_id=-1, + class_id=0, + confidence=0.9, + name="target", + pointcloud=cloud, + ) + assert groups.add(detection, radius=0.1, voxel=voxel) == 0 + merged = decode(encode(groups.clouds[0]), PointCloud2) + assert merged.header.stamp == Time(sec=1700000000, nanosec=123456789) + assert pointcloud_xyz(merged).mean(axis=0) == pytest.approx([0.015, 0, 2]) + assert len(pointcloud_xyz(merged)) == (1 if voxel else 4) + + +def test_localization_boxes_are_generated_cdr_values(): + cloud = pointcloud_from_xyz( + np.array([[1, 2, 3], [3, 4, 5]]), + header=Header(frame_id="world", stamp=time_from_seconds(2)), + ) + hit = Localization( + instance_id="object-1", + semantic_score=0.9, + ambiguity_margin=0.2, + position_world_xyz=(2, 3, 4), + frame_id="world", + last_seen_timestamp=2, + point_cloud=cloud, + n_views=2, + ) + message = as_detection_array(["cup"], [[hit]], "world") + message = decode(encode(message), type(message)) + assert message.header.frame_id == "world" + assert message.detections[0].id == "object-1" + assert message.detections[0].bbox.center.position.x == 2 + assert message.detections[0].bbox.size.z == 2 + assert message.detections[0].results[0].hypothesis.class_id == "cup" + + +def test_documented_localization_geometry_executes(): + guide = Path(__file__).resolve().parents[3] / "docs/capabilities/perception/index.md" + section = ( + guide.read_text().split("## Query-time localization geometry\n", 1)[1].split("\n## ", 1)[0] + ) + example = section.split("```python\n", 1)[1].split("```", 1)[0] + exec(compile(example, str(guide), "exec"), {}) + + +def test_offline_renderer_uses_generated_images_and_local_camera_pinhole(tmp_path, monkeypatch): + monkeypatch.setenv("CI", "1") + monkeypatch.delenv("FORCE_RERUN_ANALYTICS", raising=False) + output = tmp_path / "localization.rrd" + # rr.init owns process-global state: the child releases the recording on exit. + script = """ +import sys +import numpy as np +import rerun as rr +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.std_msgs.msg import Header +from dimos.memory.store.memory import MemoryStore +from dimos.memory.type.observation import Observation +from dimos.message_utils.camera_info import camera_info_from_intrinsics +from dimos.message_utils.geometry import inverse_transform +from dimos.message_utils.image import image_from_array +from dimos.message_utils.pointcloud import pointcloud_from_xyz +from dimos.message_utils.time import time_from_seconds +from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D +from dimos.perception.detection.type.detection3d.pointcloud import Detection3DPC +from dimos.perception.localize.localize import LocalizeTrace +from dimos.perception.localize.rig import Rig, RigCamera +from dimos.perception.localize.tools.tool_localize import render +from dimos.protocol.tf.tf import MultiTBuffer +with MemoryStore() as memory: + header = Header(frame_id="front", stamp=time_from_seconds(2)) + image = image_from_array( + np.full((4, 4, 3), 100, dtype=np.uint8), encoding="rgb8", header=header + ) + color = memory.stream("color", Image) + color.append(image, ts=2) + tf = MultiTBuffer() + mount = TransformStamped( + header=Header(frame_id="world", stamp=header.stamp), child_frame_id="front", + transform=Transform(translation=Vector3(x=0, y=0, z=0), rotation=Quaternion(x=0, y=0, z=0, w=1)), + ) + tf.receive_transform(mount) + cloud = pointcloud_from_xyz( + np.array([[0, 0, 2], [0.01, 0, 2.01]]), header=Header(frame_id="world", stamp=header.stamp) + ) + rig = Rig( + cameras={ + "front": RigCamera( + info=camera_info_from_intrinsics(1, 1, 2, 2, 4, 4, header=header), + color=color, + depth=None, + ) + }, + tf=tf, + lidar=[Observation(ts=2, _data=cloud)], + ) + detection = Detection3DPC( + image=image, + ts=2, + bbox=(0, 0, 2, 2), + track_id=-1, + class_id=0, + confidence=0.9, + name="cup", + pointcloud=cloud, + transform=inverse_transform(mount), + ) + trace = LocalizeTrace( + answers=[[detection]], + detection_frames=[Observation(ts=2, _data=ImageDetections2D(image, [detection]))], + ) + render(sys.argv[1], rig, [("cup", trace)], 2, 1, 3) + recording = rr.get_global_data_recording() + recording.flush() + recording.disconnect() +""" + result = subprocess.run( + [sys.executable, "-I", "-c", script, str(output)], + capture_output=True, + text=True, + timeout=30, + ) + assert result.returncode == 0, result.stderr + result = subprocess.run( + [str(Path(sys.executable).parent / "rerun"), "rrd", "print", "-vvv", str(output)], + check=True, + capture_output=True, + text=True, + timeout=30, + ) + assert "Pinhole:image_from_camera" in result.stdout + assert "Points3D:positions" in result.stdout + assert "Image:buffer" in result.stdout + assert "detections/cup/answer/0/front" in result.stdout + + +def test_camera_travel_uses_generated_tf_positions(memory): + color = memory.stream("color", Image) + for ts in (0.0, 11.0): + color.append( + image_from_array( + np.ones((2, 2, 3), dtype=np.uint8), + encoding="rgb8", + header=Header(stamp=time_from_seconds(ts), frame_id="camera"), + ), + ts=ts, + ) + tf = MultiTBuffer(buffer_size=100) + for ts in range(12): + tf.receive_transform(_transform("world", "camera", ts * 0.5, ts=float(ts))) + info = camera_info_from_intrinsics( + 1, 1, 1, 1, 2, 2, header=Header(stamp=time_from_seconds(0), frame_id="camera") + ) + cameras = {"camera": RigCamera(info=info, color=color, depth=None)} + assert _camera_span(cameras, tf, "world") == 5.5 diff --git a/dimos/perception/localize/tools/tool_localize.py b/dimos/perception/localize/tools/tool_localize.py index 2047618efa7..cf9ed17ccf7 100644 --- a/dimos/perception/localize/tools/tool_localize.py +++ b/dimos/perception/localize/tools/tool_localize.py @@ -47,8 +47,23 @@ from dimos.memory.store.sqlite import SqliteStore from dimos.memory.transform import throttle -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.Image import Image +from dimos.message_utils.geometry import ( + inverse_transform, + pose_from_transform, + pose_matrix, + transform_matrix, + vector_array, +) +from dimos.message_utils.image import image_from_array, image_to_bgr, image_to_rgb +from dimos.message_utils.pointcloud import ( + pointcloud_from_xyz, + pointcloud_xyz, + voxel_downsample_cloud, +) +from dimos.message_utils.time import time_from_seconds +from dimos.messages.geometry_msgs.msg import PoseStamped +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.std_msgs.msg import Header from dimos.perception.detection.type.detection3d.pointcloud import Detection3DPC, lattice_quantum from dimos.perception.localize.dandetect import DanDetector from dimos.perception.localize.localize import LocalizeTrace, embed_index @@ -155,13 +170,18 @@ def render( with it. The 3D view hides those frames, and ``cameras`` unless there is a robot, until toggled on. """ + from matplotlib import colormaps import rerun as rr import rerun.blueprint as rrb from dimos.memory.vis.color import Color - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2, _get_colormap_lut + from dimos.visualization.rerun.cdr import camera_pinhole, image_archetype from dimos.visualization.rerun.init import rerun_init + def local_pinhole(info: Any) -> Any: + # Camera poses are logged directly at these entities, not in a TF frame tree. + return camera_pinhole(replace(info, header=replace(info.header, frame_id=""))) + names = {frame: rr.escape_entity_path_part(frame) for frame in rig.cameras} robot = base_pose(rig, backdrop_ts) is not None hidden = [] if robot else ["cameras"] @@ -187,6 +207,10 @@ def render( ) ) + def pose_archetype(pose: PoseStamped) -> Any: + matrix = pose_matrix(pose.pose) + return rr.Transform3D(translation=matrix[:3, 3], mat3x3=matrix[:3, :3]) + def at(ts: float) -> None: rr.set_time("ts", timestamp=ts) @@ -197,7 +221,7 @@ def cell_keys(points: np.ndarray) -> np.ndarray: # map cells claimed by a detected object, plus one cell of halo answer_points = np.vstack( [ - det.pointcloud.points_f32() + pointcloud_xyz(det.pointcloud) for _, trace in traces for members in trace.answers for det in members @@ -212,15 +236,17 @@ def carve(points: np.ndarray) -> np.ndarray: def image_colors(det: Any) -> np.ndarray: """Sample the sighting's own image at each cloud point's reprojection.""" - points = det.pointcloud.points_f32() - matrix = det.transform.to_matrix() + points = pointcloud_xyz(det.pointcloud) + matrix = transform_matrix(det.transform.transform) cam = points @ matrix[:3, :3].T + matrix[:3, 3] - pixels = Detection3DPC.project_pixels(cam, rig.cameras[det.image.frame_id].info) + pixels = Detection3DPC.project_pixels(cam, rig.cameras[det.image.header.frame_id].info) cols = np.round(pixels[:, 0]).astype(int) rows = np.round(pixels[:, 1]).astype(int) - rgb = det.image.to_rgb().data + rgb = image_to_rgb(det.image) height, width = rgb.shape[:2] - return rgb[np.clip(rows, 0, height - 1), np.clip(cols, 0, width - 1)] + return np.asarray( + rgb[np.clip(rows, 0, height - 1), np.clip(cols, 0, width - 1)], dtype=np.uint8 + ) # backdrop: each depth camera's RGBD cloud at the answer frame, on a mobile # rig its frames over the window merged, and the lidar's scans over the @@ -232,27 +258,30 @@ def image_colors(det: Any) -> np.ndarray: continue if rig.mobile: parts = [ - cloud.voxel_downsample(WALK_MAP_CELL).points_f32() + pointcloud_xyz(voxel_downsample_cloud(cloud, WALK_MAP_CELL)) for obs in rig.cameras[frame].color.after(t0).before(t1).transform(throttle(1.0)) if (cloud := rig.backdrop(obs.ts, frame, WALK_DEPTH_TRUNC)) is not None ] if not parts: continue - merged = PointCloud2.from_numpy(np.vstack(parts), frame_id=rig.world_frame) - carved = carve(merged.voxel_downsample(WALK_MAP_CELL).points_f32()) + merged = pointcloud_from_xyz( + np.vstack(parts), + header=Header(stamp=time_from_seconds(backdrop_ts), frame_id=rig.world_frame), + ) + carved = carve(pointcloud_xyz(voxel_downsample_cloud(merged, WALK_MAP_CELL))) spacing = WALK_MAP_CELL else: backdrop = rig.backdrop(backdrop_ts, frame) if backdrop is None: continue - carved = carve(backdrop.voxel_downsample(MAP_CELL).points_f32()) + carved = carve(pointcloud_xyz(voxel_downsample_cloud(backdrop, MAP_CELL))) spacing = POINT_SIZE if len(carved): views[name] = (carved, spacing) lidar_size = LIDAR_POINT_SIZE lidar_map = rig.lidar_map() if through_lidar else None if lidar_map is not None: - points = lidar_map.points_f32() + points = pointcloud_xyz(lidar_map) lidar_size = lattice_quantum(points) or LIDAR_POINT_SIZE carved = carve(points) if len(carved): @@ -261,7 +290,7 @@ def image_colors(det: Any) -> np.ndarray: if views: heights = np.concatenate([carved[:, 2] for carved, _ in views.values()]) low, high = float(heights.min()), float(heights.max()) - turbo = _get_colormap_lut("turbo") + turbo = (colormaps["turbo"](np.linspace(0, 1, 256))[:, :3] * 255).astype(np.uint8) for name, (carved, spacing) in views.items(): t = np.clip((carved[:, 2] - low) / (high - low + 1e-8), 0.0, 1.0) colors = turbo[(t * 255).astype(np.uint8)].astype(np.float32) @@ -276,7 +305,11 @@ def image_colors(det: Any) -> np.ndarray: # each camera's live feed + frustum tracking its capture pose along the # timeline; a robot's body box follows its base frame for frame, name in names.items(): - rr.log(f"cameras/{name}", rig.cameras[frame].info.to_rerun(), static=True) + rr.log( + f"cameras/{name}", + local_pinhole(rig.cameras[frame].info), + static=True, + ) if robot: body = rr.Boxes3D(half_sizes=[ROBOT_HALF_SIZES], colors=[ROBOT_COLOR]) rr.log("robot", body, static=True) @@ -288,16 +321,16 @@ def image_colors(det: Any) -> np.ndarray: key=lambda obs: obs.ts, ) for obs in feed: - frame = obs.data.frame_id + frame = obs.data.header.frame_id pose = rig.camera_pose(obs.ts, frame) if pose is None: continue at(obs.ts) - rr.log(f"cameras/{names[frame]}/image", obs.data.to_rerun()) - rr.log(f"cameras/{names[frame]}", pose.to_rerun()) + rr.log(f"cameras/{names[frame]}/image", image_archetype(obs.data)) + rr.log(f"cameras/{names[frame]}", pose_archetype(pose)) base = base_pose(rig, obs.ts) if robot else None if base is not None: - rr.log("robot", base.to_rerun()) + rr.log("robot", pose_archetype(base)) n_instances = sum(len(trace.answers) for _, trace in traces) box_colors = [ @@ -315,29 +348,31 @@ def image_colors(det: Any) -> np.ndarray: for _, trace in traces: for obs in trace.detection_frames: image = obs.data.image - shots.setdefault((image.frame_id, obs.ts), (image, []))[1].extend(obs.data.detections) + shots.setdefault((image.header.frame_id, obs.ts), (image, []))[1].extend( + obs.data.detections + ) for (camera, ts), (image, dets) in sorted(shots.items(), key=lambda shot: shot[0][1]): if rig.camera_pose(ts, camera) is None: continue at(ts) marked = overlay(image, best_per_box(dets), label_colors) - rr.log(f"cameras/{names[camera]}/image", marked.to_rerun()) + rr.log(f"cameras/{names[camera]}/image", image_archetype(marked)) for query, trace in traces: root = f"detections/{query.replace(' ', '_')}" # a frozen frustum at each detection frame's capture pose, with this label's masks for i, obs in enumerate(trace.detection_frames): - camera = obs.data.image.frame_id + camera = obs.data.image.header.frame_id pose = rig.camera_pose(obs.ts, camera) if pose is None: continue at(obs.ts) frame = f"{root}/frames/{i}" - rr.log(frame, pose.to_rerun()) - rr.log(frame, rig.cameras[camera].info.to_rerun()) + rr.log(frame, pose_archetype(pose)) + rr.log(frame, local_pinhole(rig.cameras[camera].info)) marked = overlay(obs.data.image, obs.data.detections, label_colors) - rr.log(f"{frame}/image", marked.to_rerun()) + rr.log(f"{frame}/image", image_archetype(marked)) # the answers: per verified instance, every sighting's cloud colored # by its own image and grouped by camera, wrapped in a labeled @@ -347,13 +382,13 @@ def image_colors(det: Any) -> np.ndarray: rr.log( f"{root}/answer/{i}/{names[camera]}", rr.Points3D( - positions=np.vstack([det.pointcloud.points_f32() for det in sightings]), + positions=np.vstack([pointcloud_xyz(det.pointcloud) for det in sightings]), colors=np.vstack([image_colors(det) for det in sightings]), radii=size[camera] / 2, ), static=True, ) - positions = np.vstack([det.pointcloud.points_f32() for det in members]) + positions = np.vstack([pointcloud_xyz(det.pointcloud) for det in members]) aabb_min, aabb_max = positions.min(axis=0), positions.max(axis=0) score = max(det.confidence for det in members) rr.log( @@ -375,13 +410,15 @@ def overlay(image: Image, detections: list[Any], colors: dict[str, tuple[int, in its box and its label; a detection without a mask gets the box alone.""" import cv2 - canvas = image.to_opencv().copy() + canvas = image_to_bgr(image).copy() tint = canvas.copy() for det in detections: mask = getattr(det, "mask", None) if mask is not None: tint[mask > 0] = colors[det.name][::-1] - canvas = cv2.addWeighted(tint, MASK_ALPHA, canvas, 1.0 - MASK_ALPHA, 0.0) + canvas = np.asarray( + cv2.addWeighted(tint, MASK_ALPHA, canvas, 1.0 - MASK_ALPHA, 0.0), dtype=np.uint8 + ) for det in detections: bgr = colors[det.name][::-1] mask = getattr(det, "mask", None) @@ -399,7 +436,7 @@ def overlay(image: Image, detections: list[Any], colors: dict[str, tuple[int, in cv2.putText( canvas, label, (x1 + 2, y1 - 4), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1 ) - return Image.from_opencv(canvas, frame_id=image.frame_id, ts=image.ts) + return image_from_array(canvas, encoding="bgr8", header=image.header) def best_per_box(detections: list[Any]) -> list[Any]: @@ -416,14 +453,14 @@ def best_per_box(detections: list[Any]) -> list[Any]: def base_pose(rig: Rig, ts: float) -> PoseStamped | None: """World pose of the robot's base frame at ts; None when tf has none.""" transform = rig.tf.get(BASE_FRAME, rig.world_frame, ts, TF_TOLERANCE) - return (-transform).to_pose() if transform is not None else None + return pose_from_transform(inverse_transform(transform)) if transform is not None else None def by_camera(rig: Rig, sightings: list[Detection3DPC]) -> dict[str, list[Detection3DPC]]: """An instance's sightings per camera, in the rig's camera order.""" grouped: dict[str, list[Detection3DPC]] = {frame: [] for frame in rig.cameras} for det in sightings: - grouped[det.image.frame_id].append(det) + grouped[det.image.header.frame_id].append(det) return {frame: dets for frame, dets in grouped.items() if dets} @@ -433,8 +470,10 @@ def camera_line(rig: Rig, sightings: list[Detection3DPC]) -> str: means: list[np.ndarray] = [] parts: list[str] = [] for frame, dets in by_camera(rig, sightings).items(): - centroids = [np.asarray(det.pointcloud.pointcloud.points).mean(axis=0) for det in dets] - cameras = np.array([(-det.transform).translation.to_numpy() for det in dets]) + centroids = [pointcloud_xyz(det.pointcloud).mean(axis=0) for det in dets] + cameras = np.array( + [vector_array(inverse_transform(det.transform).transform.translation) for det in dets] + ) views = len(np.unique(np.round(cameras, 2), axis=0)) mean = np.mean(centroids, axis=0) means.append(mean) @@ -456,7 +495,7 @@ def report(query: str, hits: list[Localization], trace: LocalizeTrace, rig: Rig, print( f"hit {query!r} [{hit.instance_id}]: position=({x:.3f}, {y:.3f}, {z:.3f}) " f"frame={hit.frame_id} ts_offset={hit.last_seen_timestamp - lo:.1f}s " - f"points={len(hit.point_cloud)} views={hit.n_views} " + f"points={hit.point_cloud.width * hit.point_cloud.height} views={hit.n_views} " f"score={hit.semantic_score:.2f} margin={hit.ambiguity_margin:.2f}" ) if len(rig.cameras) > 1: diff --git a/dimos/perception/localize/types.py b/dimos/perception/localize/types.py index daec0115f1d..a5896bc2d4f 100644 --- a/dimos/perception/localize/types.py +++ b/dimos/perception/localize/types.py @@ -20,7 +20,7 @@ from typing import TYPE_CHECKING if TYPE_CHECKING: - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + from dimos.messages.sensor_msgs.msg import PointCloud2 @dataclass diff --git a/dimos/protocol/pubsub/impl/webrtc/providers/test_video_track.py b/dimos/protocol/pubsub/impl/webrtc/providers/test_video_track.py index 4a8abcf5eb6..52f6da8891e 100644 --- a/dimos/protocol/pubsub/impl/webrtc/providers/test_video_track.py +++ b/dimos/protocol/pubsub/impl/webrtc/providers/test_video_track.py @@ -18,11 +18,19 @@ from __future__ import annotations import asyncio +from contextlib import ExitStack +from dimos_cdrgen.registry import decode, encode import numpy as np import pytest +from dimos.core.transport import WebRTCVideoTransport +from dimos.message_utils.image import image_from_array +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.sensor_msgs.msg import Image as CdrImage +from dimos.messages.std_msgs.msg import Header from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.teleop.hosted.camera_mux import CameraMuxModule pytest.importorskip("aiortc") @@ -70,3 +78,98 @@ async def test_unconvertible_frames_are_skipped_not_raised() -> None: track.set_latest(Image(data=np.zeros((4, 6), np.uint8), format=ImageFormat.GRAY)) frame = await asyncio.wait_for(pending, 2) assert frame.format.name == "gray" + + +@pytest.fixture +async def cdr_track(): + track = CameraVideoTrack(asyncio.get_running_loop()) + track.arm() + yield track + track.stop() + + +async def test_generated_16bit_and_padded_images_convert_without_changing_source(cdr_track): + for encoding in ("mono16", "16UC1"): + source = image_from_array( + np.full((4, 6), 0x8000, np.uint16), + encoding=encoding, + header=Header(Time(1_789_654_321, 1), "camera"), + ) + cdr_track.set_latest(source) + frame = await asyncio.wait_for(cdr_track.recv(), 2) + assert (frame.format.name, frame.width, frame.height) == ("rgb24", 6, 4) + assert frame.to_ndarray()[0, 0].tolist() == [128, 128, 128] + assert source.header == Header(Time(1_789_654_321, 1), "camera") + pixels = np.array([255, 0, 0, 0, 255, 0, 9, 9] * 2, np.uint8) + padded = CdrImage(Header(Time(0, 1), "camera"), 2, 2, "rgb8", 0, 8, pixels) + cdr_track.set_latest(padded) + frame = await asyncio.wait_for(cdr_track.recv(), 2) + assert frame.to_ndarray(format="rgb24")[0].tolist() == [[255, 0, 0], [0, 255, 0]] + np.testing.assert_array_equal(padded.data, pixels) + + +async def test_generated_odd_image_opens_real_h264_encoder(cdr_track): + source = image_from_array(np.full((47, 63, 3), [255, 0, 0], np.uint8), encoding="rgb8") + cdr_track.set_latest(source) + frame = await asyncio.wait_for(cdr_track.recv(), 2) + assert (frame.width, frame.height) == (62, 46) + assert frame.to_ndarray(format="rgb24")[0, 0].tolist() == [255, 0, 0] + payloads, _ = H264Encoder().encode(frame, force_keyframe=True) + assert payloads + assert (source.width, source.height) == (63, 47) + + +async def test_generated_bad_frames_leave_receiver_waiting_for_valid_image(cdr_track, mocker): + converted = asyncio.Event() + original = cdr_track._to_av + + def convert(image): + result = original(image) + converted.set() + return result + + mocker.patch.object(cdr_track, "_to_av", side_effect=convert) + pending = asyncio.create_task(cdr_track.recv()) + try: + for bad in ( + image_from_array(np.zeros((4, 6), np.float32), encoding="32FC1"), + CdrImage(Header(Time(0, 0), ""), 4, 6, "mono16", 0, 1, np.zeros(24, np.uint8)), + ): + cdr_track.set_latest(bad) + await asyncio.wait_for(converted.wait(), 2) + converted.clear() + assert not pending.done() + cdr_track.set_latest(image_from_array(np.zeros((4, 6), np.uint8), encoding="mono8")) + frame = await asyncio.wait_for(pending, 2) + assert frame.format.name == "gray" + finally: + pending.cancel() + await asyncio.gather(pending, return_exceptions=True) + + +async def test_cdr_camera_mux_port_feeds_media_track_and_real_encoder(cdr_track, mocker): + provider = mocker.Mock() + provider.is_connected = True + provider.set_video_frame.side_effect = cdr_track.set_latest + config = mocker.Mock() + config.provider.return_value = provider + source = image_from_array( + np.full((47, 63, 3), [255, 0, 0], np.uint8), + encoding="rgb8", + header=Header(Time(17, 1), "camera"), + ) + with ExitStack() as resources: + mux = CameraMuxModule(cameras=["cam1"]) + resources.callback(mux.stop) + mux.set_transport("mux_image", WebRTCVideoTransport(config=config)) + mux._on_cam("cam1", decode(encode(source), CdrImage)) + frame = await asyncio.wait_for(cdr_track.recv(), 2) + assert (frame.width, frame.height) == (62, 46) + assert frame.to_ndarray(format="rgb24")[0, 0].tolist() == [255, 0, 0] + sent = provider.set_video_frame.call_args.args[0] + assert type(sent) is CdrImage + assert sent.header == Header(Time(17, 1), "camera") + provider.start.assert_not_called() + payloads, _ = H264Encoder().encode(frame, force_keyframe=True) + assert payloads + assert source.width == 63 and source.height == 47 diff --git a/dimos/protocol/pubsub/impl/webrtc/providers/video_track.py b/dimos/protocol/pubsub/impl/webrtc/providers/video_track.py index eaa443b4367..4cc37eb74cf 100644 --- a/dimos/protocol/pubsub/impl/webrtc/providers/video_track.py +++ b/dimos/protocol/pubsub/impl/webrtc/providers/video_track.py @@ -29,6 +29,8 @@ import av import numpy as np +from dimos.message_utils.image import image_to_rgb, image_view +from dimos.messages.sensor_msgs.msg import Image as CdrImage from dimos.msgs.sensor_msgs.Image import Image, ImageFormat from dimos.utils.logging_config import setup_logger @@ -72,13 +74,13 @@ class CameraVideoTrack(VideoStreamTrack): def __init__(self, loop: asyncio.AbstractEventLoop) -> None: super().__init__() self._loop: asyncio.AbstractEventLoop = loop - self._latest: Image | None = None + self._latest: Image | CdrImage | None = None self._frame_seq = 0 self._consumed_seq = 0 self._armed = False self._first_mono: float | None = None self._new_frame = asyncio.Event() - self._dropped: set[ImageFormat] = set() + self._dropped: set[str] = set() def arm(self) -> None: """Discard buffered frames; start delivering from now. @@ -89,7 +91,7 @@ def arm(self) -> None: self._consumed_seq = self._frame_seq self._armed = True - def set_latest(self, img: Image) -> None: + def set_latest(self, img: Image | CdrImage) -> None: """Publish the latest frame. Called from the producer (stream) thread. aiortc / asyncio.Event aren't thread-safe, so marshal the swap + @@ -106,26 +108,37 @@ def _set() -> None: except RuntimeError: return - def _to_av(self, img: Image) -> av.VideoFrame | None: + def _to_av(self, img: Image | CdrImage) -> av.VideoFrame | None: """None, logged once per source format, for what neither to_rgb() nor av takes (DEPTH float32, a malformed array). Never raises: recv() runs in aiortc's sender task, which ends the track on an exception.""" try: - frame = img if img.format in _AV_FORMAT_MAP else img.to_rgb() - data = frame.data + if isinstance(img, CdrImage): + formats = { + "bgr8": "bgr24", + "rgb8": "rgb24", + "bgra8": "bgra", + "rgba8": "rgba", + "mono8": "gray", + } + data = image_view(img) if img.encoding in formats else image_to_rgb(img) + av_format = formats.get(img.encoding, "rgb24") + else: + frame = img if img.format in _AV_FORMAT_MAP else img.to_rgb() + data = frame.data + av_format = _AV_FORMAT_MAP[frame.format] h, w = data.shape[:2] if h % 2 or w % 2: # libx264 takes yuv420p, whose chroma planes are half-size: an # odd width or height fails avcodec_open2 in aiortc's sender # task, past this guard. The last row/column is cropped. data = np.ascontiguousarray(data[: h - h % 2, : w - w % 2]) - return av.VideoFrame.from_ndarray(data, format=_AV_FORMAT_MAP[frame.format]) + return av.VideoFrame.from_ndarray(data, format=av_format) except Exception as e: - if img.format not in self._dropped: - self._dropped.add(img.format) - logger.warning( - "video track: dropping %s/%s frames: %s", img.format.value, img.data.dtype, e - ) + encoding = img.encoding if isinstance(img, CdrImage) else img.format.value + if encoding not in self._dropped: + self._dropped.add(encoding) + logger.warning("video track: dropping %s frames: %s", encoding, e) return None async def recv(self) -> av.VideoFrame: diff --git a/dimos/protocol/tf/static_tf_publisher.py b/dimos/protocol/tf/static_tf_publisher.py index 0f5b6d12625..8f42a27e15c 100644 --- a/dimos/protocol/tf/static_tf_publisher.py +++ b/dimos/protocol/tf/static_tf_publisher.py @@ -32,10 +32,12 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import Out -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.tf2_msgs.TFMessage import TFMessage +from dimos.message_utils.geometry import quaternion_from_euler +from dimos.message_utils.time import time_from_nanoseconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Transform, TransformStamped, Vector3 +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -44,23 +46,25 @@ FrameSpec = tuple[str, str | None, tuple[float, float, float], tuple[float, float, float]] -def frames_to_edge_transforms(frames: list[FrameSpec]) -> list[Transform]: - """Build a ``parent -> child`` Transform for each non-root edge of a frame tree. +def frames_to_edge_transforms(frames: list[FrameSpec]) -> list[TransformStamped]: + """Build a ``parent -> child`` TransformStamped for each non-root edge of a frame tree. This is the static mount tree (the rigid sensor offsets); a tf buffer composes these edges to answer any ``world <- frame`` query once odometry supplies the moving ``world <- root`` edge. """ - transforms: list[Transform] = [] + transforms: list[TransformStamped] = [] for name, parent, translation, rpy in frames: if parent is None: continue transforms.append( - Transform( - translation=Vector3(*translation), - rotation=Quaternion.from_euler(Vector3(*rpy)), - frame_id=parent, + TransformStamped( + header=Header(frame_id=parent, stamp=Time(sec=0, nanosec=0)), child_frame_id=name, + transform=Transform( + translation=Vector3(x=translation[0], y=translation[1], z=translation[2]), + rotation=quaternion_from_euler(*rpy), + ), ) ) return transforms @@ -77,9 +81,9 @@ class StaticTfPublisher(Module): tf: Out[TFMessage] _running: bool = False - _transforms: list[Transform] = [] + _transforms: list[TransformStamped] = [] - def transforms(self) -> list[Transform]: + def transforms(self) -> list[TransformStamped]: """The static transforms to publish. Override in a rig-specific subclass.""" raise NotImplementedError( f"{type(self).__name__} must override transforms() with its mount frames" @@ -104,10 +108,10 @@ def start(self) -> None: async def _publish_loop(self) -> None: period = 1.0 / self.config.publish_hz while self._running: - now = time.time() + now = time_from_nanoseconds(time.time_ns()) for transform in self._transforms: - transform.ts = now - self.tf.publish(TFMessage(*self._transforms)) + transform.header.stamp = now + self.tf.publish(TFMessage(transforms=self._transforms)) await asyncio.sleep(period) @rpc diff --git a/dimos/protocol/tf/test_static_tf_publisher.py b/dimos/protocol/tf/test_static_tf_publisher.py new file mode 100644 index 00000000000..f27dc4055c1 --- /dev/null +++ b/dimos/protocol/tf/test_static_tf_publisher.py @@ -0,0 +1,48 @@ +# 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. + +"""Static frame definitions become generated ROS TF edges without convenience types.""" + +import math + +from dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode +import pytest + +from dimos.message_utils.geometry import compose_transforms +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.protocol.tf.static_tf_publisher import frames_to_edge_transforms + + +def test_frame_tree_skips_root_and_composes_rotated_mounts(): + edges = frames_to_edge_transforms( + [ + ("world", None, (0, 0, 0), (0, 0, 0)), + ("body", "world", (1, 0, 0), (0, 0, math.pi / 2)), + ("camera", "body", (2, 0, 3), (0, 0, 0)), + ] + ) + assert [(edge.header.frame_id, edge.child_frame_id) for edge in edges] == [ + ("world", "body"), + ("body", "camera"), + ] + decoded = cdr_decode(cdr_encode(TFMessage(transforms=edges)), TFMessage) + combined = compose_transforms(*decoded.transforms) + translation = combined.transform.translation + assert (translation.x, translation.y, translation.z) == pytest.approx((1, 2, 3)) + assert combined.header.stamp.sec == 0 + assert combined.header.stamp.nanosec == 0 + + +def test_empty_frame_tree_produces_no_edges(): + assert frames_to_edge_transforms([]) == [] diff --git a/dimos/protocol/tf/test_tf.py b/dimos/protocol/tf/test_tf.py index 47e791ec94c..a06fc58fa7e 100644 --- a/dimos/protocol/tf/test_tf.py +++ b/dimos/protocol/tf/test_tf.py @@ -14,92 +14,114 @@ # See the License for the specific language governing permissions and # limitations under the License. +from contextlib import ExitStack import math import threading import time import pytest -from dimos.core.transport_factory import make_transport +from dimos.core.transport import LCMTransport from dimos.memory.store.memory import MemoryStore from dimos.memory.store.sqlite import SqliteStore from dimos.memory.tf import StreamTF -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -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.tf2_msgs.TFMessage import TFMessage +from dimos.message_utils.geometry import quaternion_from_euler +from dimos.message_utils.time import time_from_seconds, to_seconds +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage from dimos.protocol.tf.tf import TF, MultiTBuffer, TBuffer, TFLookup +@pytest.fixture +def tf_pair(): + with ExitStack() as cleanup: + transport = LCMTransport("/tf", TFMessage, url="memq://") + cleanup.callback(transport.stop) + broadcaster = TF(transport) + cleanup.callback(broadcaster.dispose) + querier = TF(transport) + cleanup.callback(querier.dispose) + yield broadcaster, querier + + +@pytest.fixture +def wait_started(mocker): + def observe(buffer): + waiting = threading.Event() + actual_wait = buffer._cv.wait + + def wait(timeout=None): + waiting.set() + return actual_wait(timeout) + + mocker.patch.object(buffer._cv, "wait", side_effect=wait) + return waiting + + return observe + + # from https://foxglove.dev/blog/understanding-ros-transforms def test_tf_ros_example() -> None: tf = TF() - base_link_to_arm = Transform( - translation=Vector3(1.0, -1.0, 0.0), - rotation=Quaternion.from_euler(Vector3(0, 0, math.pi / 6)), - frame_id="base_link", + base_link_to_arm = TransformStamped( + header=Header(frame_id="base_link", stamp=time_from_seconds(time.time())), child_frame_id="arm", - ts=time.time(), + transform=Transform( + translation=Vector3(x=1.0, y=-1.0, z=0.0), + rotation=quaternion_from_euler(0, 0, math.pi / 6), + ), ) - arm_to_end = Transform( - translation=Vector3(1.0, 1.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), # Identity rotation - frame_id="arm", + arm_to_end = TransformStamped( + header=Header(frame_id="arm", stamp=time_from_seconds(time.time())), child_frame_id="end_effector", - ts=time.time(), + transform=Transform( + translation=Vector3(x=1.0, y=1.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) tf.publish(base_link_to_arm, arm_to_end) - time.sleep(0.2) end_effector_global_pose = tf.get("base_link", "end_effector") assert end_effector_global_pose is not None - assert end_effector_global_pose.translation.x == pytest.approx(1.366, abs=1e-3) - assert end_effector_global_pose.translation.y == pytest.approx(0.366, abs=1e-3) + assert end_effector_global_pose.transform.translation.x == pytest.approx(1.366, abs=1e-3) + assert end_effector_global_pose.transform.translation.y == pytest.approx(0.366, abs=1e-3) tf.dispose() -def test_tf_main() -> None: - """Test TF broadcasting and querying between two TF instances. - If you run rerun-bridge this will show up in the UI""" - - # here we create broadcasting and receiving TF instance. - # this is to verify that comms work multiprocess, normally - # you'd use only one instance in your module - broadcaster_transport = make_transport("/tf", TFMessage) - querier_transport = make_transport("/tf", TFMessage) - broadcaster = TF(broadcaster_transport) - querier = TF(querier_transport) +def test_tf_main(tf_pair) -> None: + """Two TF views exchange generated values over an explicit CDR transport.""" + broadcaster, querier = tf_pair # Create a transform from world to robot current_time = time.time() - world_to_charger = Transform( - translation=Vector3(2.0, -2.0, 0.0), - rotation=Quaternion.from_euler(Vector3(0, 0, 2)), - frame_id="world", + world_to_charger = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(current_time)), child_frame_id="charger", - ts=current_time, + transform=Transform( + translation=Vector3(x=2.0, y=-2.0, z=0.0), rotation=quaternion_from_euler(0, 0, 2) + ), ) - world_to_robot = Transform( - translation=Vector3(1.0, 2.0, 3.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), # Identity rotation - frame_id="world", + world_to_robot = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(current_time)), child_frame_id="robot", - ts=current_time, + transform=Transform( + translation=Vector3(x=1.0, y=2.0, z=3.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) # Broadcast the transform broadcaster.publish(world_to_robot) broadcaster.publish(world_to_charger) - # Give time for the message to propagate - time.sleep(0.05) + assert querier.get("world", "robot", forward_tolerance=2.0) is not None # Verify frames are available frames = querier.get_frames() @@ -107,19 +129,20 @@ def test_tf_main() -> None: assert "robot" in frames # Add another transform in the chain - robot_to_sensor = Transform( - translation=Vector3(0.5, 0.0, 0.2), - rotation=Quaternion(0.0, 0.0, 0.707107, 0.707107), # 90 degrees around Z - frame_id="robot", + robot_to_sensor = TransformStamped( + header=Header(frame_id="robot", stamp=time_from_seconds(current_time)), child_frame_id="sensor", - ts=current_time, + transform=Transform( + translation=Vector3(x=0.5, y=0.0, z=0.2), + rotation=Quaternion(x=0.0, y=0.0, z=0.707107, w=0.707107), + ), ) broadcaster.publish(robot_to_sensor) - time.sleep(0.05) + assert querier.get("world", "sensor", forward_tolerance=2.0) is not None - # we can now query (from a separate process given we use querier) the transform tree + # The receiving TF view resolves the same frame chain as the publisher. chain_transform = querier.get("world", "sensor") # broadcaster will agree with us @@ -128,27 +151,19 @@ def test_tf_main() -> None: # The chain should compose: world->robot (1,2,3) + robot->sensor (0.5,0,0.2) # Expected translation: (1.5, 2.0, 3.2) assert chain_transform is not None - assert abs(chain_transform.translation.x - 1.5) < 0.001 - assert abs(chain_transform.translation.y - 2.0) < 0.001 - assert abs(chain_transform.translation.z - 3.2) < 0.001 - - # we see something on camera - random_object_in_view = PoseStamped( - frame_id="random_object", - position=Vector3(1, 0, 0), + assert abs(chain_transform.transform.translation.x - 1.5) < 0.001 + assert abs(chain_transform.transform.translation.y - 2.0) < 0.001 + assert abs(chain_transform.transform.translation.z - 3.2) < 0.001 + + # A perceived object is expressed as a stamped sensor -> object transform. + random_t = TransformStamped( + header=Header(frame_id="sensor", stamp=time_from_seconds(time.time())), + child_frame_id="random_object", + transform=Transform( + translation=Vector3(x=1, y=0, z=0), rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ), ) - print("Random obj", random_object_in_view) - - # random_object is perceived by the sensor - # we create a transform pointing from sensor to object - random_t = random_object_in_view.new_transform_from("sensor") - - # we could have also done - assert random_t == random_object_in_view.new_transform_to("sensor").inverse() - - print("randm t", random_t) - # we broadcast our object location broadcaster.publish(random_t) @@ -159,10 +174,7 @@ def test_tf_main() -> None: ## (this would mess with the transform system because it expects trees not graphs) ## and our random_object would get re-connected to world from sensor - print(broadcaster) - - # Give time for the message to propagate - time.sleep(0.05) + assert querier.get("world", "random_object", forward_tolerance=2.0) is not None # we know where the object is in the world frame now world_object = broadcaster.get("world", "random_object") @@ -170,44 +182,34 @@ def test_tf_main() -> None: # both instances agree assert querier.get("world", "random_object") == world_object - print("world object", world_object) - # if you have "diagon" https://diagon.arthursonzogni.com/ installed you can draw a graph - print(broadcaster.graph()) assert world_object is not None - assert abs(world_object.translation.x - 1.5) < 0.001 - assert abs(world_object.translation.y - 3.0) < 0.001 - assert abs(world_object.translation.z - 3.2) < 0.001 + assert abs(world_object.transform.translation.x - 1.5) < 0.001 + assert abs(world_object.transform.translation.y - 3.0) < 0.001 + assert abs(world_object.transform.translation.z - 3.2) < 0.001 # this doesn't work atm robot_to_charger = broadcaster.get("robot", "charger") assert robot_to_charger is not None # Expected: robot->world->charger - print(f"robot_to_charger translation: {robot_to_charger.translation}") - print(f"robot_to_charger rotation: {robot_to_charger.rotation}") - assert abs(robot_to_charger.translation.x - 1.0) < 0.001 - assert abs(robot_to_charger.translation.y - (-4.0)) < 0.001 - assert abs(robot_to_charger.translation.z - (-3.0)) < 0.001 - - # Stop services (they were autostarted but don't know how to autostop) - broadcaster.dispose() - querier.dispose() - broadcaster_transport.stop() - querier_transport.stop() + assert abs(robot_to_charger.transform.translation.x - 1.0) < 0.001 + assert abs(robot_to_charger.transform.translation.y - (-4.0)) < 0.001 + assert abs(robot_to_charger.transform.translation.z - (-3.0)) < 0.001 class TestTBuffer: def test_add_transform(self) -> None: buffer = TBuffer(buffer_size=10.0) - transform = Transform( - translation=Vector3(1.0, 2.0, 3.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="world", + transform = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(time.time())), child_frame_id="robot", - ts=time.time(), + transform=Transform( + translation=Vector3(x=1.0, y=2.0, z=3.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) buffer.add(transform) @@ -220,23 +222,25 @@ def test_get(self) -> None: # Add transforms at different times for i in range(3): - transform = Transform( - translation=Vector3(float(i), 0.0, 0.0), - frame_id="world", + transform = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time + i * 0.5)), child_frame_id="robot", - ts=base_time + i * 0.5, + transform=Transform( + translation=Vector3(x=float(i), y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) buffer.add(transform) # Test getting latest transform latest = buffer.get() assert latest is not None - assert latest.translation.x == 2.0 + assert latest.transform.translation.x == 2.0 # Test getting transform at specific time middle = buffer.get(time_point=base_time + 0.75) assert middle is not None - assert middle.translation.x == 2.0 # Closest to i=1 + assert middle.transform.translation.x == 2.0 # Closest to i=1 # Test time tolerance result = buffer.get(time_point=base_time + 10.0, time_tolerance=0.1) @@ -247,20 +251,24 @@ def test_buffer_pruning(self) -> None: # Add old transform old_time = time.time() - 2.0 - old_transform = Transform( - translation=Vector3(1.0, 0.0, 0.0), - frame_id="world", + old_transform = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(old_time)), child_frame_id="robot", - ts=old_time, + transform=Transform( + translation=Vector3(x=1.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) buffer.add(old_transform) # Add recent transform - recent_transform = Transform( - translation=Vector3(2.0, 0.0, 0.0), - frame_id="world", + recent_transform = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(time.time())), child_frame_id="robot", - ts=time.time(), + transform=Transform( + translation=Vector3(x=2.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) buffer.add(recent_transform) @@ -268,7 +276,7 @@ def test_buffer_pruning(self) -> None: assert len(buffer) == 1 first = buffer.first() assert first is not None - assert first.translation.x == 2.0 + assert first.transform.translation.x == 2.0 class TestMultiTBuffer: @@ -276,18 +284,22 @@ def test_multiple_frame_pairs(self) -> None: ttbuffer = MultiTBuffer(buffer_size=10.0) # Add transforms for different frame pairs - transform1 = Transform( - translation=Vector3(1.0, 0.0, 0.0), - frame_id="world", + transform1 = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(time.time())), child_frame_id="robot1", - ts=time.time(), + transform=Transform( + translation=Vector3(x=1.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) - transform2 = Transform( - translation=Vector3(2.0, 0.0, 0.0), - frame_id="world", + transform2 = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(time.time())), child_frame_id="robot2", - ts=time.time(), + transform=Transform( + translation=Vector3(x=2.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) ttbuffer.receive_transform(transform1, transform2) @@ -297,55 +309,71 @@ def test_multiple_frame_pairs(self) -> None: assert ("world", "robot1") in ttbuffer.buffers assert ("world", "robot2") in ttbuffer.buffers - def test_graph(self) -> None: + def test_graph(self, mocker) -> None: ttbuffer = MultiTBuffer(buffer_size=10.0) # Add transforms for different frame pairs - transform1 = Transform( - translation=Vector3(1.0, 0.0, 0.0), - frame_id="world", + transform1 = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(time.time())), child_frame_id="robot1", - ts=time.time(), + transform=Transform( + translation=Vector3(x=1.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) - transform2 = Transform( - translation=Vector3(2.0, 0.0, 0.0), - frame_id="world", + transform2 = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(time.time())), child_frame_id="robot2", - ts=time.time(), + transform=Transform( + translation=Vector3(x=2.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) ttbuffer.receive_transform(transform1, transform2) + render = mocker.patch("dimos.protocol.tf.tf.subprocess.run") + render.return_value.returncode = 0 + render.return_value.stdout = "rendered frame tree" + assert ttbuffer.graph() == "rendered frame tree" + assert render.call_args.kwargs["input"].splitlines() == [ + "world -> robot1", + "world -> robot2", + ] - print(ttbuffer.graph()) - - def test_forward_tolerance_returns_when_buffer_fills(self) -> None: + def test_forward_tolerance_returns_when_buffer_fills(self, wait_started) -> None: ttbuffer = MultiTBuffer() base_time = time.time() + waiting = wait_started(ttbuffer) + def publish_after_delay() -> None: - time.sleep(0.05) + waiting.wait(timeout=1.0) ttbuffer.receive_transform( - Transform( - translation=Vector3(1.0, 0.0, 0.0), - frame_id="world", + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), child_frame_id="robot", - ts=base_time, + transform=Transform( + translation=Vector3(x=1.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) ) publisher = threading.Thread(target=publish_after_delay) publisher.start() - - t0 = time.monotonic() - result = ttbuffer.get( - "world", "robot", time_point=base_time, time_tolerance=0.1, forward_tolerance=1.0 - ) - elapsed = time.monotonic() - t0 - publisher.join() + try: + t0 = time.monotonic() + result = ttbuffer.get( + "world", "robot", time_point=base_time, time_tolerance=0.1, forward_tolerance=1.0 + ) + elapsed = time.monotonic() - t0 + finally: + publisher.join(timeout=2.0) + assert not publisher.is_alive() assert result is not None - assert result.translation.x == 1.0 + assert result.transform.translation.x == 1.0 assert elapsed < 0.5 def test_forward_tolerance_times_out(self) -> None: @@ -360,11 +388,13 @@ def test_forward_tolerance_fast_path_when_already_available(self) -> None: ttbuffer = MultiTBuffer() base_time = time.time() ttbuffer.receive_transform( - Transform( - translation=Vector3(2.0, 0.0, 0.0), - frame_id="world", + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), child_frame_id="robot", - ts=base_time, + transform=Transform( + translation=Vector3(x=2.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) ) @@ -373,55 +403,65 @@ def test_forward_tolerance_fast_path_when_already_available(self) -> None: elapsed = time.monotonic() - t0 assert result is not None - assert result.translation.x == 2.0 + assert result.transform.translation.x == 2.0 assert elapsed < 0.05 - def test_forward_tolerance_wakes_on_chain_completion(self) -> None: + def test_forward_tolerance_wakes_on_chain_completion(self, wait_started) -> None: ttbuffer = MultiTBuffer() base_time = time.time() ttbuffer.receive_transform( - Transform( - translation=Vector3(1.0, 0.0, 0.0), - frame_id="world", + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), child_frame_id="robot", - ts=base_time, + transform=Transform( + translation=Vector3(x=1.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) ) + waiting = wait_started(ttbuffer) + def publish_after_delay() -> None: - time.sleep(0.05) + waiting.wait(timeout=1.0) ttbuffer.receive_transform( - Transform( - translation=Vector3(0.0, 2.0, 0.0), - frame_id="robot", + TransformStamped( + header=Header(frame_id="robot", stamp=time_from_seconds(base_time)), child_frame_id="sensor", - ts=base_time, + transform=Transform( + translation=Vector3(x=0.0, y=2.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) ) publisher = threading.Thread(target=publish_after_delay) publisher.start() - - result = ttbuffer.get( - "world", "sensor", time_point=base_time, time_tolerance=0.1, forward_tolerance=1.0 - ) - publisher.join() + try: + result = ttbuffer.get( + "world", "sensor", time_point=base_time, time_tolerance=0.1, forward_tolerance=1.0 + ) + finally: + publisher.join(timeout=2.0) + assert not publisher.is_alive() assert result is not None - assert result.translation.x == 1.0 - assert result.translation.y == 2.0 + assert result.transform.translation.x == 1.0 + assert result.transform.translation.y == 2.0 def test_get_transform_search_direct(self) -> None: ttbuffer = MultiTBuffer() base_time = time.time() # Add direct transform - transform = Transform( - translation=Vector3(1.0, 0.0, 0.0), - frame_id="world", + transform = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), child_frame_id="robot", - ts=base_time, + transform=Transform( + translation=Vector3(x=1.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) ttbuffer.receive_transform(transform) @@ -429,24 +469,28 @@ def test_get_transform_search_direct(self) -> None: result = ttbuffer.get_transform_search("world", "robot") assert result is not None assert len(result) == 1 - assert result[0].translation.x == 1.0 + assert result[0].transform.translation.x == 1.0 def test_get_transform_search_chain(self) -> None: ttbuffer = MultiTBuffer() base_time = time.time() # Create transform chain: world -> robot -> sensor - transform1 = Transform( - translation=Vector3(1.0, 0.0, 0.0), - frame_id="world", + transform1 = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), child_frame_id="robot", - ts=base_time, + transform=Transform( + translation=Vector3(x=1.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) - transform2 = Transform( - translation=Vector3(0.0, 2.0, 0.0), - frame_id="robot", + transform2 = TransformStamped( + header=Header(frame_id="robot", stamp=time_from_seconds(base_time)), child_frame_id="sensor", - ts=base_time, + transform=Transform( + translation=Vector3(x=0.0, y=2.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) ttbuffer.receive_transform(transform1, transform2) @@ -454,8 +498,8 @@ def test_get_transform_search_chain(self) -> None: result = ttbuffer.get_transform_search("world", "sensor") assert result is not None assert len(result) == 2 - assert result[0].translation.x == 1.0 # world -> robot - assert result[1].translation.y == 2.0 # robot -> sensor + assert result[0].transform.translation.x == 1.0 # world -> robot + assert result[1].transform.translation.y == 2.0 # robot -> sensor def test_get_transform_search_complex_chain(self) -> None: ttbuffer = MultiTBuffer() @@ -465,35 +509,45 @@ def test_get_transform_search_complex_chain(self) -> None: # world -> base -> arm -> hand # \-> robot -> sensor transforms = [ - Transform( - frame_id="world", + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), child_frame_id="base", - translation=Vector3(1.0, 0.0, 0.0), - ts=base_time, + transform=Transform( + translation=Vector3(x=1.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ), - Transform( - frame_id="base", + TransformStamped( + header=Header(frame_id="base", stamp=time_from_seconds(base_time)), child_frame_id="arm", - translation=Vector3(0.0, 1.0, 0.0), - ts=base_time, + transform=Transform( + translation=Vector3(x=0.0, y=1.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ), - Transform( - frame_id="arm", + TransformStamped( + header=Header(frame_id="arm", stamp=time_from_seconds(base_time)), child_frame_id="hand", - translation=Vector3(0.0, 0.0, 1.0), - ts=base_time, + transform=Transform( + translation=Vector3(x=0.0, y=0.0, z=1.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ), - Transform( - frame_id="world", + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), child_frame_id="robot", - translation=Vector3(2.0, 0.0, 0.0), - ts=base_time, + transform=Transform( + translation=Vector3(x=2.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ), - Transform( - frame_id="robot", + TransformStamped( + header=Header(frame_id="robot", stamp=time_from_seconds(base_time)), child_frame_id="sensor", - translation=Vector3(0.0, 2.0, 0.0), - ts=base_time, + transform=Transform( + translation=Vector3(x=0.0, y=2.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ), ] @@ -513,8 +567,22 @@ def test_get_transform_search_no_path(self) -> None: base_time = time.time() # Create disconnected transforms - transform1 = Transform(frame_id="world", child_frame_id="robot", ts=base_time) - transform2 = Transform(frame_id="base", child_frame_id="sensor", ts=base_time) + transform1 = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), + child_frame_id="robot", + transform=Transform( + translation=Vector3(x=0.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ) + transform2 = TransformStamped( + header=Header(frame_id="base", stamp=time_from_seconds(base_time)), + child_frame_id="sensor", + transform=Transform( + translation=Vector3(x=0.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ) ttbuffer.receive_transform(transform1, transform2) # No path exists @@ -526,24 +594,28 @@ def test_get_transform_search_with_time(self) -> None: base_time = time.time() # Add transforms at different times - old_transform = Transform( - frame_id="world", + old_transform = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time - 10.0)), child_frame_id="robot", - translation=Vector3(1.0, 0.0, 0.0), - ts=base_time - 10.0, + transform=Transform( + translation=Vector3(x=1.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) - new_transform = Transform( - frame_id="world", + new_transform = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), child_frame_id="robot", - translation=Vector3(2.0, 0.0, 0.0), - ts=base_time, + transform=Transform( + translation=Vector3(x=2.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) ttbuffer.receive_transform(old_transform, new_transform) # Search at specific time result = ttbuffer.get_transform_search("world", "robot", time_point=base_time) assert result is not None - assert result[0].translation.x == 2.0 + assert result[0].transform.translation.x == 2.0 # Search with time tolerance result = ttbuffer.get_transform_search( @@ -559,10 +631,38 @@ def test_get_transform_search_shortest_path(self) -> None: # world -> A -> B -> target (3 hops) # world -> target (direct, 1 hop) transforms = [ - Transform(frame_id="world", child_frame_id="A", ts=base_time), - Transform(frame_id="A", child_frame_id="B", ts=base_time), - Transform(frame_id="B", child_frame_id="target", ts=base_time), - Transform(frame_id="world", child_frame_id="target", ts=base_time), + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), + child_frame_id="A", + transform=Transform( + translation=Vector3(x=0.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ), + TransformStamped( + header=Header(frame_id="A", stamp=time_from_seconds(base_time)), + child_frame_id="B", + transform=Transform( + translation=Vector3(x=0.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ), + TransformStamped( + header=Header(frame_id="B", stamp=time_from_seconds(base_time)), + child_frame_id="target", + transform=Transform( + translation=Vector3(x=0.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ), + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), + child_frame_id="target", + transform=Transform( + translation=Vector3(x=0.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ), ] for t in transforms: @@ -586,11 +686,13 @@ def test_string_representations(self) -> None: buffer = TBuffer() base_time = time.time() for i in range(3): - transform = Transform( - translation=Vector3(float(i), 0.0, 0.0), - frame_id="world", + transform = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time + i * 0.1)), child_frame_id="robot", - ts=base_time + i * 0.1, + transform=Transform( + translation=Vector3(x=float(i), y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) buffer.add(transform) @@ -602,17 +704,36 @@ def test_string_representations(self) -> None: # Test MultiTBuffer with multiple frame pairs ttbuffer = MultiTBuffer() transforms = [ - Transform(frame_id="world", child_frame_id="robot1", ts=base_time), - Transform(frame_id="world", child_frame_id="robot2", ts=base_time + 0.5), - Transform(frame_id="robot1", child_frame_id="sensor", ts=base_time + 1.0), + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), + child_frame_id="robot1", + transform=Transform( + translation=Vector3(x=0.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ), + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time + 0.5)), + child_frame_id="robot2", + transform=Transform( + translation=Vector3(x=0.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ), + TransformStamped( + header=Header(frame_id="robot1", stamp=time_from_seconds(base_time + 1.0)), + child_frame_id="sensor", + transform=Transform( + translation=Vector3(x=0.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ), ] for t in transforms: ttbuffer.receive_transform(t) ttbuffer_str = str(ttbuffer) - print("\nMultiTBuffer string representation:") - print(ttbuffer_str) assert "MultiTBuffer(3 buffers):" in ttbuffer_str assert "TBuffer(world -> robot1, 1 msgs" in ttbuffer_str @@ -626,12 +747,13 @@ def test_string_representations(self) -> None: T0 = 1_700_000_000.0 -def _t(parent: str, child: str, x: float, ts: float) -> Transform: - return Transform( - frame_id=parent, +def _t(parent: str, child: str, x: float, ts: float) -> TransformStamped: + return TransformStamped( + header=Header(frame_id=parent, stamp=time_from_seconds(ts)), child_frame_id=child, - translation=Vector3(x, 0.0, 0.0), - ts=ts, + transform=Transform( + translation=Vector3(x=x, y=0.0, z=0.0), rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ), ) @@ -649,7 +771,7 @@ def make_tf(request, tmp_path): # type: ignore[no-untyped-def] """Builder fixture: feed it transforms, get back a TFLookup over them.""" stores = [] - def build(*transforms: Transform) -> TFLookup: + def build(*transforms: TransformStamped) -> TFLookup: if request.param == "live": buf = MultiTBuffer() buf.receive_transform(*transforms) @@ -662,7 +784,7 @@ def build(*transforms: Transform) -> TFLookup: stores.append(store) stream = store.stream("tf", TFMessage) for t in transforms: - stream.append(TFMessage(t), ts=t.ts, pose=None) + stream.append(TFMessage(transforms=[t]), ts=to_seconds(t.header.stamp), pose=None) return StreamTF(store.stream("tf", TFMessage)) yield build @@ -677,20 +799,20 @@ def test_latest(self, make_tf) -> None: # type: ignore[no-untyped-def] tf = make_tf(*(_t("world", "robot", float(i), T0 + i * 0.1) for i in range(3))) got = tf.get("world", "robot") assert got is not None - assert got.translation.x == 2.0 + assert got.transform.translation.x == 2.0 def test_nearest_in_time(self, make_tf) -> None: # type: ignore[no-untyped-def] tf = make_tf(*(_t("world", "robot", float(i), T0 + i * 0.5) for i in range(5))) got = tf.get("world", "robot", time_point=T0 + 1.25) assert got is not None # Equidistant between i=2 (t=1.0) and i=3 (t=1.5) — the later one wins. - assert got.translation.x == 3.0 + assert got.transform.translation.x == 3.0 def test_inverse(self, make_tf) -> None: # type: ignore[no-untyped-def] tf = make_tf(_t("world", "robot", 5.0, T0)) got = tf.get("robot", "world", time_point=T0) assert got is not None - assert got.translation.x == pytest.approx(-5.0) + assert got.transform.translation.x == pytest.approx(-5.0) def test_time_tolerance(self, make_tf) -> None: # type: ignore[no-untyped-def] tf = make_tf(_t("world", "robot", 1.0, T0)) @@ -701,10 +823,10 @@ def test_same_frame_identity(self, make_tf) -> None: # type: ignore[no-untyped- tf = make_tf(_t("world", "robot", 1.0, T0)) got = tf.get("world", "world", time_point=T0) assert got is not None - assert got.frame_id == "world" + assert got.header.frame_id == "world" assert got.child_frame_id == "world" - assert got.translation.x == 0.0 - assert got.rotation.w == 1.0 + assert got.transform.translation.x == 0.0 + assert got.transform.rotation.w == 1.0 def test_unknown_frame(self, make_tf) -> None: # type: ignore[no-untyped-def] tf = make_tf(_t("world", "robot", 1.0, T0)) @@ -714,29 +836,33 @@ def test_chain_composition(self, make_tf) -> None: # type: ignore[no-untyped-de # world -> robot: translate (1, 0, 0); robot -> sensor: translate # (0, 2, 0) and rotate 90° around Z. tf = make_tf( - Transform( - translation=Vector3(1.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="world", + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(T0)), child_frame_id="robot", - ts=T0, + transform=Transform( + translation=Vector3(x=1.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ), - Transform( - translation=Vector3(0.0, 2.0, 0.0), - rotation=Quaternion(0.0, 0.0, math.sin(math.pi / 4), math.cos(math.pi / 4)), - frame_id="robot", + TransformStamped( + header=Header(frame_id="robot", stamp=time_from_seconds(T0)), child_frame_id="sensor", - ts=T0, + transform=Transform( + translation=Vector3(x=0.0, y=2.0, z=0.0), + rotation=Quaternion( + x=0.0, y=0.0, z=math.sin(math.pi / 4), w=math.cos(math.pi / 4) + ), + ), ), ) result = tf.get("world", "sensor", time_point=T0) assert result is not None - assert result.translation.x == pytest.approx(1.0) - assert result.translation.y == pytest.approx(2.0) - assert result.translation.z == pytest.approx(0.0) - assert result.rotation.z == pytest.approx(math.sin(math.pi / 4)) - assert result.rotation.w == pytest.approx(math.cos(math.pi / 4)) - assert result.frame_id == "world" + assert result.transform.translation.x == pytest.approx(1.0) + assert result.transform.translation.y == pytest.approx(2.0) + assert result.transform.translation.z == pytest.approx(0.0) + assert result.transform.rotation.z == pytest.approx(math.sin(math.pi / 4)) + assert result.transform.rotation.w == pytest.approx(math.cos(math.pi / 4)) + assert result.header.frame_id == "world" assert result.child_frame_id == "sensor" def test_chain_with_sparse_static_edge(self, make_tf) -> None: # type: ignore[no-untyped-def] @@ -748,7 +874,7 @@ def test_chain_with_sparse_static_edge(self, make_tf) -> None: # type: ignore[n ) got = tf.get("world", "base", time_point=T0 + 9.0) assert got is not None - assert got.translation.x == pytest.approx(109.0) + assert got.transform.translation.x == pytest.approx(109.0) def test_conforms_to_tf_lookup(self, make_tf) -> None: # type: ignore[no-untyped-def] assert isinstance(make_tf(_t("world", "robot", 1.0, T0)), TFLookup) @@ -763,10 +889,10 @@ def store(self): # type: ignore[no-untyped-def] stream = store.stream("tf", TFMessage) # Startup burst: a static stamped exactly at the stream start (regression: # strict `ts >` range queries used to drop it), then dynamic map→base. - stream.append(TFMessage(_t("world", "map", 100.0, T0)), ts=T0, pose=None) + stream.append(TFMessage(transforms=[_t("world", "map", 100.0, T0)]), ts=T0, pose=None) for i in range(100): ts = T0 + i / 10 - stream.append(TFMessage(_t("map", "base", i / 10, ts)), ts=ts, pose=None) + stream.append(TFMessage(transforms=[_t("map", "base", i / 10, ts)]), ts=ts, pose=None) yield store store.stop() @@ -803,7 +929,7 @@ def test_get_pose(self, store) -> None: # type: ignore[no-untyped-def] tf = StreamTF(store.stream("tf", TFMessage)) pose = tf.get_pose("map", "base", time_point=T0 + 5.0) assert pose is not None - assert pose.position.x == pytest.approx(5.0) + assert pose.pose.position.x == pytest.approx(5.0) def test_cache_prefetch_and_eviction(self) -> None: # 40 s of data with a small cache_span: a miss caches the query window @@ -813,23 +939,23 @@ def test_cache_prefetch_and_eviction(self) -> None: stream = store.stream("tf", TFMessage) for i in range(400): ts = T0 + i / 10 - stream.append(TFMessage(_t("map", "base", i / 10, ts)), ts=ts, pose=None) + stream.append(TFMessage(transforms=[_t("map", "base", i / 10, ts)]), ts=ts, pose=None) try: tf = StreamTF(store.stream("tf", TFMessage), cache_span=2.0) got = tf.get("map", "base", time_point=T0 + 35.0, time_tolerance=0.5) assert got is not None - assert got.translation.x == pytest.approx(35.0) + assert got.transform.translation.x == pytest.approx(35.0) covered = tf._covered assert covered == pytest.approx((T0 + 34.5, T0 + 37.5)) # Inside the prefetched span: served from cache, no re-query. got = tf.get("map", "base", time_point=T0 + 37.0, time_tolerance=0.5) assert got is not None - assert got.translation.x == pytest.approx(37.0) + assert got.transform.translation.x == pytest.approx(37.0) assert tf._covered == covered # Past the span: evict and re-cache around the new query. got = tf.get("map", "base", time_point=T0 + 39.0, time_tolerance=0.5) assert got is not None - assert got.translation.x == pytest.approx(39.0) + assert got.transform.translation.x == pytest.approx(39.0) assert tf._covered == pytest.approx((T0 + 38.5, T0 + 41.5)) # Bounded: the buffer holds the re-cached span, not the stream. assert len(tf.buffers[("map", "base")]) < 100 diff --git a/dimos/protocol/tf/tf.py b/dimos/protocol/tf/tf.py index 03a4706deda..99d54e658c8 100644 --- a/dimos/protocol/tf/tf.py +++ b/dimos/protocol/tf/tf.py @@ -16,16 +16,25 @@ from collections import deque from functools import reduce +import subprocess import threading import time from typing import TYPE_CHECKING, Any, Protocol, runtime_checkable -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.message_utils.geometry import compose_transforms, inverse_transform, pose_from_transform +from dimos.message_utils.time import time_from_seconds +from dimos.messages.geometry_msgs.msg import ( + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.protocol.tf.buffer import TBuffer from dimos.types.timestamped import to_human_readable from dimos.utils.logging_config import setup_logger -from dimos.utils.timeseries.inmemory import InMemoryStore if TYPE_CHECKING: from collections.abc import Callable @@ -48,45 +57,7 @@ def get( child_frame: str, time_point: float | None = None, time_tolerance: float | None = None, - ) -> Transform | None: ... - - -class TBuffer(InMemoryStore[Transform]): - def __init__(self, buffer_size: float = 10.0) -> None: - super().__init__() - self.buffer_size = buffer_size - - def add(self, transform: Transform) -> None: - self.save(transform) - self.prune_old(transform.ts - self.buffer_size) - - def get(self, time_point: float | None = None, time_tolerance: float = 1.0) -> Transform | None: - """Get transform at specified time or latest if no time given.""" - if time_point is None: - return self.last() - return self.find_closest(time_point, time_tolerance) - - def __str__(self) -> str: - if len(self) == 0: - return "TBuffer(empty)" - - first_item = self.first() - time_range = self.time_range() - if time_range and first_item: - start_time = to_human_readable(time_range[0]) - end_time = to_human_readable(time_range[1]) - duration = time_range[1] - time_range[0] - - frame_str = f"{first_item.frame_id} -> {first_item.child_frame_id}" - - return ( - f"TBuffer(" - f"{frame_str}, " - f"{len(self)} msgs, " - f"{duration:.2f}s [{start_time} - {end_time}])" - ) - - return f"TBuffer({len(self)} msgs)" + ) -> TransformStamped | None: ... # stores multiple transform buffers @@ -97,10 +68,10 @@ def __init__(self, buffer_size: float = 10.0) -> None: self.buffer_size = buffer_size self._cv = threading.Condition() - def receive_transform(self, *args: Transform) -> None: + def receive_transform(self, *args: TransformStamped) -> None: with self._cv: for transform in args: - key = (transform.frame_id, transform.child_frame_id) + key = (transform.header.frame_id, transform.child_frame_id) if key not in self.buffers: self.buffers[key] = TBuffer(self.buffer_size) self.buffers[key].add(transform) @@ -134,12 +105,18 @@ def get_transform( child_frame: str, time_point: float | None = None, time_tolerance: float | None = None, - ) -> Transform | None: + ) -> TransformStamped | None: if parent_frame == child_frame: - return Transform( - frame_id=parent_frame, + return TransformStamped( + header=Header( + frame_id=parent_frame, + stamp=time_from_seconds(time_point if time_point is not None else time.time()), + ), child_frame_id=child_frame, - ts=time_point if time_point is not None else time.time(), + transform=Transform( + translation=Vector3(x=0.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) # No explicit tolerance means "anything still buffered" — the buffer @@ -156,7 +133,7 @@ def get_transform( reverse_key = (child_frame, parent_frame) if reverse_key in self.buffers: transform = self.buffers[reverse_key].get(time_point, tolerance) - return transform.inverse() if transform else None + return inverse_transform(transform) if transform else None return None @@ -166,7 +143,7 @@ def _get( child_frame: str, time_point: float | None = None, time_tolerance: float | None = None, - ) -> Transform | None: + ) -> TransformStamped | None: with self._cv: simple = self.get_transform(parent_frame, child_frame, time_point, time_tolerance) @@ -180,7 +157,7 @@ def _get( if complex is None: return None - return reduce(lambda t1, t2: t1 + t2, complex) + return reduce(compose_transforms, complex) def _wait_get( self, @@ -189,7 +166,7 @@ def _wait_get( time_point: float | None, time_tolerance: float | None, forward_tolerance: float, - ) -> Transform | None: + ) -> TransformStamped | None: deadline = time.monotonic() + forward_tolerance with self._cv: while True: @@ -210,7 +187,7 @@ def get( *, forward_tolerance: float = 0.0, warn: bool = True, - ) -> Transform | None: + ) -> TransformStamped | None: result = self._get(parent_frame, child_frame, time_point, time_tolerance) if result is None and forward_tolerance > 0: result = self._wait_get( @@ -240,7 +217,7 @@ def get_pose( ) if not tf: return None - return tf.to_pose() + return pose_from_transform(tf) def get_transform_search( self, @@ -248,7 +225,7 @@ def get_transform_search( child_frame: str, time_point: float | None = None, time_tolerance: float | None = None, - ) -> list[Transform] | None: + ) -> list[TransformStamped] | None: """Search for shortest transform chain between parent and child frames using BFS.""" with self._cv: # Check if direct transform exists (already checked in get_transform, but for clarity) @@ -257,7 +234,7 @@ def get_transform_search( return [direct] # BFS to find shortest path - queue: deque[tuple[str, list[Transform]]] = deque([(parent_frame, [])]) + queue: deque[tuple[str, list[TransformStamped]]] = deque([(parent_frame, [])]) visited = {parent_frame} while queue: @@ -283,8 +260,6 @@ def get_transform_search( return None def graph(self) -> str: - import subprocess - def connection_str(connection: tuple[str, str]) -> str: (frame_from, frame_to) = connection return f"{frame_from} -> {frame_to}" @@ -335,10 +310,10 @@ def __init__(self, stream: Any | None = None, buffer_size: float = 10.0) -> None if stream is not None: self._unsubscribe = stream.subscribe(self.receive_tfmessage) - def publish(self, *transforms: Transform) -> None: + def publish(self, *transforms: TransformStamped) -> None: self.receive_transform(*transforms) if self._stream is not None: - self._stream.publish(TFMessage(*transforms)) + self._stream.publish(TFMessage(transforms=list(transforms))) def dispose(self) -> None: if self._unsubscribe is not None: diff --git a/dimos/robot/unitree/go2/dds/rust/Cargo.lock b/dimos/robot/unitree/go2/dds/rust/Cargo.lock index 167370fc481..df06852f5a4 100644 --- a/dimos/robot/unitree/go2/dds/rust/Cargo.lock +++ b/dimos/robot/unitree/go2/dds/rust/Cargo.lock @@ -679,7 +679,7 @@ dependencies = [ [[package]] name = "dimos-lcm" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", "socket2 0.5.10", @@ -689,11 +689,11 @@ dependencies = [ [[package]] name = "dimos-module" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "dimos-lcm", "dimos-module-macros", - "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen)", + "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549)", "nalgebra", "rayon", "serde", @@ -709,7 +709,7 @@ dependencies = [ [[package]] name = "dimos-module-macros" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "proc-macro2", "quote", @@ -1516,7 +1516,7 @@ dependencies = [ [[package]] name = "lcm-msgs" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", ] diff --git a/dimos/robot/unitree/go2/dds/rust/Cargo.toml b/dimos/robot/unitree/go2/dds/rust/Cargo.toml index 96957ff404a..0218cb42f30 100644 --- a/dimos/robot/unitree/go2/dds/rust/Cargo.toml +++ b/dimos/robot/unitree/go2/dds/rust/Cargo.toml @@ -16,7 +16,7 @@ path = "src/bin/go2_dds.rs" [dependencies] # Our forks, vendored: the robot has no GitHub access. See vendor/*/Cargo.toml. cyclonedds-rs = { path = "vendor/cyclonedds-rs", default-features = false } -dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6", version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", tag = "v0.1.6" } serde = { version = "1", features = ["derive"] } serde-big-array = "0.5" diff --git a/dimos/robot/unitree/go2/dds/rust/crate-hashes.json b/dimos/robot/unitree/go2/dds/rust/crate-hashes.json index df121a4fbe9..b01d1af11bf 100644 --- a/dimos/robot/unitree/go2/dds/rust/crate-hashes.json +++ b/dimos/robot/unitree/go2/dds/rust/crate-hashes.json @@ -1,7 +1,7 @@ { - "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", - "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", "git+https://github.com/dimensionalOS/dimos-lcm.git?tag=v0.1.6#lcm-msgs@0.1.0": "sha256-n2/OkEsmTmjPzYv5O7kOBzJRZmU/pPozSvLDdHPiCKg=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module-macros@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=" + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module-macros@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=" } \ No newline at end of file diff --git a/dimos/spec/perception.py b/dimos/spec/perception.py index 4fac65ad020..29c715710be 100644 --- a/dimos/spec/perception.py +++ b/dimos/spec/perception.py @@ -15,9 +15,8 @@ from typing import Protocol from dimos.core.stream import Out +from dimos.messages.sensor_msgs.msg import CameraInfo, Image as ImageMsg from dimos.msgs.nav_msgs.Odometry import Odometry as OdometryMsg -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image as ImageMsg from dimos.msgs.sensor_msgs.Imu import Imu from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 diff --git a/dimos/teleop/hosted/blueprints/cloudflare.py b/dimos/teleop/hosted/blueprints/cloudflare.py index cadd369279d..27faf4b2f2b 100644 --- a/dimos/teleop/hosted/blueprints/cloudflare.py +++ b/dimos/teleop/hosted/blueprints/cloudflare.py @@ -36,9 +36,9 @@ from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera from dimos.mapping.costmapper import CostMapper from dimos.mapping.voxels.module import VoxelGridMapper +from dimos.messages.sensor_msgs.msg import Image from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped -from dimos.msgs.sensor_msgs.Image import Image from dimos.navigation.go2.replanning_a_star.module import ReplanningAStarPlanner from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.robot.manipulators.xarm.blueprints.teleop import ( diff --git a/dimos/teleop/hosted/camera_mux.py b/dimos/teleop/hosted/camera_mux.py index d6eeb7ae3c7..aa5fbabb82f 100644 --- a/dimos/teleop/hosted/camera_mux.py +++ b/dimos/teleop/hosted/camera_mux.py @@ -23,18 +23,22 @@ from __future__ import annotations from collections.abc import Callable +from copy import deepcopy import json import threading import time from typing import Any +import cv2 import numpy as np from reactivex.disposable import Disposable from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.sensor_msgs.Image import Image +from dimos.message_utils.image import image_from_array, image_to_bgr, image_to_rgb, image_view +from dimos.message_utils.time import header_now +from dimos.messages.sensor_msgs.msg import Image from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -136,21 +140,29 @@ def _composite(self) -> Image | None: try: if len(imgs) == 1: return self._even_dims(self._stamp(self._downscale(imgs[0]))) - import cv2 - - target_h = min(im.data.shape[0] for im in imgs) + target_h = min(im.height for im in imgs) tiles = [] for im in imgs: - h, w = im.data.shape[:2] + h, w = im.height, im.width + data = image_view(im) + if im.encoding != imgs[0].encoding and {im.encoding, imgs[0].encoding} <= { + "rgb8", + "bgr8", + }: + data = image_to_bgr(im) if imgs[0].encoding == "bgr8" else image_to_rgb(im) tiles.append( - cv2.resize(im.data, (max(1, int(w * target_h / h)), target_h)) + cv2.resize(data, (max(1, int(w * target_h / h)), target_h)) if h != target_h - else im.data + else data ) return self._even_dims( self._stamp( self._downscale( - Image(data=np.hstack(tiles), format=imgs[0].format, frame_id="camera_mux") + image_from_array( + np.hstack(tiles), + encoding=imgs[0].encoding, + header=header_now("camera_mux"), + ) ) ) ) @@ -162,28 +174,28 @@ def _composite(self) -> Image | None: def _even_dims(img: Image) -> Image: """Crop to even width/height — an odd composite crashes libx264's avcodec_open2 when a camera switch reopens the encoder.""" - data = img.data + data = image_view(img) if data.ndim < 2: return img h, w = data.shape[:2] if h % 2 == 0 and w % 2 == 0: return img data = data[: h - (h % 2), : w - (w % 2)] - return Image(data=np.ascontiguousarray(data), format=img.format, frame_id=img.frame_id) + return image_from_array( + np.ascontiguousarray(data), encoding=img.encoding, header=deepcopy(img.header) + ) def _downscale(self, img: Image) -> Image: """Cap publish width at config.video_max_width (0 = off). Runs before _stamp so the strip's 16px cells stay decodable at the sent size.""" max_w = self.config.video_max_width - if max_w <= 0 or img.data.ndim < 2: + if max_w <= 0: return img - h, w = img.data.shape[:2] + h, w = img.height, img.width if w <= max_w: return img - import cv2 - - out = cv2.resize(img.data, (max_w, max(1, int(h * max_w / w)))) - return Image(data=out, format=img.format, frame_id=img.frame_id) + out = cv2.resize(image_view(img), (max_w, max(1, int(h * max_w / w)))) + return image_from_array(out, encoding=img.encoding, header=deepcopy(img.header)) def _stamp(self, img: Image) -> Image: """Append (not overwrite) a bottom strip encoding capture time as B/W @@ -197,7 +209,7 @@ def _stamp(self, img: Image) -> Image: ] s = _STAMP_CELL_PX - data = img.data + data = image_view(img) if data.ndim < 2 or data.shape[1] < _STAMP_CELLS * s: if not self._stamp_warned: self._stamp_warned = True @@ -215,7 +227,7 @@ def _stamp(self, img: Image) -> Image: if bit: strip[:, i * s : (i + 1) * s] = 255 out = np.vstack([data, strip]) - return Image(data=out, format=img.format, frame_id=img.frame_id) + return image_from_array(out, encoding=img.encoding, header=deepcopy(img.header)) def _set_cam_selection(self, data: bytes) -> None: """camera_select kind → filter to known cams, republish immediately so diff --git a/dimos/teleop/hosted/test_camera_mux.py b/dimos/teleop/hosted/test_camera_mux.py index c7a82fcfd67..e0a65c9428d 100644 --- a/dimos/teleop/hosted/test_camera_mux.py +++ b/dimos/teleop/hosted/test_camera_mux.py @@ -22,49 +22,49 @@ from __future__ import annotations import json -from types import SimpleNamespace import numpy as np import pytest -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.message_utils.image import image_from_array, image_to_rgb, image_view +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.std_msgs.msg import Header +from dimos.teleop.hosted.blueprints.cloudflare import ( + teleop_hosted_go2_multicam, + teleop_hosted_go2_transport, + teleop_hosted_xarm6, + teleop_hosted_xarm7, +) from dimos.teleop.hosted.camera_mux import CameraMuxModule -class _Mux(CameraMuxModule): - """Bare host: just the config the module reads + a captured publish. - - Bypasses ``Module.__init__`` (no ports/RPC plumbing) — build the mux state - directly via ``_mux_init`` like the module does after ``super().__init__``. - """ +@pytest.fixture +def make_mux(mocker): + instances = [] - def __init__(self, cameras: list[str], **cfg: object) -> None: - self.config = SimpleNamespace( - video_max_width=cfg.get("video_max_width", 0), - video_max_fps=cfg.get("video_max_fps", 0.0), - latency_stamp=cfg.get("latency_stamp", False), - ) - self.published: list[Image] = [] - self.mux_image = SimpleNamespace(publish=self.published.append) - self._mux_init(cameras) + def make(cameras, **config): + module = CameraMuxModule(cameras=cameras, **config) + mocker.patch.object(module.mux_image, "publish") + instances.append(module) + return module - -def _make(cameras: list[str], **cfg: object) -> _Mux: - """Construct without triggering CameraMuxModule.__init__ / Module.__init__.""" - return _Mux(cameras, **cfg) + yield make + for module in instances: + module.stop() def _img(w: int, h: int) -> Image: - return Image(data=np.zeros((h, w, 3), dtype=np.uint8), format=ImageFormat.BGR) + return image_from_array(np.zeros((h, w, 3), dtype=np.uint8), encoding="bgr8") -def _feed(mux: _Mux, cam: str, img: Image) -> None: +def _feed(mux: CameraMuxModule, cam: str, img: Image) -> None: with mux._cam_lock: mux._cam_frames[cam] = img def _is_even(img: Image) -> bool: - h, w = img.data.shape[:2] + h, w = image_view(img).shape[:2] return h % 2 == 0 and w % 2 == 0 @@ -73,8 +73,8 @@ def _is_even(img: Image) -> bool: def test_even_dims_crops_odd_width_and_height() -> None: out = CameraMuxModule._even_dims(_img(641, 481)) - assert out.data.shape[:2] == (480, 640) - assert out.data.flags["C_CONTIGUOUS"] # from_ndarray needs contiguous + assert image_view(out).shape[:2] == (480, 640) + assert image_view(out).flags["C_CONTIGUOUS"] # from_ndarray needs contiguous def test_even_dims_passes_even_through_unchanged() -> None: @@ -85,19 +85,23 @@ def test_even_dims_passes_even_through_unchanged() -> None: # ─── _composite always even (the actual crash path) ─────────────────── -def test_single_camera_downscale_to_odd_is_evened() -> None: +def test_single_camera_downscale_to_odd_is_evened( + make_mux, +) -> None: # 1280→641 cap yields an odd width, and 641*720/1280 = 360 (even h) — # width alone would crash the encoder; the guard fixes it. - mux = _make(["cam1"], video_max_width=641) + mux = make_mux(["cam1"], video_max_width=641) _feed(mux, "cam1", _img(1280, 720)) out = mux._composite() assert out is not None and _is_even(out) -def test_hstack_odd_tile_width_is_evened() -> None: +def test_hstack_odd_tile_width_is_evened( + make_mux, +) -> None: # Two cams of different aspect → per-tile int() scaling can sum to an odd # composite width. Guard must even it regardless of the arithmetic. - mux = _make(["cam1", "cam2"]) + mux = make_mux(["cam1", "cam2"]) with mux._cam_lock: mux._cam_selected = ["cam1", "cam2"] _feed(mux, "cam1", _img(853, 480)) # 16:9-ish, odd width @@ -106,12 +110,14 @@ def test_hstack_odd_tile_width_is_evened() -> None: assert out is not None and _is_even(out) -def test_hstack_degenerate_tile_width_does_not_crash() -> None: +def test_hstack_degenerate_tile_width_does_not_crash( + make_mux, +) -> None: # A very short reference tile (sets target_h) beside a tall-narrow one makes # int(w * target_h / h) round to 0; cv2.resize raises on a 0 dimension. # target_h = min(4, 1000) = 4; cam2 tile width = int(100*4/1000) = 0 → must # be clamped to 1, not crash. - mux = _make(["cam1", "cam2"]) + mux = make_mux(["cam1", "cam2"]) with mux._cam_lock: mux._cam_selected = ["cam1", "cam2"] _feed(mux, "cam1", _img(200, 4)) # short → target_h = 4 @@ -120,21 +126,25 @@ def test_hstack_degenerate_tile_width_does_not_crash() -> None: assert out is not None and _is_even(out) -def test_composite_returns_none_on_error_does_not_raise() -> None: +def test_composite_returns_none_on_error_does_not_raise( + make_mux, +) -> None: # Mismatched channel counts make np.hstack raise; _composite must return # None (drop the frame), not kill its RxPY subscription. - mux = _make(["cam1", "cam2"]) + mux = make_mux(["cam1", "cam2"]) with mux._cam_lock: mux._cam_selected = ["cam1", "cam2"] - _feed(mux, "cam1", Image(data=np.zeros((480, 640, 3), np.uint8), format=ImageFormat.BGR)) - _feed(mux, "cam2", Image(data=np.zeros((480, 640, 4), np.uint8), format=ImageFormat.BGRA)) + _feed(mux, "cam1", image_from_array(np.zeros((480, 640, 3), np.uint8), encoding="bgr8")) + _feed(mux, "cam2", image_from_array(np.zeros((480, 640, 4), np.uint8), encoding="bgra8")) assert mux._composite() is None -def test_switch_between_selections_stays_even() -> None: +def test_switch_between_selections_stays_even( + make_mux, +) -> None: # Reproduces the report: flipping selection changes frame size (encoder # reopen). Every produced frame must be even so libx264 never fails. - mux = _make(["cam1", "cam2"], video_max_width=641, latency_stamp=True) + mux = make_mux(["cam1", "cam2"], video_max_width=641, latency_stamp=True) _feed(mux, "cam1", _img(1280, 721)) _feed(mux, "cam2", _img(647, 483)) for selection in (["cam1", "cam2"], ["cam1"], ["cam2"], ["cam1", "cam2"]): @@ -145,18 +155,20 @@ def test_switch_between_selections_stays_even() -> None: @pytest.mark.parametrize("cams", [None, "cam1", 5, {"cam1": 1}]) -def test_set_cam_selection_ignores_non_list(cams: object) -> None: +def test_set_cam_selection_ignores_non_list(make_mux, cams: object) -> None: # Untrusted wire payload (e.g. cams:null) must not raise into the state # handler; a bad value falls back to the first camera. - mux = _make(["cam1", "cam2"]) + mux = make_mux(["cam1", "cam2"]) payload = json.dumps({"type": "camera_select", "cams": cams}).encode() mux._set_cam_selection(payload) assert mux._cam_selected == ["cam1"] -def test_set_cam_selection_switches_camera() -> None: +def test_set_cam_selection_switches_camera( + make_mux, +) -> None: # The happy path: a well-formed camera_select flips the active camera. - mux = _make(["cam1", "cam2"]) + mux = make_mux(["cam1", "cam2"]) mux._set_cam_selection(json.dumps({"type": "camera_select", "cams": ["cam2"]}).encode()) assert mux._cam_selected == ["cam2"] @@ -170,38 +182,89 @@ def test_set_cam_selection_switches_camera() -> None: b'{"malformed', # truncated JSON ], ) -def test_set_cam_selection_ignores_foreign_frames(payload: bytes) -> None: +def test_set_cam_selection_ignores_foreign_frames(make_mux, payload: bytes) -> None: # camera_select shares the state_reliable plane with estop/sport/etc.; a # frame that isn't ours must leave the selection untouched (no exception, # no spurious switch, no log spam). - mux = _make(["cam1", "cam2"]) + mux = make_mux(["cam1", "cam2"]) with mux._cam_lock: mux._cam_selected = ["cam2"] mux._set_cam_selection(payload) assert mux._cam_selected == ["cam2"] # unchanged -def test_latency_stamp_fits_640_width() -> None: - mux = _make(["cam1"], latency_stamp=True) +def test_latency_stamp_fits_640_width( + make_mux, +) -> None: + mux = make_mux(["cam1"], latency_stamp=True) _feed(mux, "cam1", _img(640, 480)) out = mux._composite() - assert out is not None and out.data.shape[:2] == (480 + 16, 640) + assert out is not None and image_view(out).shape[:2] == (480 + 16, 640) -def test_fps_cap_drops_frames_within_window() -> None: - mux = _make(["cam1"], video_max_fps=30.0) +def test_fps_cap_drops_frames_within_window(make_mux, mocker) -> None: + mocker.patch("dimos.teleop.hosted.camera_mux.time.monotonic", return_value=100.0) + mux = make_mux(["cam1"], video_max_fps=30.0) mux._on_cam("cam1", _img(640, 480)) mux._on_cam("cam1", _img(640, 480)) # immediately again — inside 1/30 s - assert len(mux.published) == 1 + assert mux.mux_image.publish.call_count == 1 -def test_fps_cap_window_released_on_failed_composite() -> None: - mux = _make(["cam1", "cam2"], video_max_fps=30.0) +def test_fps_cap_window_released_on_failed_composite( + make_mux, +) -> None: + mux = make_mux(["cam1", "cam2"], video_max_fps=30.0) with mux._cam_lock: mux._cam_selected = ["cam1", "cam2"] - _feed(mux, "cam2", Image(data=np.zeros((480, 640, 4), np.uint8), format=ImageFormat.BGRA)) + _feed(mux, "cam2", image_from_array(np.zeros((480, 640, 4), np.uint8), encoding="bgra8")) mux._on_cam("cam1", _img(640, 480)) # BGR + BGRA hstack fails → None - assert mux.published == [] + mux.mux_image.publish.assert_not_called() _feed(mux, "cam2", _img(640, 480)) mux._on_cam("cam1", _img(640, 480)) # still inside the window — must publish - assert len(mux.published) == 1 + assert mux.mux_image.publish.call_count == 1 + + +def test_generated_header_survives_crop_resize_and_latency_strip(make_mux, mocker): + mocker.patch("dimos.teleop.hosted.camera_mux.time.time", return_value=17.125) + source = image_from_array( + np.full((481, 853, 3), [255, 0, 0], np.uint8), + encoding="rgb8", + header=Header(Time(1_789_654_321, 1), "camera"), + ) + original = source.data.copy() + mux = make_mux(["cam1"], video_max_width=641, latency_stamp=True) + _feed(mux, "cam1", source) + out = mux._composite() + assert out is not None + assert out.header == source.header + assert out.header is not source.header + assert out.width == 640 + assert out.height % 2 == 0 + assert image_to_rgb(out)[0, 0].tolist() == [255, 0, 0] + np.testing.assert_array_equal(source.data, original) + out.header.frame_id = "changed" + assert source.header.frame_id == "camera" + + +def test_mixed_rgb_bgr_tiles_keep_the_same_visible_color(make_mux): + mux = make_mux(["cam1", "cam2"]) + with mux._cam_lock: + mux._cam_selected = ["cam1", "cam2"] + _feed(mux, "cam1", image_from_array(np.full((4, 4, 3), [0, 0, 255], np.uint8), encoding="bgr8")) + _feed(mux, "cam2", image_from_array(np.full((4, 4, 3), [255, 0, 0], np.uint8), encoding="rgb8")) + out = mux._composite() + assert out is not None + assert out.encoding == "bgr8" + assert out.header.frame_id == "camera_mux" + assert image_to_rgb(out)[0, 0].tolist() == [255, 0, 0] + assert image_to_rgb(out)[0, 6].tolist() == [255, 0, 0] + + +def test_hosted_blueprints_bind_canonical_mux_image_to_video_transport(): + for blueprint in ( + teleop_hosted_go2_transport, + teleop_hosted_go2_multicam, + teleop_hosted_xarm6, + teleop_hosted_xarm7, + ): + assert ("mux_image", Image) in blueprint.transport_map diff --git a/dimos/types/timestamped.py b/dimos/types/timestamped.py index 284c56c1eae..81e93c18b17 100644 --- a/dimos/types/timestamped.py +++ b/dimos/types/timestamped.py @@ -13,7 +13,7 @@ # limitations under the License. from collections import defaultdict from datetime import datetime, timezone -from typing import Generic, TypeVar, Union +from typing import Any, Generic, TypeVar, Union, overload from dimos_lcm.builtin_interfaces import Time as ROSTime from reactivex import create @@ -116,6 +116,17 @@ def ros_timestamp(self) -> list[int]: T = TypeVar("T", bound=Timestamped) +VALUE = TypeVar("VALUE") + + +class TimestampedData(Timestamped, Generic[VALUE]): + """Local time-series adapter; leave timestamp metadata outside wire values.""" + + def __init__(self, value: VALUE, ts: float) -> None: + super().__init__(ts) + self.value = value + + PRIMARY = TypeVar("PRIMARY", bound=Timestamped) SECONDARY = TypeVar("SECONDARY", bound=Timestamped) @@ -166,12 +177,32 @@ def get_tuple(self) -> tuple[PRIMARY, ...]: return (self.primary, *self.matches) # type: ignore[arg-type] +@overload +def align_timestamped( + primary_observable: Observable[PRIMARY], + secondary_observable: Observable[SECONDARY], + /, + *, + buffer_size: float = 1.0, + match_tolerance: float = 0.1, +) -> Observable[tuple[PRIMARY, SECONDARY]]: ... + + +@overload +def align_timestamped( + primary_observable: Observable[PRIMARY], + *secondary_observables: Observable[SECONDARY], + buffer_size: float = 1.0, + match_tolerance: float = 0.1, +) -> Observable[tuple[PRIMARY | SECONDARY, ...]]: ... + + def align_timestamped( primary_observable: Observable[PRIMARY], *secondary_observables: Observable[SECONDARY], buffer_size: float = 1.0, # seconds match_tolerance: float = 0.1, # seconds -) -> Observable[tuple[PRIMARY, ...]]: +) -> Observable[tuple[Any, ...]]: """Align a primary observable with one or more secondary observables. Args: diff --git a/dimos/visualization/vis_module.py b/dimos/visualization/vis_module.py index 6bffd9a5c7b..c439ad3c42f 100644 --- a/dimos/visualization/vis_module.py +++ b/dimos/visualization/vis_module.py @@ -54,11 +54,9 @@ def vis_module( match viewer_backend: case "rerun": - from dimos.protocol.pubsub.impl.lcmpubsub import LCM from dimos.visualization.rerun.bridge import RerunBridgeModule rerun_config = {**rerun_config} # copy (avoid mutation) - rerun_config.setdefault("pubsubs", [LCM()]) bundle = autoconnect( RerunBridgeModule.blueprint( **rerun_config, diff --git a/docs/capabilities/perception/index.md b/docs/capabilities/perception/index.md index 7dfdee1f67d..90718e61922 100644 --- a/docs/capabilities/perception/index.md +++ b/docs/capabilities/perception/index.md @@ -2,6 +2,185 @@ ## Detections +On the CDR proposal branch, detector inputs are generated `Image` values from +`dimos.messages.sensor_msgs.msg`. Detection collections remain local Python +application objects; `to_ros_detection2d_array()` produces a generated +`Detection2DArray` for typed ports, recording and CDR transport. + +This complete CPU example uses the project's sample image and model assets +(downloaded on first use). It performs detection without a robot or API key: + +```python +from dimos.message_utils.image import image_from_file +from dimos.message_utils.time import header_now +from dimos.perception.detection.detectors.yolo import Yolo2DDetector +from dimos.utils.data import get_data + +image = image_from_file(get_data("cafe.jpg"), header=header_now("camera_optical")) +detector = Yolo2DDetector(device="cpu") +try: + detections = detector.process_image(image) + message = detections.to_ros_detection2d_array() + assert message.header == image.header + assert len(message.detections) > 0 +finally: + detector.stop() +``` + +`Detection2DModule` and `Detection3DModule` keep their existing processing and +port APIs. Their image, calibration, TF, point-cloud and detection ports now +use generated values. Until the proposal's default transport cutover is +complete, select the explicit CDR transport when wiring these ports; see +[message packages](/docs/development/messages-external-project.md). + +3D projection takes a generated calibration and a `TransformStamped` mapping +the cloud frame into the optical frame. Cloud filters preserve all selected +point records, including extra fields and source nanoseconds. Generated +messages do not acquire `.ts` or NumPy/Open3D methods: use the helpers in +`dimos.message_utils.image`, `dimos.message_utils.pointcloud` and `dimos.message_utils.geometry`. + +The real Go2 regression fixture is a small canonical CDR selection from +`unitree_go2_lidar_corrected`. Its manifest records the original archive OID, +source entry hashes, recorder times, exact source stamps and CDR hashes. +Recorder-time pairing does not make sensor source clocks identical: the +fixture retains their measured offset, and TF lookups must select the right +time domain. Historical LCM/pickle recordings require explicit conversion. + +### Tracking and embeddings + +Person tracking and re-ID keep their existing module methods and ports. Their +zero-tolerance image/detection join compares exact source nanoseconds inside +the existing two-second window. A published target retains the image source +stamp even when its TF sample is older. + +With the existing ML dependencies, the same generated image can be embedded: + +```python +from dimos.models.embedding.clip import CLIPModel +from dimos.message_utils.image import image_from_file +from dimos.message_utils.time import header_now +from dimos.utils.data import get_data + +image = image_from_file(get_data("cafe.jpg"), header=header_now("camera_optical")) +model = CLIPModel(device="cpu") +try: + embedding = model.embed(image) + assert tuple(embedding.vector.shape) == (512,) +finally: + model.stop() +``` + +CLIP, DINO, SigLIP, SigLIP2 and TorchReID use generated images and shared pixel +helpers. Their embedding API keeps seconds-based timestamps, including a +valid zero stamp. Model weights follow each existing backend's cache policy; +message import and message installation do not trigger model downloads. + +## Fiducial markers + +Fiducial detection uses generated images, calibration and stamped transforms. +This example renders and detects a marker entirely on CPU; it needs the existing +OpenCV contrib dependency, but no camera, model download, network or API key: + +```python +import cv2 +import numpy as np + +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.std_msgs.msg import Header +from dimos.message_utils.camera_info import camera_info_from_intrinsics +from dimos.message_utils.image import image_from_array +from dimos.perception.fiducial.marker_detect import detect_markers_in_image + +pixels = np.full((400, 400), 255, dtype=np.uint8) +dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) +pixels[100:300, 100:300] = cv2.aruco.generateImageMarker(dictionary, 7, 200) +header = Header(frame_id="camera_optical", stamp=Time(sec=0, nanosec=123456789)) +image = image_from_array(pixels, encoding="mono8", header=header) +camera = camera_info_from_intrinsics(400, 400, 200, 200, 400, 400, header=header) +mount = TransformStamped( + header=Header(frame_id="world", stamp=header.stamp), + child_frame_id="camera_optical", + transform=Transform(translation=Vector3(x=2, y=0, z=0), rotation=Quaternion(x=0, y=0, z=0, w=1)), +) +markers = detect_markers_in_image( + image, camera_info=camera, world_T_optical=mount, + marker_length_m=0.2, aruco_dictionary="DICT_4X4_50", +) +message = markers[0].to_detection3d_msg() +assert message.id == "7" +assert message.header == Header(frame_id="world", stamp=header.stamp) +assert abs(message.bbox.center.position.z - 0.4) < 0.01 +``` + +`MarkerDetectionStreamModule` and `MarkerTfModule` keep their existing stream +names. Marker arrays and resulting TF messages preserve the image source +nanoseconds, including zero. Frame aggregation also distinguishes adjacent +nanoseconds when the local Observation API represents their times as the same +floating-point second. Select explicit CDR transports until the default runtime +cutover is complete; the desk blueprint retains its LCM backend with CDR encoding. + +## Query-time localization geometry + +Localization discovers generated calibration, color, depth and lidar streams by +message identity and source frame. Depth classification inspects decoded pixels, +including both byte orders; wire image data is a flat byte array. Existing model +selection and localization thresholds remain unchanged. + +This standalone geometry example composes two mounts without loading models: + +```python +from dimos.perception.localize.rig import Rig +from dimos.protocol.tf.static_tf_publisher import frames_to_edge_transforms +from dimos.protocol.tf.tf import MultiTBuffer + +tf = MultiTBuffer() +tf.receive_transform(*frames_to_edge_transforms([ + ("base_link", "world", (2, 0, 0), (0, 0, 0)), + ("front", "base_link", (0.5, 0, 0), (0, 0, 0)), +])) +rig = Rig(cameras={}, tf=tf) +pose = rig.camera_pose(0, "front") +assert pose is not None +assert pose.header.frame_id == "world" +assert pose.pose.position.x == 2.5 +``` + +The local color cache keeps JPEG compression in a generated CompressedImage +CDR envelope (`jpegcdr` storage codec); geometry uses lossless `lz4+cdr`. +JPEG decoding normalizes color to BGR8 while retaining the exact source Header. +These are local storage codecs, not new transport or MCAP encodings. + +Merged lidar maps and grouped object clouds retain the newest contributing +source stamp in integer nanoseconds. Query results still expose seconds through +the existing `Localization.last_seen_timestamp` API; `as_detection_array()` +converts that value to a generated Detection3DArray. Live module/blueprint +execution requires the remaining default transport cutover. Historical recordings +still require explicit conversion; this slice does not add a legacy decoder. + +## Experimental object memory + +The experimental object, scene-registration and tracker modules now use generated +image, calibration, detection and TF values on their typed ports. EdgeTAM and +YOLO-E segmentation retain their existing model APIs and use the same image +helpers. OpenCV tracking keeps its existing geometry and processing-time policy. + +Object geometry keeps the color source stamp on its pose/detection and the depth +source stamp on its projected cloud. Scene scans preserve their capture Header; +spatial deduplication compares source frame and integer nanoseconds rather than +rounded application seconds. Wall-clock pending-object TTL remains separate, and +source timestamp zero is not replaced by wall time. Until default encoding +cutover, these generated ports require the explicit CDR transports described +above. Offline boundary tests do not constitute full model or robot acceptance. + +Temporal memory and PerceiveLoop also accept generated images. Pixel comparison +and CLIP frame preparation respect encoding and row stride. PerceiveLoop retains +its rolling sharpest-frame selection and repeated timer output, with disposable +source subscriptions. Temporal memory publishes generated `dimos_msgs/EntityMarkers` +with a world Header; the Rerun adapter owns marker labels, colors and geometry. +Generated values themselves have no rendering methods. Existing query and skill +APIs remain unchanged. + ## Experimental WorldBelief The experimental xArm6 WorldBelief stack records RGB-D observations and processes diff --git a/docs/usage/sensor_streams/index.md b/docs/usage/sensor_streams/index.md index 1f328977681..4dbc219fdc0 100644 --- a/docs/usage/sensor_streams/index.md +++ b/docs/usage/sensor_streams/index.md @@ -39,3 +39,139 @@ aligned = align_timestamped( aligned.subscribe(lambda pair: process_frame_with_pointcloud(*pair)) ``` + +## Generated camera values in the CDR proposal + +`Webcam` produces `dimos.messages.sensor_msgs.msg.Image` values. It converts +OpenCV BGR frames to `rgb8` and applies the selected stereo crop before setting +`width`, `height` and `step`. `CameraModule` exposes these generated ports: + +| Port | Value type | Contents | +| --- | --- | --- | +| `color_image` | `sensor_msgs/msg/Image` | Pixels and capture Header | +| `camera_info` | `sensor_msgs/msg/CameraInfo` | Calibration and optical frame | +| `tf` | `tf2_msgs/msg/TFMessage` | Mount and optical-frame transforms | + +Read pixels with the array adapter. Keep the generated value for typed ports +and recording: + +```python +from dimos.messages.sensor_msgs.msg import Image +from dimos.message_utils.image import image_view + + +def mean_red(image: Image) -> float: + assert image.encoding == "rgb8" + return float(image_view(image)[..., 0].mean()) +``` + +Configured calibration remains a generated `CameraInfo` instance. When no valid +intrinsics are supplied, the existing nominal pinhole behavior uses the emitted +frame size, including a stereo crop. Metadata publishes a fresh Header timestamp +without changing the configured mount transform. + +`GstreamerCameraModule.video` also emits generated `Image` values, with `bgr8` +pixels. It copies padded rows before releasing the SDK buffer and keeps source +PTS plus the configured offset in integer nanoseconds. Its existing rejection +of pre-2000 source timestamps remains in place. + +These ports need explicit CDR transport wiring until the default-transport +cutover is complete. ZED producers remain a separate migration slice. This change does not start cameras or switch robot blueprints. + +Run the offline producer checks from the checkout: + +```sh +python -m pytest --noconftest -o addopts='' -q \ + dimos/hardware/sensors/camera/test_webcam.py \ + dimos/hardware/sensors/camera/test_webcam_backend.py \ + dimos/hardware/sensors/camera/test_module.py \ + dimos/hardware/sensors/camera/gstreamer/test_generated_frames.py +``` + +The GStreamer cases use a real in-memory `appsrc`/`appsink` pipeline, not a camera +or TCP stream. They require PyGObject and the GStreamer core, app and video +libraries/typelibs; they are skipped when that optional binding is absent. +No test installs system libraries. Constructing a viewer blueprint defers +backend allocation until startup, so importing a camera module opens no viewer +multicast socket. + +### Native V4L2 producer + +`V4L2Camera.jpeg_out` now uses generated `sensor_msgs/msg/CompressedImage` +values. The Rust worker serializes those values with the SDK CDR codec. Device +selection, driver-clock correction, JPEG encoding and retries keep their +existing behavior. + +The independent Nix build prepares the generated Rust owner from fixed-hash +project and upstream inputs. It calls the same Python source-preparation and +code-generation functions as the main package build; it does not build the +whole Python runtime or commit upstream `.msg` copies. The SDK source and +message configuration are pinned to a published project commit. Updating that +pin is a maintainer action when the SDK or built-in schemas change. + +Build without opening a device: + +```sh +cd dimos/hardware/sensors/camera/v4l2/rust +nix build path:. +``` + +For a repository developer using Cargo, first install the checkout normally so +the package backend prepares `build/message-source/rust`, then run library +acceptance only: + +```sh +uv pip install -e . +cargo test --manifest-path dimos/hardware/sensors/camera/v4l2/rust/Cargo.toml \ + --locked --lib --jobs 2 +``` + +This needs the native Rust/C toolchains. The library tests check CDR, clock +correction and software JPEG with synthetic data; they do not open V4L2 or start +the worker. Jetson driver validation and physical capture remain separate +hardware acceptance. + +### Native RealSense producer + +`RealSenseCamera` uses generated values on all camera ports: + +| Ports | Value type | +| --- | --- | +| `color_image`, `depth_image`, `infrared_left`, `infrared_right` | `sensor_msgs/msg/Image` | +| `camera_info`, `depth_camera_info`, `infrared_left_camera_info`, `infrared_right_camera_info` | `sensor_msgs/msg/CameraInfo` | +| `pointcloud` | `sensor_msgs/msg/PointCloud2` | +| `imu` | `sensor_msgs/msg/Imu` | +| `imu_info` | `dimos_msgs/msg/ImuInfo` | +| `tf` | `tf2_msgs/msg/TFMessage` | + +The IMU noise message is generated from the project-owned definition; it no +longer uses a handwritten LCM fingerprint/encoder. Calibration, RGBD projection, +IMU pairing, frame names and capture configuration retain their existing roles. +The Rust TF handle retains its lookup/publish operations. RealSense selects CDR +through explicit codec hooks; an existing bare `#[tf]` declaration retains its +existing wire format. The graph's existing floating-point seconds API remains +unchanged; raw messages use the generated Time fields. + +The RealSense Nix build uses the same isolated source assembly as V4L2, with its +own pinned SDK/configuration snapshot. To build and run library regression tests +without starting a camera: + +```sh +cd dimos/hardware/sensors/camera/realsense/rust +nix build path:.#tests +``` + +For a Cargo developer with the checkout installed and `librealsense2` available +through pkg-config: + +```sh +cargo test --manifest-path dimos/hardware/sensors/camera/realsense/rust/Cargo.toml \ + --locked --bin realsense_native --jobs 2 -- tests:: --skip bench:: +``` + +These tests use real SDK intrinsics values and synthetic frame data. They check +canonical CDR in both byte orders, normalized time, calibration, point/color +alignment and source headers. They do not enumerate or capture from devices. +Nix execution, physical camera behavior and platform-specific drivers require +separate verification; the local regression build used an isolated official +librealsense 2.56.5 build tree without system installation. diff --git a/docs/usage/transforms.md b/docs/usage/transforms.md index dcee1610187..f1c825415f4 100644 --- a/docs/usage/transforms.md +++ b/docs/usage/transforms.md @@ -1,469 +1,147 @@ # Transforms -## The Problem: Everything Measures from Its Own Perspective +A transform maps points from a child frame into a parent frame. For example, +`world -> base_link -> camera_optical` describes the camera pose in the world. +Camera intrinsics convert pixels and depth into camera-frame points; this frame +chain then places those points in the robot or world frame. -Imagine your robot has an RGB-D camera, which captures both color images and depth (distance to each pixel). These are common in robotics: Intel RealSense, Microsoft Kinect, and similar sensors. +![Coordinate frame tree](assets/transforms_tree.svg) -The camera spots a coffee mug at pixel (320, 240), and the depth sensor says it's 1.2 meters away. You want the robot arm to pick it up. But the arm doesn't understand pixels or camera-relative distances. It needs coordinates in its own workspace: "move to position (0.8, 0.3, 0.1) meters from my base." +The CDR proposal uses generated `TransformStamped`, `PoseStamped` and +`TFMessage` values from `dimos.messages`. Frame names and source time live in +`header.frame_id` and `header.stamp`; the destination frame is `child_frame_id`. +Geometry operations live in `dimos.message_utils.geometry`. -To convert camera measurements to arm coordinates, you need to know: -- The camera's intrinsic parameters (focal length, sensor size) to convert pixels to a 3D direction -- The depth value to get the full 3D position relative to the camera -- Where the camera is mounted relative to the arm, and at what angle +## Define a mount -This chain of conversions is what **transforms** handle: (pixels + depth) → 3D point in camera frame → robot coordinates. - -
-diagram source - -```pikchr fold output=assets/transforms_tree.svg -color = white -fill = none - -# Root (left side) -W: box "world" rad 5px fit wid 170% ht 170% -arrow right 0.4in -RB: box "robot_base" rad 5px fit wid 170% ht 170% - -# Camera branch (top) -arrow from RB.e right 0.3in then up 0.4in then right 0.3in -CL: box "camera_link" rad 5px fit wid 170% ht 170% -arrow right 0.4in -CO: box "camera_optical" rad 5px fit wid 170% ht 170% -text "mug here" small italic at (CO.s.x, CO.s.y - 0.25in) - -# Arm branch (bottom) -arrow from RB.e right 0.3in then down 0.4in then right 0.3in -AB: box "arm_base" rad 5px fit wid 170% ht 170% -arrow right 0.4in -GR: box "gripper" rad 5px fit wid 170% ht 170% -text "target here" small italic at (GR.s.x, GR.s.y - 0.25in) -``` - -
- -![output](assets/transforms_tree.svg) - -Each arrow in this tree is a transform. To get the mug's position in gripper coordinates, you chain transforms through their common parent: camera → robot_base → arm → gripper. - -## What's a Coordinate Frame? - -A **coordinate frame** is simply a point of view: an origin point and a set of axes (X, Y, Z) from which you measure positions and orientations. - -Think of it like giving directions: -- **GPS** says you're at 37.7749° N, 122.4194° W -- The **coffee shop floor plan** says "table 5 is 3 meters from the entrance" -- Your **friend** says "I'm two tables to your left" - -These all describe positions in the same physical space, but from different reference points. Each is a coordinate frame. - -In a robot: -- The **camera** measures in pixels, or in meters relative to its lens -- The **LIDAR** measures distances from its own mounting point -- The **robot arm** thinks in terms of its base or end-effector position -- The **world** has a fixed coordinate system everything lives in - -Each sensor, joint, and reference point has its own frame. - -## The Transform Class - -The `Transform` class at [`geometry_msgs/Transform.py`](/dimos/msgs/geometry_msgs/Transform.py#L21) represents a spatial transformation with: - -- `frame_id` - The parent frame name -- `child_frame_id` - The child frame name -- `translation` - A `Vector3` (x, y, z) offset -- `rotation` - A `Quaternion` (x, y, z, w) orientation -- `ts` - Timestamp for temporal lookups +`frames_to_edge_transforms` converts frame specifications into generated values. +Each specification contains a frame name, parent name, translation in meters, +and fixed-axis XYZ roll/pitch/yaw in radians. A parent of `None` marks the root. ```python -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 - -# Camera 0.5m forward and 0.3m up from base, no rotation -camera_transform = Transform( - translation=Vector3(0.5, 0.0, 0.3), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), # Identity rotation - frame_id="base_link", - child_frame_id="camera_link", -) -print(camera_transform) +from dimos.protocol.tf.static_tf_publisher import frames_to_edge_transforms + +edges = frames_to_edge_transforms([ + ("world", None, (0, 0, 0), (0, 0, 0)), + ("base_link", "world", (1, 0, 0), (0, 0, 0)), + ("camera_optical", "base_link", (0.5, 0, 0.3), (0, 0, 0)), +]) +assert edges[0].header.frame_id == "world" +assert edges[1].child_frame_id == "camera_optical" ``` -```results -base_link -> camera_link - Translation: → Vector Vector([0.5 0. 0.3]) - Rotation: Quaternion(0.000000, 0.000000, 0.000000, 1.000000) -``` +These definitions have zero timestamps. `StaticTFPublisher` publishes configured +mounts repeatedly with a fresh timestamp, so late subscribers and recorders +receive them. -### Transform Operations +## Compose and invert -Transforms can be composed and inverted: +Compose connected edges in parent-to-child order. Inversion swaps the frames. ```python -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 - -# Create two transforms -t1 = Transform( - translation=Vector3(1.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", - child_frame_id="camera_link", -) -t2 = Transform( - translation=Vector3(0.0, 0.5, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="camera_link", - child_frame_id="end_effector", -) - -# Compose: base_link -> camera -> end_effector -t3 = t1 + t2 -print(f"Composed: {t3.frame_id} -> {t3.child_frame_id}") -print(f"Translation: ({t3.translation.x}, {t3.translation.y}, {t3.translation.z})") - -# Inverse: if t goes A -> B, -t goes B -> A -t_inverse = -t1 -print(f"Inverse: {t_inverse.frame_id} -> {t_inverse.child_frame_id}") +from dimos.message_utils.geometry import compose_transforms, inverse_transform +from dimos.protocol.tf.static_tf_publisher import frames_to_edge_transforms + +first, second = frames_to_edge_transforms([ + ("base_link", "world", (1, 0, 0), (0, 0, 0)), + ("camera_optical", "base_link", (0.5, 0, 0.3), (0, 0, 0)), +]) +world_camera = compose_transforms(first, second) +assert world_camera.transform.translation.x == 1.5 +camera_world = inverse_transform(world_camera) +assert camera_world.header.frame_id == "camera_optical" +assert camera_world.child_frame_id == "world" ``` -```results -Composed: base_link -> end_effector -Translation: (1.0, 0.5, 0.0) -Inverse: camera_link -> base_link -``` +Use `transform_matrix(value.transform)` for a homogeneous NumPy matrix and +`pose_from_transform(value)` for a generated `PoseStamped`. -### Converting to Matrix Form +## Query a frame chain -For integration with libraries like NumPy or OpenCV: +`MultiTBuffer` keeps recent samples for each parent/child pair. `get()` resolves +forward or inverse edges and composes the shortest available chain. It returns +`None` when no eligible chain is available. ```python -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.protocol.tf.static_tf_publisher import frames_to_edge_transforms +from dimos.protocol.tf.tf import MultiTBuffer -t = Transform( - translation=Vector3(1.0, 2.0, 3.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), -) -matrix = t.to_matrix() -print("4x4 transformation matrix:") -print(matrix) +buffer = MultiTBuffer() +buffer.receive_transform(*frames_to_edge_transforms([ + ("base_link", "world", (1, 0, 0), (0, 0, 0)), + ("camera_optical", "base_link", (0.5, 0, 0.3), (0, 0, 0)), +])) +pose = buffer.get_pose("world", "camera_optical") +assert pose is not None +assert pose.pose.position.x == 1.5 ``` -```results -4x4 transformation matrix: -[[1. 0. 0. 1.] - [0. 1. 0. 2.] - [0. 0. 1. 3.] - [0. 0. 0. 1.]] -``` +Pass `time_point` and `time_tolerance` in seconds to select the nearest sample. +`forward_tolerance` bounds how long a lookup waits for a missing sample or chain. +The time index retains integer source nanoseconds and owns its snapshots; the +existing query arguments remain seconds. -## Frame IDs in Modules +## Attach a stream -Modules in dimOS automatically get a `frame_id` property. This is controlled by two config options in [`core/module.py`](/dimos/core/module.py#L78): +`TF(stream)` subscribes to `TFMessage` values and exposes the same lookups. +`publish()` updates its local graph and publishes the generated message through +that stream. With no stream, `TF()` is a local buffer. -- `frame_id` - The base frame name (defaults to the class name) -- `frame_id_prefix` - Optional prefix for namespacing +This complete offline example uses the raw LCM in-process queue with CDR. The +transport owns the encoding; callers publish generated values. ```python -from dimos.core.module import Module, ModuleConfig - -class MyModuleConfig(ModuleConfig): - frame_id: str = "sensor_link" - frame_id_prefix: str | None = None - -class MySensorModule(Module): - config: MyModuleConfig - -# With default config: -sensor = MySensorModule() -print(f"Default frame_id: {sensor.frame_id}") - -# With prefix (useful for multi-robot scenarios): -sensor2 = MySensorModule(frame_id_prefix="robot1") -print(f"With prefix: {sensor2.frame_id}") +from contextlib import ExitStack + +from dimos.core.cdr_transport import CdrTransport +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.protocol.tf.static_tf_publisher import frames_to_edge_transforms +from dimos.protocol.tf.tf import TF + +with ExitStack() as cleanup: + wire = CdrTransport("mounts", TFMessage, backend="lcm", url="memq://") + cleanup.callback(wire.stop) + sender, receiver = TF(wire), TF(wire) + cleanup.callback(sender.dispose) + cleanup.callback(receiver.dispose) + sender.publish(*frames_to_edge_transforms([ + ("camera", "world", (1, 2, 3), (0, 0, 0)), + ])) + result = receiver.get("world", "camera", forward_tolerance=2.0) + assert result is not None + assert result.transform.translation.z == 3.0 ``` -```results -00:33:00.665 [inf][otocol/service/zenohservice.py] Zenoh session opened connect=[] gossip=True listen=['tcp/127.0.0.1:0'] mode=peer multicast_interface=lo -Default frame_id: sensor_link -With prefix: robot1/sensor_link -``` - -## The tf Topic - -Transforms travel on an ordinary stream named `tf` carrying [`TFMessage`](/dimos/msgs/tf2_msgs/TFMessage.py)s. A module declares the port like any other stream, choosing the direction it actually uses: - -- `tf: Out[TFMessage]`: publishes transforms -- `tf: In[TFMessage]`: consumes transforms -- `tf: IO[TFMessage]`: both, on the same topic - -The coordinator wires every port named `tf` onto one shared `/tf` transport, so all modules see one transform tree. - -For lookups, use `self.tfbuffer`, a lazy [`TF`](/dimos/protocol/tf/tf.py) buffer view over the module's `tf` port that subscribes to the stream, buffers what it sees, and answers `get()` queries (including chained and inverse lookups). It is built on first touch and disposed with the module. Outside modules, construct the view explicitly: `TF(stream)` accepts any port or raw transport. - -### Multi-Module Transform Example - -This example demonstrates how multiple modules publish and receive transforms. Three modules work together: - -1. **RobotBaseModule** - Publishes `world -> base_link` (robot's position in the world) -2. **CameraModule** - Publishes `base_link -> camera_link` (camera mounting position) and `camera_link -> camera_optical` (optical frame convention) -3. **PerceptionModule** - Looks up transforms between any frames - -```python skip ansi=false -import time -import reactivex as rx -from dimos.core.core import rpc -from dimos.core.module import Module -from dimos.core.stream import In, Out -from dimos.core.coordination.blueprints import autoconnect -from dimos.core.coordination.module_coordinator import ModuleCoordinator -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.tf2_msgs.TFMessage import TFMessage - -class RobotBaseModule(Module): - """Publishes the robot's position in the world frame at 10Hz.""" - - tf: Out[TFMessage] - - @rpc - def start(self) -> None: - super().start() - - def publish_pose(_): - robot_pose = Transform( - translation=Vector3(2.5, 3.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="world", - child_frame_id="base_link", - ts=time.time(), - ) - self.tf.publish(TFMessage(robot_pose)) - - self.register_disposable( - rx.interval(0.1).subscribe(publish_pose) - ) - -class CameraModule(Module): - """Publishes camera transforms at 10Hz.""" - - tf: Out[TFMessage] - - @rpc - def start(self) -> None: - super().start() +Modules use the existing `tf: In[TFMessage]` or `tf: IO[TFMessage]` port and lazy +`module.tfbuffer` accessor after stream wiring. Generated ports use the shared CDR codec through the existing LCM or Zenoh +transport. See [module composition](/docs/usage/modules.md) for deployment and +[recording generated CDR](/examples/cdr-recording/README.md) for storage. - def publish_transforms(_): - camera_mount = Transform( - translation=Vector3(1.0, 0.0, 0.3), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", - child_frame_id="camera_link", - ts=time.time(), - ) - optical_frame = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), - frame_id="camera_link", - child_frame_id="camera_optical", - ts=time.time(), - ) - self.tf.publish(TFMessage(camera_mount, optical_frame)) +## Query a recording - self.register_disposable( - rx.interval(0.1).subscribe(publish_transforms) - ) - -class PerceptionModule(Module): - """Receives transforms and performs lookups.""" - - tf: In[TFMessage] - - @rpc - def lookup(self) -> None: - - # Will pretty-print information on transforms in the buffer - print(self.tfbuffer) - - direct = self.tfbuffer.get("world", "base_link") - print(f"Direct: robot is at ({direct.translation.x}, {direct.translation.y})m in world\n") - - # Chained lookup - automatically composes world -> base -> camera -> optical - chained = self.tfbuffer.get("world", "camera_optical") - print(f"Chained: {chained}\n") - - # Inverse lookup - automatically inverts direction - inverse = self.tfbuffer.get("camera_optical", "world") - print(f"Inverse: {inverse}\n") - - print("Transform tree:") - print(self.tfbuffer.graph()) - -if __name__ == "__main__": - dimos = ModuleCoordinator.build(autoconnect( - RobotBaseModule.blueprint(), - CameraModule.blueprint(), - PerceptionModule.blueprint(), - )) - - # Give worker TF publishers a moment to populate the buffer before querying. - time.sleep(2.5) - - dimos.get_instance(PerceptionModule).lookup() - - dimos.stop() - -``` - -```results -16:21:45.203 [inf][ation/worker_manager_python.py] Worker pool started. n_workers=2 -16:21:45.445 [inf][/coordination/python_worker.py] Deployed module. module=RobotBaseModule module_id=0 worker_id=0 -16:21:45.451 [inf][/coordination/python_worker.py] Deployed module. module=CameraModule module_id=1 worker_id=1 -16:21:45.452 [inf][/coordination/python_worker.py] Deployed module. module=PerceptionModule module_id=2 worker_id=0 -16:21:47.968 [inf][dination/module_coordinator.py] Stopping module... module=PerceptionModule -16:21:48.022 [inf][dination/module_coordinator.py] Module stopped. module=PerceptionModule -16:21:48.022 [inf][dination/module_coordinator.py] Stopping module... module=CameraModule -16:21:48.041 [inf][dination/module_coordinator.py] Module stopped. module=CameraModule -16:21:48.041 [inf][dination/module_coordinator.py] Stopping module... module=RobotBaseModule -16:21:48.062 [inf][dination/module_coordinator.py] Module stopped. module=RobotBaseModule -16:21:48.062 [inf][ation/worker_manager_python.py] Shutting down all workers... -16:21:48.062 [inf][/coordination/python_worker.py] Worker stopping module... module=CameraModule module_id=1 worker_id=1 -16:21:48.063 [inf][/coordination/python_worker.py] Worker module stopped. module=CameraModule module_id=1 worker_id=1 -TF(3 buffers): - TBuffer(base_link -> camera_link, 24 msgs, 2.37s [2026-04-21 01:21:45 - 2026-04-21 01:21:47]) - TBuffer(camera_link -> camera_optical, 24 msgs, 2.37s [2026-04-21 01:21:45 - 2026-04-21 01:21:47]) - TBuffer(world -> base_link, 24 msgs, 2.37s [2026-04-21 01:21:45 - 2026-04-21 01:21:47]) -Direct: robot is at (2.5, 3.0)m in world - -Chained: world -> camera_optical - Translation: → Vector Vector([3.5 3. 0.3]) - Rotation: Quaternion(-0.500000, 0.500000, -0.500000, 0.500000) - -Inverse: camera_optical -> world - Translation: → Vector Vector([ 3. 0.3 -3.5]) - Rotation: Quaternion(0.500000, -0.500000, 0.500000, 0.500000) - -Transform tree: -┌─────┐ -│world│ -└┬────┘ -┌▽────────┐ -│base_link│ -└┬────────┘ -┌▽──────────┐ -│camera_link│ -└┬──────────┘ -┌▽─────────────┐ -│camera_optical│ -└──────────────┘ -``` - - -You can view these transforms in 3D using the Rerun viewer (see [Visualization](/docs/usage/visualization.md)). - -![transforms](assets/transforms.png) - -Key points: - -- **One shared topic**: every `tf` port is autoconnected onto the same `/tf` transport -- **Chained lookups**: TF finds paths through the tree automatically -- **Inverse lookups**: Request transforms in either direction -- **Temporal buffering**: Transforms are timestamped and buffered (default 10s) for sensor fusion - -The transform tree from the example above, showing which module publishes each transform: - -
-diagram source - -```pikchr fold output=assets/transforms_modules.svg -color = white -fill = none - -# Frame boxes -W: box "world" rad 5px fit wid 170% ht 170% -A1: arrow right 0.4in -BL: box "base_link" rad 5px fit wid 170% ht 170% -A2: arrow right 0.4in -CL: box "camera_link" rad 5px fit wid 170% ht 170% -A3: arrow right 0.4in -CO: box "camera_optical" rad 5px fit wid 170% ht 170% - -# RobotBaseModule box - encompasses world->base_link -box width (BL.e.x - W.w.x + 0.15in) height 0.7in \ - at ((W.w.x + BL.e.x)/2, W.y - 0.05in) \ - rad 10px color 0x6699cc fill none -text "RobotBaseModule" italic at ((W.x + BL.x)/2, W.n.y + 0.25in) - -# CameraModule box - encompasses camera_link->camera_optical (starts after base_link) -box width (CO.e.x - BL.e.x + 0.1in) height 0.7in \ - at ((BL.e.x + CO.e.x)/2, CL.y + 0.05in) \ - rad 10px color 0xcc9966 fill none -text "CameraModule" italic at ((CL.x + CO.x)/2, CL.s.y - 0.25in) -``` - -
- -![output](assets/transforms_modules.svg) - -## Internals - -## Transform Buffer - -`TF` is a thin subscription layer over `MultiTBuffer`, a standalone class that maintains a temporal buffer of transforms (default 10 seconds) allowing queries at past timestamps. You can use it directly: +`StreamTF` uses the recording's typed TF stream with the same lookup arguments. +The generated stream selects the CDR codec through normal mem2 configuration. ```python -import time - -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.protocol.tf.tf import MultiTBuffer - -tf = MultiTBuffer() - -# Simulate transforms at different times -for i in range(5): - t = Transform( - translation=Vector3(float(i), 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", - child_frame_id="camera_link", - ts=time.time() + i * 0.1, - ) - tf.receive_transform(t) - -# Query the latest transform -result = tf.get("base_link", "camera_link") -print(f"Latest transform: x={result.translation.x}") -print(f"Buffer has {len(tf.buffers)} transform pair(s)") -print(tf) +from dimos.memory.store.memory import MemoryStore +from dimos.memory.tf import StreamTF +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.protocol.tf.static_tf_publisher import frames_to_edge_transforms + +with MemoryStore() as store: + stream = store.stream("tf", TFMessage) + edges = frames_to_edge_transforms([ + ("camera", "world", (1, 2, 3), (0, 0, 0)), + ]) + stream.append(TFMessage(transforms=edges), ts=0.0, pose=None) + result = StreamTF(stream).get("world", "camera", time_point=0.0) + assert result is not None + assert result.transform.translation.y == 2.0 ``` -```results -Latest transform: x=4.0 -Buffer has 1 transform pair(s) -MultiTBuffer(1 buffers): - TBuffer(base_link -> camera_link, 5 msgs, 0.40s [2026-08-24 15:36:04 - 2026-08-24 15:36:04]) -``` - -This is essential for sensor fusion where you need to know where the camera was when an image was captured, not where it is now. - -## Further Reading - -For a visual introduction to transforms and coordinate frames: -- [Coordinate Transforms (YouTube)](https://www.youtube.com/watch?v=NGPn9nvLPmg) - -For the mathematical foundations, the ROS documentation provides detailed background: - -- [ROS tf2 Concepts](http://wiki.ros.org/tf2) -- [ROS REP 103 - Standard Units and Coordinate Conventions](https://www.ros.org/reps/rep-0103.html) -- [ROS REP 105 - Coordinate Frames for Mobile Platforms](https://www.ros.org/reps/rep-0105.html) +SQLite and MCAP retain the same generated message fields and complete dependency +schemas. Historical LCM recordings need explicit conversion; this guide does +not provide automatic legacy decoding. -See also: -- [Modules](/docs/usage/modules.md) for understanding the module system -- [Configuration](/docs/usage/configuration.md) for module configuration patterns +For frame conventions, see [ROS REP 103](https://www.ros.org/reps/rep-0103.html) +and [ROS REP 105](https://www.ros.org/reps/rep-0105.html). diff --git a/examples/native-modules/rust/Cargo.lock b/examples/native-modules/rust/Cargo.lock index 5d93b115412..d01c97a8ee7 100644 --- a/examples/native-modules/rust/Cargo.lock +++ b/examples/native-modules/rust/Cargo.lock @@ -582,7 +582,7 @@ dependencies = [ [[package]] name = "dimos-lcm" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", "socket2 0.5.10", @@ -592,11 +592,11 @@ dependencies = [ [[package]] name = "dimos-module" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "dimos-lcm", "dimos-module-macros", - "lcm-msgs", + "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549)", "nalgebra", "rayon", "serde", @@ -612,7 +612,7 @@ dependencies = [ [[package]] name = "dimos-module-macros" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" +source = "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#fc6a4e58da89bb073a38dce5ac63728c6559d3a6" dependencies = [ "proc-macro2", "quote", @@ -625,7 +625,7 @@ name = "dimos-native-module-examples" version = "0.1.0" dependencies = [ "dimos-module", - "lcm-msgs", + "lcm-msgs 0.1.0 (git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen)", "serde", "tokio", "tracing", @@ -1403,6 +1403,14 @@ dependencies = [ "byteorder", ] +[[package]] +name = "lcm-msgs" +version = "0.1.0" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" +dependencies = [ + "byteorder", +] + [[package]] name = "libc" version = "0.2.189" diff --git a/examples/native-modules/rust/Cargo.toml b/examples/native-modules/rust/Cargo.toml index b6fb7fbc647..bacc5989800 100644 --- a/examples/native-modules/rust/Cargo.toml +++ b/examples/native-modules/rust/Cargo.toml @@ -20,7 +20,7 @@ name = "tf_broadcaster" path = "src/tf_broadcaster.rs" [dependencies] -dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6", version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } tokio = { version = "1", features = ["rt-multi-thread", "macros", "time"] } serde = { version = "1", features = ["derive"] } diff --git a/examples/native-modules/rust/crate-hashes.json b/examples/native-modules/rust/crate-hashes.json index 4c5fde86d6c..187d28513c6 100644 --- a/examples/native-modules/rust/crate-hashes.json +++ b/examples/native-modules/rust/crate-hashes.json @@ -1,6 +1,7 @@ { - "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module-macros@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=", - "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#dimos-module@0.1.0": "sha256-6ikqdS3sroOJGI4WGuSyFx2yZjtJ+4Qgt+IGlkFl+qA=" + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module-macros@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=", + "git+https://github.com/dimensionalOS/dimos?rev=fc6a4e58da89bb073a38dce5ac63728c6559d3a6#dimos-module@0.1.0": "sha256-ECU2xYh/j+a3XwyhO3UR/GPruo5jNzM3n/9S5BIvVrA=" } \ No newline at end of file