From 3bfd3c8ba1d54926892797bde51a7d4d4bdd52df Mon Sep 17 00:00:00 2001 From: cc Date: Tue, 20 Oct 2026 22:20:56 -0700 Subject: [PATCH 1/6] feat(perception): restack generated sensor and TF migration --- data/.lfs/unitree_go2_detection_cdr.tar.gz | 3 + .../camera/gstreamer/gstreamer_camera.py | 40 +- .../camera/gstreamer/test_generated_frames.py | 105 +++ dimos/hardware/sensors/camera/module.py | 57 +- .../sensors/camera/realsense/camera.py | 17 +- .../sensors/camera/realsense/rust/Cargo.lock | 108 ++- .../sensors/camera/realsense/rust/Cargo.toml | 28 +- .../camera/realsense/rust/crate-hashes.json | 6 +- .../sensors/camera/realsense/rust/flake.nix | 28 +- .../sensors/camera/realsense/rust/src/main.rs | 326 ++++++-- .../sensors/camera/realsense/test_camera.py | 5 + dimos/hardware/sensors/camera/spec.py | 7 +- dimos/hardware/sensors/camera/test_module.py | 73 ++ dimos/hardware/sensors/camera/test_webcam.py | 56 +- dimos/hardware/sensors/camera/v4l2/module.py | 2 +- .../sensors/camera/v4l2/rust/Cargo.lock | 108 ++- .../sensors/camera/v4l2/rust/Cargo.toml | 6 +- .../camera/v4l2/rust/crate-hashes.json | 6 +- .../sensors/camera/v4l2/rust/flake.nix | 27 +- .../sensors/camera/v4l2/rust/src/module.rs | 53 +- dimos/hardware/sensors/camera/webcam.py | 58 +- dimos/hardware/sensors/camera/zed/camera.py | 201 ++--- dimos/hardware/sensors/camera/zed/compat.py | 2 +- dimos/hardware/sensors/camera/zed/messages.py | 112 +++ .../sensors/camera/zed/test_messages.py | 128 ++++ dimos/memory/module.py | 20 +- dimos/memory/test_module.py | 110 ++- dimos/memory/tf.py | 8 +- dimos/models/embedding/base.py | 4 +- dimos/models/embedding/clip.py | 8 +- dimos/models/embedding/dino.py | 8 +- dimos/models/embedding/siglip.py | 9 +- dimos/models/embedding/siglip2.py | 9 +- .../models/embedding/test_generated_images.py | 53 ++ dimos/models/embedding/treid.py | 8 +- dimos/models/segmentation/edge_tam.py | 11 +- .../test_generated_edge_tam_images.py | 50 ++ dimos/models/segmentation/test_yoloe.py | 20 +- dimos/models/segmentation/yoloe.py | 5 +- dimos/models/vl/base.py | 33 +- dimos/models/vl/qwen.py | 18 +- dimos/models/vl/test_base.py | 72 +- dimos/msgs/camera_info.py | 178 +++++ dimos/msgs/geometry.py | 376 +++++++++ dimos/msgs/image.py | 311 ++++++++ dimos/msgs/pointcloud.py | 380 ++++++++++ dimos/msgs/test_camera_info.py | 149 ++++ dimos/msgs/test_geometry.py | 283 +++++++ dimos/msgs/test_image_compression.py | 187 +++++ dimos/msgs/test_image_views.py | 233 ++++++ dimos/msgs/test_pointcloud.py | 462 +++++++++++ dimos/msgs/test_pointcloud_colors.py | 85 +++ dimos/msgs/test_time.py | 82 ++ dimos/msgs/time.py | 77 ++ dimos/perception/depth2depth_cloud/module.py | 6 +- .../depth2depth_cloud/rust/Cargo.lock | 88 ++- .../depth2depth_cloud/rust/Cargo.toml | 14 +- .../depth2depth_cloud/rust/crate-hashes.json | 6 +- .../depth2depth_cloud/rust/flake.nix | 29 +- .../depth2depth_cloud/rust/src/module.rs | 234 +++++- .../depth2depth_cloud/rust/src/undistort.rs | 40 +- .../test_depth2depth_cloud_config.py | 7 +- dimos/perception/detection/conftest.py | 239 +++--- dimos/perception/detection/detectors/base.py | 2 +- .../detection/detectors/grounding_dino.py | 8 +- dimos/perception/detection/detectors/omdet.py | 10 +- dimos/perception/detection/detectors/owlv2.py | 14 +- .../detection/detectors/person/yolo.py | 11 +- .../detectors/test_generated_images.py | 93 +++ dimos/perception/detection/detectors/yolo.py | 9 +- dimos/perception/detection/detectors/yoloe.py | 5 +- dimos/perception/detection/module2D.py | 38 +- dimos/perception/detection/module3D.py | 62 +- dimos/perception/detection/person_tracker.py | 81 +- dimos/perception/detection/project.py | 51 +- dimos/perception/detection/reid/module.py | 52 +- .../detection/reid/test_generated_module.py | 141 ++++ .../detection/type/detection2d/base.py | 5 +- .../detection/type/detection2d/bbox.py | 63 +- .../type/detection2d/imageDetections2D.py | 4 +- .../detection/type/detection2d/person.py | 5 +- .../detection/type/detection2d/point.py | 41 +- .../detection/type/detection2d/seg.py | 7 +- .../detection/type/detection2d/test_bbox.py | 4 +- .../type/detection2d/test_generated_bbox.py | 102 +++ .../detection/type/detection2d/test_person.py | 29 +- .../detection/type/detection3d/base.py | 19 +- .../detection/type/detection3d/bbox.py | 78 +- .../type/detection3d/imageDetections3D.py | 7 +- .../type/detection3d/imageDetections3DPC.py | 25 +- .../detection/type/detection3d/marker.py | 11 +- .../detection/type/detection3d/pointcloud.py | 185 +++-- .../type/detection3d/pointcloud_filters.py | 75 +- .../detection3d/test_generated_filters.py | 98 +++ .../detection3d/test_generated_projection.py | 174 +++++ .../detection3d/test_imageDetections3D.py | 88 ++- .../detection/type/detection3d/test_marker.py | 32 +- .../type/detection3d/test_pointcloud.py | 92 ++- .../detection/type/imageDetections.py | 23 +- .../detection/type/test_detection3d.py | 36 +- dimos/perception/experimental/moduleDB.py | 71 +- dimos/perception/experimental/object.py | 237 +++--- dimos/perception/experimental/objectDB.py | 46 +- .../experimental/object_scene_registration.py | 79 +- .../object_scene_registration_spec.py | 4 +- .../perception/experimental/object_tracker.py | 195 +++-- .../experimental/object_tracker_2d.py | 51 +- .../experimental/object_tracker_3d.py | 155 ++-- .../experimental/perceive_loop_skill.py | 35 +- .../experimental/spatial_perception.py | 54 +- .../temporal_memory/clip_filter.py | 7 +- .../temporal_memory/entity_graph_db.py | 2 +- .../frame_window_accumulator.py | 9 +- .../temporal_memory/temporal_memory.py | 31 +- .../temporal_memory/temporal_utils/helpers.py | 8 +- .../test_temporal_memory_module.py | 144 ++-- .../temporal_memory/window_analyzer.py | 2 +- .../experimental/test_generated_tracker.py | 143 ++++ .../experimental/test_object_generated.py | 202 +++++ ...test_object_scene_registration_temporal.py | 445 +++++------ .../experimental/test_perceive_loop_skill.py | 43 ++ .../experimental/test_spatial_memory.py | 69 +- .../fiducial/blueprints/desk_marker_tf.py | 61 +- .../blueprints/test_desk_marker_tf.py | 19 +- .../fiducial/fixture_verification.py | 11 +- dimos/perception/fiducial/marker_detect.py | 43 +- .../marker_detection_stream_module.py | 32 +- dimos/perception/fiducial/marker_pose.py | 96 ++- dimos/perception/fiducial/marker_tf_module.py | 50 +- .../perception/fiducial/marker_transformer.py | 90 +-- .../fiducial/test_fixture_verification.py | 43 +- .../fiducial/test_generated_marker_detect.py | 79 ++ dimos/perception/fiducial/test_helpers.py | 60 +- .../test_marker_detection_stream_module.py | 188 +++-- dimos/perception/fiducial/test_marker_pose.py | 59 +- .../fiducial/test_marker_tf_integration.py | 127 ++-- .../fiducial/test_marker_tf_module.py | 112 ++- .../fiducial/test_marker_transformer.py | 59 +- dimos/perception/localize/association.py | 24 +- dimos/perception/localize/dandetect.py | 2 +- .../localize/demo_blueprints/xarm_feed.py | 5 +- dimos/perception/localize/localize.py | 36 +- dimos/perception/localize/module.py | 68 +- dimos/perception/localize/rig.py | 96 ++- dimos/perception/localize/support_plane.py | 10 +- dimos/perception/localize/test_dandetect.py | 42 +- dimos/perception/localize/test_rig.py | 365 +++++++++ .../localize/tools/tool_localize.py | 107 ++- dimos/perception/localize/types.py | 2 +- .../impl/webrtc/providers/test_video_track.py | 103 +++ .../impl/webrtc/providers/video_track.py | 37 +- dimos/protocol/tf/static_tf_publisher.py | 36 +- dimos/protocol/tf/test_static_tf_publisher.py | 48 ++ dimos/protocol/tf/test_tf.py | 717 +++++++++++------- dimos/protocol/tf/tf.py | 139 ++-- dimos/spec/perception.py | 3 +- dimos/teleop/hosted/blueprints/cloudflare.py | 2 +- dimos/teleop/hosted/camera_mux.py | 48 +- dimos/teleop/hosted/test_camera_mux.py | 173 +++-- dimos/types/timestamped.py | 35 +- dimos/visualization/vis_module.py | 2 - docs/capabilities/perception/index.md | 179 +++++ docs/usage/sensor_streams/index.md | 136 ++++ docs/usage/transforms.md | 542 +++---------- native/message-source.nix | 52 ++ native/rust/dimos-module-macros/src/lib.rs | 98 ++- native/rust/dimos-module/Cargo.lock | 31 +- native/rust/dimos-module/Cargo.toml | 8 +- native/rust/dimos-module/src/cdr.rs | 95 +++ native/rust/dimos-module/src/lib.rs | 2 + native/rust/dimos-module/src/module.rs | 102 ++- native/rust/dimos-module/src/pointcloud.rs | 123 ++- native/rust/dimos-module/src/tf.rs | 275 ++++++- 173 files changed, 10881 insertions(+), 3277 deletions(-) create mode 100644 data/.lfs/unitree_go2_detection_cdr.tar.gz create mode 100644 dimos/hardware/sensors/camera/gstreamer/test_generated_frames.py create mode 100644 dimos/hardware/sensors/camera/test_module.py create mode 100644 dimos/hardware/sensors/camera/zed/messages.py create mode 100644 dimos/hardware/sensors/camera/zed/test_messages.py create mode 100644 dimos/models/embedding/test_generated_images.py create mode 100644 dimos/models/segmentation/test_generated_edge_tam_images.py create mode 100644 dimos/msgs/camera_info.py create mode 100644 dimos/msgs/geometry.py create mode 100644 dimos/msgs/image.py create mode 100644 dimos/msgs/pointcloud.py create mode 100644 dimos/msgs/test_camera_info.py create mode 100644 dimos/msgs/test_geometry.py create mode 100644 dimos/msgs/test_image_compression.py create mode 100644 dimos/msgs/test_image_views.py create mode 100644 dimos/msgs/test_pointcloud.py create mode 100644 dimos/msgs/test_pointcloud_colors.py create mode 100644 dimos/msgs/test_time.py create mode 100644 dimos/msgs/time.py create mode 100644 dimos/perception/detection/detectors/test_generated_images.py create mode 100644 dimos/perception/detection/reid/test_generated_module.py create mode 100644 dimos/perception/detection/type/detection2d/test_generated_bbox.py create mode 100644 dimos/perception/detection/type/detection3d/test_generated_filters.py create mode 100644 dimos/perception/detection/type/detection3d/test_generated_projection.py create mode 100644 dimos/perception/experimental/test_generated_tracker.py create mode 100644 dimos/perception/experimental/test_object_generated.py create mode 100644 dimos/perception/fiducial/test_generated_marker_detect.py create mode 100644 dimos/perception/localize/test_rig.py create mode 100644 dimos/protocol/tf/test_static_tf_publisher.py create mode 100644 native/message-source.nix create mode 100644 native/rust/dimos-module/src/cdr.rs 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/hardware/sensors/camera/gstreamer/gstreamer_camera.py b/dimos/hardware/sensors/camera/gstreamer/gstreamer_camera.py index 055d50f9602..028e22b87f2 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.messages.sensor_msgs.msg import Image +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.image import image_from_array +from dimos.msgs.time import duration_from_seconds, time_from_nanoseconds, to_nanoseconds 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..cf74ebf9afb --- /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.messages.sensor_msgs.msg import Image +from dimos.msgs.image import image_view +from dimos.msgs.time import to_nanoseconds +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..74095066a97 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.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.msgs.image import image_sharpness +from dimos.msgs.time import time_from_nanoseconds 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..5571b5fc4f8 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.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 diff --git a/dimos/hardware/sensors/camera/realsense/rust/Cargo.lock b/dimos/hardware/sensors/camera/realsense/rust/Cargo.lock index 89422d755e8..eaa775e545d 100644 --- a/dimos/hardware/sensors/camera/realsense/rust/Cargo.lock +++ b/dimos/hardware/sensors/camera/realsense/rust/Cargo.lock @@ -220,6 +220,20 @@ name = "bytemuck" version = "1.25.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "95832e849adfb21180ccb6826a99da14e5d266ae5c2e668e1602cf234f153797" +dependencies = [ + "bytemuck_derive", +] + +[[package]] +name = "bytemuck_derive" +version = "1.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6a1f896587b6f2c069c73d2f0913e2d590c3990285cd2f0b6aa02b786b4c679c" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.4", +] [[package]] name = "byteorder" @@ -539,10 +553,21 @@ dependencies = [ "subtle", ] +[[package]] +name = "dimos-generated-messages" +version = "0.0.14" +dependencies = [ + "heapless", + "re_cdr", + "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", @@ -552,13 +577,13 @@ dependencies = [ [[package]] name = "dimos-module" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" dependencies = [ "dimos-lcm", "dimos-module-macros", "lcm-msgs", "nalgebra", "rayon", + "re_cdr", "serde", "serde_json", "tokio", @@ -572,7 +597,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 +608,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 +950,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" @@ -968,6 +1001,23 @@ version = "0.17.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ed5909b6e89a2db4456e54cd5f673791d7eca6732202bbf2a9cc504fe2f9b84a" +[[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" version = "0.5.2" @@ -1360,7 +1410,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 +1968,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" @@ -2156,6 +2216,18 @@ dependencies = [ "yasna", ] +[[package]] +name = "re_cdr" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bba10c21433b597481f366b978cdf2617e56785b38cb0dcc3833c6b8ad7e061a" +dependencies = [ + "bytemuck", + "byteorder", + "serde", + "thiserror 2.0.20", +] + [[package]] name = "realsense-rust" version = "1.3.0" @@ -2294,6 +2366,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 +2623,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" diff --git a/dimos/hardware/sensors/camera/realsense/rust/Cargo.toml b/dimos/hardware/sensors/camera/realsense/rust/Cargo.toml index 5bd9e8c3c7e..9a491bf2d0b 100644 --- a/dimos/hardware/sensors/camera/realsense/rust/Cargo.toml +++ b/dimos/hardware/sensors/camera/realsense/rust/Cargo.toml @@ -17,24 +17,24 @@ 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" [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" [dependencies] -dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443" } -lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } +dimos-module = { path = "../../../../../../native/rust/dimos-module", features = ["cdr"] } +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/crate-hashes.json b/dimos/hardware/sensors/camera/realsense/rust/crate-hashes.json index 4c5fde86d6c..677538f3a27 100644 --- a/dimos/hardware/sensors/camera/realsense/rust/crate-hashes.json +++ b/dimos/hardware/sensors/camera/realsense/rust/crate-hashes.json @@ -1,6 +1,4 @@ { - "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=" } \ 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..505a7048540 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/62b899b2d3b0d315988df7db25664264391fb4ea"; + sha256 = "8d920aba91b51ba63f18e4cd88cdfff95ff4f3aa2dfbb34f2ef209a59182c75c"; + }; + 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..36d4d1f1496 100644 --- a/dimos/hardware/sensors/camera/realsense/rust/src/main.rs +++ b/dimos/hardware/sensors/camera/realsense/rust/src/main.rs @@ -18,20 +18,31 @@ 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::geometry_msgs::msg::{ + transform::Transform as TransformMsg, transform_stamped::TransformStamped, +}; +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_generated_messages::tf2_msgs::msg::tf_message::TFMessage; +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 realsense_rust::base::{Rs2Extrinsics, Rs2Intrinsics}; use realsense_rust::config::Config as RsConfig; use realsense_rust::context::Context; @@ -56,8 +67,63 @@ 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; +fn encode_cdr(value: &T) -> Vec { + cdr::encode(value).expect("serialize a valid camera value") +} + +fn encode_cdr_tf(values: &[Transform]) -> std::io::Result> { + let message = TFMessage { + transforms: values + .iter() + .map(|value| { + let t = value.translation(); + let q = value.rotation(); + TransformStamped { + header: header(&value.parent, value.ts), + child_frame_id: value.child.clone(), + transform: TransformMsg { + translation: Vector3Msg { + x: t.x, + y: t.y, + z: t.z, + }, + rotation: QuaternionMsg { + x: q.i, + y: q.j, + z: q.k, + w: q.w, + }, + }, + } + }) + .collect(), + }; + cdr::encode(&message) +} + +fn decode_cdr_tf(bytes: &[u8]) -> std::io::Result> { + let message: TFMessage = cdr::decode(bytes)?; + let mut values = Vec::with_capacity(message.transforms.len()); + for value in message.transforms { + let t = value.transform.translation; + let q = value.transform.rotation; + let Some(rotation) = UnitQuaternion::try_new(Quaternion::new(q.w, q.x, q.y, q.z), 1e-9) + else { + dimos_module::error_throttled!( + Duration::from_secs(1), parent = %value.header.frame_id, + child = %value.child_frame_id, "tf rotation is not a valid quaternion" + ); + continue; + }; + values.push(Transform::new( + value.header.frame_id, + value.child_frame_id, + value.header.stamp.sec as f64 + value.header.stamp.nanosec as f64 * 1e-9, + Isometry3::from_parts(Translation3::new(t.x, t.y, t.z), rotation), + )); + } + Ok(values) +} // ---- config, field for field with RealSenseCameraConfig ---- @@ -170,36 +236,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,29 +286,29 @@ struct Outs { #[derive(Module)] #[module(name = "realsense", setup = start, teardown = stop)] struct RealSense { - #[output(encode = Image::encode)] + #[output(encode = encode_cdr)] color_image: Output, - #[output(encode = Image::encode)] + #[output(encode = encode_cdr)] depth_image: Output, - #[output(encode = Image::encode)] + #[output(encode = encode_cdr)] infrared_left: Output, - #[output(encode = Image::encode)] + #[output(encode = encode_cdr)] infrared_right: Output, - #[output(encode = Imu::encode)] + #[output(encode = encode_cdr)] imu: Output, - #[output(encode = ImuInfo::encode)] + #[output(encode = encode_cdr)] imu_info: Output, - #[output(encode = PointCloud2::encode)] + #[output(encode = encode_cdr)] pointcloud: Output, - #[output(encode = CameraInfo::encode)] + #[output(encode = encode_cdr)] camera_info: Output, - #[output(encode = CameraInfo::encode)] + #[output(encode = encode_cdr)] depth_camera_info: Output, - #[output(encode = CameraInfo::encode)] + #[output(encode = encode_cdr)] infrared_left_camera_info: Output, - #[output(encode = CameraInfo::encode)] + #[output(encode = encode_cdr)] infrared_right_camera_info: Output, - #[tf] + #[tf(decode = decode_cdr_tf, encode = encode_cdr_tf)] tf: Tf, #[config] config: Config, @@ -358,12 +394,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 +430,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 +451,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 +717,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; } } } @@ -937,10 +977,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 +1008,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 +1060,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 +1414,134 @@ 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 canonical_tf_preserves_frames_math_and_normalized_time_in_both_endians() { + for ts in [3.25, -0.25, 1.0 - 1e-12] { + let transform = + Transform::new("map", "camera", ts, Isometry3::translation(1.0, 2.0, 3.0)); + let bytes = encode_cdr_tf(&[transform]).unwrap(); + assert_eq!(&bytes[..4], &[0, 1, 0, 0]); + let message: TFMessage = cdr::decode(&bytes).unwrap(); + assert!(message.transforms[0].header.stamp.nanosec < 1_000_000_000); + for payload in [bytes, cdr::encode_endian(&message, false).unwrap()] { + let decoded = decode_cdr_tf(&payload).unwrap(); + assert_eq!(decoded.len(), 1); + assert_eq!(decoded[0].parent, "map"); + assert_eq!(decoded[0].child, "camera"); + assert!((decoded[0].ts - ts).abs() <= 1e-9); + assert_eq!(decoded[0].translation(), Vector3::new(1.0, 2.0, 3.0)); + } + } + let bytes = + encode_cdr_tf(&[Transform::new("map", "camera", 3.25, Isometry3::identity())]).unwrap(); + export_fixture("tf.cdr", &cdr::decode::(&bytes).unwrap()); + assert!(decode_cdr_tf(b"legacy or malformed bytes").is_err()); + } + + #[test] + fn canonical_tf_keeps_valid_edges_when_one_quaternion_is_invalid() { + let bytes = + encode_cdr_tf(&[Transform::new("map", "camera", 3.25, Isometry3::identity())]).unwrap(); + let mut message: TFMessage = cdr::decode(&bytes).unwrap(); + let mut invalid = message.transforms[0].clone(); + invalid.child_frame_id = "invalid".into(); + invalid.transform.rotation = QuaternionMsg { + x: 0.0, + y: 0.0, + z: 0.0, + w: 0.0, + }; + message.transforms.push(invalid); + let decoded = decode_cdr_tf(&cdr::encode(&message).unwrap()).unwrap(); + assert_eq!(decoded.len(), 1); + assert_eq!(decoded[0].child, "camera"); + } + + #[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(&encode_cdr(&cloud)).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 +1574,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 +1595,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 +1631,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..9637a9cb8af 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_type_hints if sys.version_info >= (3, 11): import tomllib @@ -64,3 +65,7 @@ def ports(cls: type) -> set[str]: 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..2a24e5e8037 --- /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.messages.sensor_msgs.msg import CameraInfo +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.msgs.geometry import transform_matrix +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..aefc45a0e58 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.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.msgs.camera_info import camera_info_from_fov +from dimos.msgs.image import image_view 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.msgs.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..a070d011757 100644 --- a/dimos/hardware/sensors/camera/v4l2/rust/Cargo.lock +++ b/dimos/hardware/sensors/camera/v4l2/rust/Cargo.lock @@ -226,6 +226,20 @@ name = "bytemuck" version = "1.25.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "95832e849adfb21180ccb6826a99da14e5d266ae5c2e668e1602cf234f153797" +dependencies = [ + "bytemuck_derive", +] + +[[package]] +name = "bytemuck_derive" +version = "1.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6a1f896587b6f2c069c73d2f0913e2d590c3990285cd2f0b6aa02b786b4c679c" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.4", +] [[package]] name = "byteorder" @@ -554,10 +568,21 @@ dependencies = [ "subtle", ] +[[package]] +name = "dimos-generated-messages" +version = "0.0.14" +dependencies = [ + "heapless", + "re_cdr", + "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", @@ -567,13 +592,13 @@ dependencies = [ [[package]] name = "dimos-module" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" dependencies = [ "dimos-lcm", "dimos-module-macros", "lcm-msgs", "nalgebra", "rayon", + "re_cdr", "serde", "serde_json", "tokio", @@ -587,7 +612,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 +624,8 @@ name = "dimos-v4l2-camera" version = "0.1.0" dependencies = [ "cc", + "dimos-generated-messages", "dimos-module", - "lcm-msgs", "libc", "serde", "tokio", @@ -947,6 +971,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" @@ -989,6 +1022,23 @@ version = "0.17.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ed5909b6e89a2db4456e54cd5f673791d7eca6732202bbf2a9cc504fe2f9b84a" +[[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" version = "0.5.3" @@ -1381,7 +1431,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 +1972,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" @@ -2160,6 +2220,18 @@ dependencies = [ "yasna", ] +[[package]] +name = "re_cdr" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bba10c21433b597481f366b978cdf2617e56785b38cb0dcc3833c6b8ad7e061a" +dependencies = [ + "bytemuck", + "byteorder", + "serde", + "thiserror 2.0.20", +] + [[package]] name = "redox_syscall" version = "0.5.18" @@ -2276,6 +2348,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 +2605,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" diff --git a/dimos/hardware/sensors/camera/v4l2/rust/Cargo.toml b/dimos/hardware/sensors/camera/v4l2/rust/Cargo.toml index 554b02f3566..a42c8adf6b1 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" } -lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } +dimos-module = { path = "../../../../../../native/rust/dimos-module", features = ["cdr"] } +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..677538f3a27 100644 --- a/dimos/hardware/sensors/camera/v4l2/rust/crate-hashes.json +++ b/dimos/hardware/sensors/camera/v4l2/rust/crate-hashes.json @@ -1,6 +1,4 @@ { - "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=" } \ 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..153a8432ff2 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/3d68b9a7966fd7e447642bb72f3dc8ee8579f865"; + sha256 = "288b8b2bc9b00a3ba8cc58efc6340001b9eca1b9ba9eff01e94ebd487da7d304"; + }; + 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..d31d89213a6 100644 --- a/dimos/hardware/sensors/camera/v4l2/rust/src/module.rs +++ b/dimos/hardware/sensors/camera/v4l2/rust/src/module.rs @@ -17,15 +17,54 @@ 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; +fn encode_cdr(message: &CompressedImage) -> Vec { + cdr::encode(message).expect("serialize a valid compressed camera frame") +} + +#[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 = encode_cdr(&message); + 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 +95,7 @@ pub struct Config { #[derive(Module)] #[module(name = "v4l2_camera", setup = start)] pub struct V4L2Camera { - #[output(encode = CompressedImage::encode)] + #[output(encode = encode_cdr)] jpeg_out: Output, #[config] @@ -115,7 +154,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 +193,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..ce25d2b5644 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.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.msgs.camera_info import camera_info_from_fov +from dimos.msgs.image import image_from_array +from dimos.msgs.time import header_now 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..6308b700503 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.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.msgs.camera_info import camera_info_with_stamp +from dimos.msgs.image import image_from_array +from dimos.msgs.pointcloud import pointcloud_from_rgbd, voxel_downsample_cloud +from dimos.msgs.time import time_from_nanoseconds 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..2ee92c2341f 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.msgs.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..389d902e468 --- /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.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.msgs.geometry import compose_transforms, inverse_transform + + +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..07aefe3c18e --- /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.messages.geometry_msgs.msg import Transform, Vector3 +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.msgs.geometry import quaternion_from_euler, transform_matrix +from dimos.msgs.time import time_from_nanoseconds, to_nanoseconds + + +@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/memory/module.py b/dimos/memory/module.py index a5867f0c53c..06c78cabc9a 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.messages.tf2_msgs.msg import TFMessage +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.time import to_seconds 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..f429dc62a93 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.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 +from dimos.msgs.image import image_to_rgb +from dimos.msgs.time import to_seconds 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..022a442ebf7 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.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 +from dimos.msgs.image import image_to_rgb +from dimos.msgs.time import to_seconds 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..8cabb1afdf4 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.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 +from dimos.msgs.image import image_to_rgb +from dimos.msgs.time import to_seconds 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..c85932f9176 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.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 +from dimos.msgs.image import image_to_rgb +from dimos.msgs.time import to_seconds 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..a51f9778ba4 --- /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.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 +from dimos.msgs.image import image_from_array, image_view + + +@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..3ce7331eec3 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.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.msgs.image import image_to_bgr +from dimos.msgs.time import to_seconds 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..b65977283c0 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.messages.sensor_msgs.msg import Image from dimos.models.base import default_torch_device -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_to_bgr, image_to_jpeg 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..ac079411bb5 --- /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.models.segmentation import edge_tam +from dimos.msgs.image import image_from_array +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..5d2efaffa50 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.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.msgs.image import image_from_array, image_to_bgr +from dimos.msgs.time import time_from_seconds, to_seconds 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..fd075227d27 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.messages.sensor_msgs.msg import Image +from dimos.msgs.image import image_to_bgr 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..98f9db3fa84 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.messages.sensor_msgs.msg import Image +from dimos.msgs.image import image_from_array, image_view +from dimos.msgs.time import header_now, to_seconds 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..ea61a786fb2 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.messages.sensor_msgs.msg import Image from dimos.models.vl.base import VlModel, VlModelConfig -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_from_array, image_to_jpeg +from dimos.msgs.time import header_now # 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..69bcff02343 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.models.vl.qwen import QwenVlModel -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_from_array +from dimos.msgs.time import header_now 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/msgs/camera_info.py b/dimos/msgs/camera_info.py new file mode 100644 index 00000000000..c8040990805 --- /dev/null +++ b/dimos/msgs/camera_info.py @@ -0,0 +1,178 @@ +# 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. + +"""Calibration construction and array access for generated ROS CameraInfo values.""" + +from copy import deepcopy +import math +from pathlib import Path +import re +from typing import Any, Literal + +import numpy as np +from numpy.typing import NDArray +import yaml + +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 + + +def _dimensions(width: int, height: int) -> None: + if any( + isinstance(value, bool) or not isinstance(value, int) or value <= 0 + for value in (width, height) + ): + raise ValueError("Calibration width and height must be positive integers") + + +def camera_info_from_intrinsics( + fx: float, fy: float, cx: float, cy: float, width: int, height: int, *, header: Header +) -> CameraInfo: + """Construct an undistorted pinhole calibration with an explicit source header.""" + _dimensions(width, height) + if not all(math.isfinite(value) for value in (fx, fy, cx, cy)) or fx <= 0 or fy <= 0: + raise ValueError("Intrinsics must be finite with positive focal lengths") + return CameraInfo( + header=header, + width=width, + height=height, + distortion_model="plumb_bob", + d=np.asarray([0.0] * 5, dtype=np.float64), + k=np.array([fx, 0, cx, 0, fy, cy, 0, 0, 1], dtype=np.float64), + r=np.array([1, 0, 0, 0, 1, 0, 0, 0, 1], dtype=np.float64), + p=np.array([fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1, 0], dtype=np.float64), + binning_x=0, + binning_y=0, + roi=RegionOfInterest(x_offset=0, y_offset=0, height=0, width=0, do_rectify=False), + ) + + +def camera_info_from_fov( + fov_degrees: float, + width: int, + height: int, + *, + header: Header, + axis: Literal["horizontal", "vertical"] = "vertical", +) -> CameraInfo: + """Construct a centered pinhole model with square pixels from an axis's FOV.""" + _dimensions(width, height) + if not math.isfinite(fov_degrees) or not 0 < fov_degrees < 180: + raise ValueError("Field of view must be finite and between 0 and 180 degrees") + if axis not in ("horizontal", "vertical"): + raise ValueError("Field-of-view axis must be horizontal or vertical") + extent = width if axis == "horizontal" else height + focal = extent / (2 * math.tan(math.radians(fov_degrees) / 2)) + return camera_info_from_intrinsics( + focal, focal, width / 2, height / 2, width, height, header=header + ) + + +def _matrix(data: dict[str, Any], name: str, rows: int, columns: int | None) -> list[float]: + matrix = data[name] + if not isinstance(matrix, dict) or matrix.get("rows") != rows: + raise ValueError(f"{name} must declare {rows} rows") + actual_columns = matrix.get("cols") + if ( + not isinstance(actual_columns, int) + or actual_columns < 0 + or (columns is not None and actual_columns != columns) + ): + raise ValueError(f"{name} has invalid columns") + values = matrix.get("data") + if not isinstance(values, list) or len(values) != rows * actual_columns: + raise ValueError(f"{name} data length does not match its dimensions") + converted = [float(value) for value in values] + if not all(math.isfinite(value) for value in converted): + raise ValueError(f"{name} must contain finite values") + return converted + + +def camera_info_from_yaml(path: str | Path, *, header: Header) -> CameraInfo: + """Read a ROS calibration YAML file, validating declared matrix dimensions.""" + with Path(path).open() as source: + data = yaml.safe_load(source) + if not isinstance(data, dict): + raise TypeError("Calibration YAML must contain a mapping") + width, height = data["image_width"], data["image_height"] + _dimensions(width, height) + return CameraInfo( + header=header, + width=width, + height=height, + distortion_model=data["distortion_model"], + k=np.asarray(_matrix(data, "camera_matrix", 3, 3), dtype=np.float64), + d=np.asarray(_matrix(data, "distortion_coefficients", 1, None), dtype=np.float64), + r=np.asarray(_matrix(data, "rectification_matrix", 3, 3), dtype=np.float64), + p=np.asarray(_matrix(data, "projection_matrix", 3, 4), dtype=np.float64), + binning_x=0, + binning_y=0, + roi=RegionOfInterest(x_offset=0, y_offset=0, height=0, width=0, do_rectify=False), + ) + + +def intrinsic_matrix(message: CameraInfo) -> NDArray[np.float64]: + """Copy the 3x3 intrinsic matrix into independently mutable NumPy storage.""" + return np.array(message.k, dtype=np.float64, copy=True).reshape(3, 3) + + +def camera_info_with_stamp(message: CameraInfo, stamp: Time) -> CameraInfo: + """Copy a calibration with an exact source stamp, without mutating prior publishes.""" + result = deepcopy(message) + result.header.stamp = deepcopy(stamp) + return result + + +class CalibrationProvider: + """Lazily load named YAML calibrations as plain generated CameraInfo values. + + Both SingleWebcam and single_webcam resolve single_webcam.yaml. This + helper lives outside generated messages; calibration files stay local. + """ + + def __init__(self, calibration_dir: str | Path, *, frame_id: str = "camera_optical") -> None: + self._calibration_dir = Path(calibration_dir) + self._frame_id = frame_id + self._cache: dict[str, CameraInfo] = {} + + @staticmethod + def _to_snake_case(name: str) -> str: + first = re.sub("(.)([A-Z][a-z]+)", r"\1_\2", name) + return re.sub("([a-z0-9])([A-Z])", r"\1_\2", first).lower() + + def __getattr__(self, name: str) -> CameraInfo: + if not re.fullmatch(r"[A-Za-z][A-Za-z0-9_]*", name): + raise AttributeError(f"No calibration file found for: {name}") + snake = self._to_snake_case(name) + for key in (name, snake): + if key in self._cache: + return self._cache[key] + for key in (name, snake): + path = self._calibration_dir / f"{key}.yaml" + if path.is_file(): + value = camera_info_from_yaml( + path, header=Header(frame_id=self._frame_id, stamp=Time(sec=0, nanosec=0)) + ) + self._cache[name] = value + self._cache[snake] = value + return value + raise AttributeError(f"No calibration file found for: {name}") + + def __dir__(self) -> list[str]: + names: set[str] = set() + for path in self._calibration_dir.glob("*.yaml"): + names.add(path.stem) + names.add("".join(word.capitalize() for word in path.stem.split("_"))) + return sorted(names) diff --git a/dimos/msgs/geometry.py b/dimos/msgs/geometry.py new file mode 100644 index 00000000000..2ab85421267 --- /dev/null +++ b/dimos/msgs/geometry.py @@ -0,0 +1,376 @@ +# 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. + +"""Geometry operations on generated ROS value types.""" + +from collections.abc import Sequence +from copy import deepcopy +import math + +import numpy as np +from numpy.typing import NDArray +from scipy.spatial.transform import Rotation + +from dimos.messages.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Vector3, + Wrench, +) +from dimos.messages.nav_msgs.msg import Odometry +from dimos.messages.std_msgs.msg import Header + + +def yaw(rotation: Quaternion) -> float: + """Return rotation about Z in radians (ROS XYZW quaternion convention).""" + return math.atan2( + 2 * (rotation.w * rotation.z + rotation.x * rotation.y), + 1 - 2 * (rotation.y * rotation.y + rotation.z * rotation.z), + ) + + +def transform_from_odometry(message: Odometry) -> TransformStamped: + """Copy the pose and declared frames into TF, preserving the exact source stamp.""" + position = message.pose.pose.position + return TransformStamped( + header=deepcopy(message.header), + child_frame_id=message.child_frame_id, + transform=Transform( + translation=Vector3(x=position.x, y=position.y, z=position.z), + rotation=deepcopy(message.pose.pose.orientation), + ), + ) + + +def quaternion_from_euler(roll: float, pitch: float, yaw: float) -> Quaternion: + """Convert fixed-axis XYZ angles in radians to a normalized ROS quaternion.""" + if not all(math.isfinite(value) for value in (roll, pitch, yaw)): + raise ValueError("Euler angles must be finite") + x, y, z, w = Rotation.from_euler("xyz", [roll, pitch, yaw]).as_quat() + return Quaternion(x=x, y=y, z=z, w=w) + + +def _rotation(quaternion: Quaternion) -> Rotation: + values = [quaternion.x, quaternion.y, quaternion.z, quaternion.w] + if not all(math.isfinite(value) for value in values): + raise ValueError("Quaternion components must be finite") + # SciPy rejects zero norm and normalizes non-unit input. + return Rotation.from_quat(values) + + +def _translation(transform: Transform) -> NDArray[np.float64]: + translation = transform.translation + values = np.array([translation.x, translation.y, translation.z], dtype=np.float64) + if not np.isfinite(values).all(): + raise ValueError("Translation components must be finite") + return values + + +def _transform(translation: NDArray[np.float64], rotation: Rotation) -> Transform: + x, y, z, w = rotation.as_quat() + return Transform( + translation=Vector3(x=translation[0], y=translation[1], z=translation[2]), + rotation=Quaternion(x=x, y=y, z=z, w=w), + ) + + +def transform_matrix(transform: Transform) -> NDArray[np.float64]: + """Copy a generated transform into a homogeneous 4-by-4 matrix.""" + matrix = np.eye(4, dtype=np.float64) + matrix[:3, :3] = _rotation(transform.rotation).as_matrix() + matrix[:3, 3] = _translation(transform) + return matrix + + +def compose_transforms(first: TransformStamped, second: TransformStamped) -> TransformStamped: + """Compose A←B and B←C, preserving A's exact source stamp in the A←C result. + + Inputs are copied into a new value. The shared frame must match; callers + querying a time-varying chain are responsible for choosing each edge's time. + """ + if first.child_frame_id != second.header.frame_id: + raise ValueError( + f"Cannot compose frames {first.child_frame_id!r} and {second.header.frame_id!r}" + ) + rotation = _rotation(first.transform.rotation) + translation = _translation(first.transform) + rotation.apply(_translation(second.transform)) + return TransformStamped( + header=deepcopy(first.header), + child_frame_id=second.child_frame_id, + transform=_transform(translation, rotation * _rotation(second.transform.rotation)), + ) + + +def inverse_transform(message: TransformStamped) -> TransformStamped: + """Return B←A from A←B, swapping frame names and preserving the exact stamp.""" + rotation = _rotation(message.transform.rotation).inv() + return TransformStamped( + header=Header(stamp=deepcopy(message.header.stamp), frame_id=message.child_frame_id), + child_frame_id=message.header.frame_id, + transform=_transform(-rotation.apply(_translation(message.transform)), rotation), + ) + + +def pose_from_transform(message: TransformStamped) -> PoseStamped: + """Copy the child frame's pose in the parent frame into a generated PoseStamped.""" + translation = message.transform.translation + return PoseStamped( + header=deepcopy(message.header), + pose=Pose( + position=Point(x=translation.x, y=translation.y, z=translation.z), + orientation=deepcopy(message.transform.rotation), + ), + ) + + +def transform_from_pose(message: PoseStamped, *, child_frame_id: str) -> TransformStamped: + """Copy a stamped pose into TF with an explicit child and unchanged source header.""" + if not isinstance(message, PoseStamped): + raise TypeError("Expected a generated PoseStamped") + position = message.pose.position + return TransformStamped( + header=deepcopy(message.header), + child_frame_id=child_frame_id, + transform=Transform( + translation=Vector3(x=position.x, y=position.y, z=position.z), + rotation=deepcopy(message.pose.orientation), + ), + ) + + +def pose_matrix(pose: Pose) -> NDArray[np.float64]: + """Copy a generated pose into a homogeneous matrix, rejecting invalid geometry.""" + values = np.array([pose.position.x, pose.position.y, pose.position.z], dtype=np.float64) + if not np.isfinite(values).all(): + raise ValueError("Position components must be finite") + result = np.eye(4, dtype=np.float64) + result[:3, :3] = _rotation(pose.orientation).as_matrix() + result[:3, 3] = values + return result + + +def quaternion_from_matrix(matrix: NDArray[np.float64]) -> Quaternion: + """Convert a finite proper 3-by-3 rotation matrix into a generated quaternion.""" + values = np.asarray(matrix, dtype=np.float64) + if values.shape != (3, 3) or not np.isfinite(values).all(): + raise ValueError("Rotation matrix must be finite and 3-by-3") + if not np.allclose(values.T @ values, np.eye(3), atol=1e-6, rtol=0.0) or not np.isclose( + np.linalg.det(values), 1.0, atol=1e-6, rtol=0.0 + ): + raise ValueError("Rotation matrix must be orthonormal with determinant +1") + x, y, z, w = Rotation.from_matrix(values).as_quat() + return Quaternion(x=x, y=y, z=z, w=w) + + +def pose_from_matrix(matrix: NDArray[np.float64]) -> Pose: + """Copy a finite rigid 4-by-4 transform into a generated pose.""" + values = np.asarray(matrix, dtype=np.float64) + if values.shape != (4, 4) or not np.isfinite(values).all(): + raise ValueError("Pose matrix must be finite and 4-by-4") + if not np.allclose(values[3], [0.0, 0.0, 0.0, 1.0], atol=1e-9, rtol=0.0): + raise ValueError("Pose matrix must have homogeneous last row [0, 0, 0, 1]") + return Pose( + position=Point(x=values[0, 3], y=values[1, 3], z=values[2, 3]), + orientation=quaternion_from_matrix(values[:3, :3]), + ) + + +def transform_from_matrix(matrix: NDArray[np.float64]) -> Transform: + """Copy a finite rigid homogeneous matrix into a generated transform.""" + pose = pose_from_matrix(matrix) + return Transform( + translation=Vector3(x=pose.position.x, y=pose.position.y, z=pose.position.z), + rotation=deepcopy(pose.orientation), + ) + + +def quaternion_euler(rotation: Quaternion) -> tuple[float, float, float]: + """Return fixed-axis XYZ Euler angles in radians for a generated quaternion.""" + roll, pitch, yaw = _rotation(rotation).as_euler("xyz") + return float(roll), float(pitch), float(yaw) + + +def translate_pose_local(pose: Pose, offset: Vector3) -> Pose: + """Copy a pose displaced by an offset expressed in its local axes.""" + matrix = pose_matrix(pose) + values = np.array([offset.x, offset.y, offset.z], dtype=np.float64) + if not np.isfinite(values).all(): + raise ValueError("Offset components must be finite") + delta = matrix[:3, :3] @ values + return Pose( + position=Point( + x=pose.position.x + delta[0], y=pose.position.y + delta[1], z=pose.position.z + delta[2] + ), + orientation=deepcopy(pose.orientation), + ) + + +def point_distance(first: Point, second: Point) -> float: + """Euclidean distance between generated position values.""" + return math.dist((first.x, first.y, first.z), (second.x, second.y, second.z)) + + +def quaternion_angle(first: Quaternion, second: Quaternion) -> float: + """Return the shortest angular separation in radians, normalizing inputs.""" + return float((_rotation(first).inv() * _rotation(second)).magnitude()) + + +def wrench_from_array(values: Sequence[float] | NDArray[np.float64]) -> Wrench: + """Copy force in N and torque in Nm from [fx, fy, fz, tx, ty, tz].""" + array = np.asarray(values, dtype=np.float64) + if array.shape != (6,): + raise ValueError("Expected 6 elements [fx, fy, fz, tx, ty, tz]") + return Wrench( + force=Vector3(x=array[0], y=array[1], z=array[2]), + torque=Vector3(x=array[3], y=array[4], z=array[5]), + ) + + +def wrench_array(message: Wrench) -> NDArray[np.float64]: + """Return an independent numeric force/torque array for arithmetic.""" + return np.array( + [ + message.force.x, + message.force.y, + message.force.z, + message.torque.x, + message.torque.y, + message.torque.z, + ], + dtype=np.float64, + ) + + +def vector_array(value: Vector3 | Point) -> NDArray[np.float64]: + """Copy a generated spatial vector/point into an independent XYZ array.""" + return np.array([value.x, value.y, value.z], dtype=np.float64) + + +def vector_from_array(values: Sequence[float] | NDArray[np.float64]) -> Vector3: + """Copy exactly three coordinates into a generated vector.""" + array = np.asarray(values, dtype=np.float64) + if array.shape != (3,): + raise ValueError("Expected 3 vector components") + return Vector3(x=array[0], y=array[1], z=array[2]) + + +def normalized_vector(value: Vector3) -> Vector3: + """Return a unit vector; zero length remains zero.""" + array = vector_array(value) + length = float(np.linalg.norm(array)) + return vector_from_array(array / length) if length else Vector3(x=0.0, y=0.0, z=0.0) + + +def quaternion_array(value: Quaternion) -> NDArray[np.float64]: + """Copy XYZW components without normalizing their declared values.""" + return np.array([value.x, value.y, value.z, value.w], dtype=np.float64) + + +def quaternion_from_array(values: Sequence[float] | NDArray[np.float64]) -> Quaternion: + """Copy exactly four XYZW components into a generated quaternion.""" + array = np.asarray(values, dtype=np.float64) + if array.shape != (4,): + raise ValueError("Quaternion requires exactly 4 components") + return Quaternion(x=float(array[0]), y=float(array[1]), z=float(array[2]), w=float(array[3])) + + +def quaternion_product(first: Quaternion, second: Quaternion) -> Quaternion: + """Hamilton product, retaining non-unit magnitudes and multiplication order.""" + if not isinstance(first, Quaternion) or not isinstance(second, Quaternion): + raise TypeError("Cannot multiply Quaternion with a non-quaternion value") + a, b = quaternion_array(first), quaternion_array(second) + xyz = a[3] * b[:3] + b[3] * a[:3] + np.cross(a[:3], b[:3]) + return quaternion_from_array(np.append(xyz, a[3] * b[3] - np.dot(a[:3], b[:3]))) + + +def quaternion_conjugate(value: Quaternion) -> Quaternion: + """Negate the imaginary components, preserving the scalar component.""" + return Quaternion(x=-value.x, y=-value.y, z=-value.z, w=value.w) + + +def quaternion_inverse(value: Quaternion) -> Quaternion: + """Algebraic inverse, including non-unit quaternions.""" + array = quaternion_array(value) + squared = float(np.dot(array, array)) + if squared == 0: + raise ZeroDivisionError("Cannot invert zero quaternion") + return quaternion_from_array(quaternion_array(quaternion_conjugate(value)) / squared) + + +def normalized_quaternion(value: Quaternion) -> Quaternion: + """Return unit components without changing sign; reject zero norm.""" + array = quaternion_array(value) + length = float(np.linalg.norm(array)) + if length == 0: + raise ZeroDivisionError("Cannot normalize zero quaternion") + return quaternion_from_array(array / length) + + +def rotate_vector(rotation: Quaternion, value: Vector3) -> Vector3: + """Rotate an XYZ vector using a normalized spatial rotation.""" + return vector_from_array(_rotation(rotation).apply(vector_array(value))) + + +def point_from_array(values: Sequence[float] | NDArray[np.float64]) -> Point: + """Copy exactly three Cartesian coordinates into a generated point.""" + vector = vector_from_array(values) + return Point(x=vector.x, y=vector.y, z=vector.z) + + +def compose_poses(first: Pose, second: Pose) -> Pose: + """Compose parent/local poses without mutating inputs or normalizing stored quaternions.""" + offset = _rotation(first.orientation).apply(vector_array(second.position)) + return Pose( + position=point_from_array(vector_array(first.position) + offset), + orientation=quaternion_product(first.orientation, second.orientation), + ) + + +def compose_transform_values(first: Transform, second: Transform) -> Transform: + """Compose unframed spatial transforms in parent/local order.""" + if not isinstance(first, Transform) or not isinstance(second, Transform): + raise TypeError("Expected generated Transform values") + return Transform( + translation=vector_from_array( + vector_array(first.translation) + + _rotation(first.rotation).apply(vector_array(second.translation)) + ), + rotation=quaternion_product(first.rotation, second.rotation), + ) + + +def transform_pose_local(pose: Pose, transform: Transform) -> Pose: + """Apply an offset expressed in a pose's local axes.""" + return compose_poses( + pose, + Pose( + position=point_from_array(vector_array(transform.translation)), + orientation=transform.rotation, + ), + ) + + +def relative_pose_transform(first: Pose, second: Pose) -> Transform: + """Find the local transform taking the first pose to the second.""" + rotation = quaternion_inverse(first.orientation) + return Transform( + translation=vector_from_array( + _rotation(rotation).apply(vector_array(second.position) - vector_array(first.position)) + ), + rotation=quaternion_product(rotation, second.orientation), + ) diff --git a/dimos/msgs/image.py b/dimos/msgs/image.py new file mode 100644 index 00000000000..16548a5f175 --- /dev/null +++ b/dimos/msgs/image.py @@ -0,0 +1,311 @@ +# 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. + +"""Array views and compression for generated ROS image messages.""" + +from __future__ import annotations + +from pathlib import Path +from typing import Any + +import cv2 +import imagecodecs +import numpy as np +from numpy.typing import NDArray +from PIL import Image as PILImage + +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.sensor_msgs.msg import CompressedImage, Image +from dimos.messages.std_msgs.msg import Header + +_FORMATS = { + "rgb8": ("u1", 3), + "bgr8": ("u1", 3), + "rgba8": ("u1", 4), + "bgra8": ("u1", 4), + "mono8": ("u1", 1), + "mono16": ("u2", 1), + "16UC1": ("u2", 1), + "32FC1": ("f4", 1), + "64FC1": ("f8", 1), + "16SC1": ("i2", 1), + "32FC3": ("f4", 3), +} + + +def image_from_array(pixels: NDArray[Any], *, encoding: str, header: Header | None = None) -> Image: + """Copy an array into a generated image with an explicit pixel encoding.""" + if encoding not in _FORMATS: + raise ValueError(f"unsupported image encoding {encoding!r}") + kind, channels = _FORMATS[encoding] + expected = np.dtype(kind) + if pixels.dtype.kind != expected.kind or pixels.dtype.itemsize != expected.itemsize: + raise ValueError(f"array dtype does not match {encoding}") + if (channels == 1 and pixels.ndim != 2) or ( + channels != 1 and (pixels.ndim != 3 or pixels.shape[2] != channels) + ): + raise ValueError(f"array shape does not match {encoding}") + contiguous = np.array(pixels, copy=True, order="C") + return Image( + header=header if header is not None else Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + width=pixels.shape[1], + height=pixels.shape[0], + encoding=encoding, + is_bigendian=int(not pixels.dtype.isnative if np.little_endian else pixels.dtype.isnative), + step=pixels.shape[1] * channels * pixels.dtype.itemsize, + data=np.asarray(contiguous.view(np.uint8).ravel(), dtype=np.uint8), + ) + + +def image_view(msg: Image) -> NDArray[Any]: + """Borrow a read-only array, retaining the message and respecting row padding/endian.""" + if msg.encoding not in _FORMATS: + raise ValueError(f"unsupported image encoding {msg.encoding!r}") + kind, channels = _FORMATS[msg.encoding] + dtype = np.dtype((">" if msg.is_bigendian else "<") + kind) + row_bytes = msg.width * channels * dtype.itemsize + if msg.step < row_bytes or len(msg.data) != msg.height * msg.step: + raise ValueError("image data length/step does not match its dimensions") + shape = (msg.height, msg.width) if channels == 1 else (msg.height, msg.width, channels) + strides = ( + (msg.step, dtype.itemsize) + if channels == 1 + else (msg.step, channels * dtype.itemsize, dtype.itemsize) + ) + view = np.ndarray(shape, dtype=dtype, buffer=msg.data, strides=strides) + view.setflags(write=False) + return view + + +def image_to_jpeg(msg: Image, quality: int = 75) -> bytes: + """Encode an 8-bit gray or color image using OpenCV's JPEG encoder.""" + + if isinstance(quality, bool) or not 0 <= quality <= 100: + raise ValueError("JPEG quality must be in 0..100") + pixels = image_view(msg) + conversions = { + "rgb8": cv2.COLOR_RGB2BGR, + "rgba8": cv2.COLOR_RGBA2BGR, + "bgra8": cv2.COLOR_BGRA2BGR, + } + if msg.encoding in conversions: + pixels = cv2.cvtColor(pixels, conversions[msg.encoding]) + elif msg.encoding not in ("bgr8", "mono8"): + raise ValueError(f"JPEG requires 8-bit color or gray, got {msg.encoding!r}") + ok, encoded = cv2.imencode(".jpg", pixels, [cv2.IMWRITE_JPEG_QUALITY, quality]) + if not ok: + raise ValueError("JPEG encoding failed") + return bytes(encoded) + + +def compressed_image_from_image( + message: Image, + quality: int = 75, + *, + format: str = "jpeg", + max_width: int | None = None, + effort: int | None = None, +) -> CompressedImage: + """Compress generated pixels, retaining the exact header. + + PNG preserves uint16 depth. JXL preserves uint16/float32 depth losslessly; + it requires imagecodecs and is an explicit codec extension for consumers. + """ + if not isinstance(message, Image): + raise TypeError("compression expects a generated Image") + if max_width is not None: + message, _ = image_resize_to_fit(message, max_width, max_width) + if format == "jpeg": + output_encoding = "mono8" if message.encoding == "mono8" else "bgr8" + data = image_to_jpeg(message, quality=quality) + elif format == "png": + pixels = image_view(message) + if pixels.dtype.kind != "u" or pixels.dtype.itemsize not in (1, 2): + raise ValueError("PNG cannot encode floating-point depth") + if message.encoding in ("rgb8", "rgba8"): + code = cv2.COLOR_RGB2BGR if message.encoding == "rgb8" else cv2.COLOR_RGBA2BGRA + pixels = cv2.cvtColor(pixels, code) + pixels = np.ascontiguousarray(pixels, dtype=pixels.dtype.newbyteorder("=")) + ok, encoded = cv2.imencode(".png", pixels) + if not ok: + raise ValueError("PNG encoding failed") + data = bytes(encoded) + output_encoding = {"rgb8": "bgr8", "rgba8": "bgra8"}.get(message.encoding, message.encoding) + elif format == "jxl": + pixels = image_view(message) + if pixels.dtype.kind not in ("u", "f") or ( + pixels.dtype.kind, + pixels.dtype.itemsize, + ) not in (("u", 1), ("u", 2), ("f", 4)): + raise ValueError(f"JXL cannot encode dtype {pixels.dtype}") + if message.encoding == "bgr8": + pixels = pixels[..., ::-1] + elif message.encoding == "bgra8": + pixels = pixels[..., [2, 1, 0, 3]] + pixels = np.ascontiguousarray(pixels, dtype=pixels.dtype.newbyteorder("=")) + if pixels.dtype == np.uint8: + data = bytes(imagecodecs.jpegxl_encode(pixels, level=quality, effort=effort or 3)) + else: + data = bytes(imagecodecs.jpegxl_encode(pixels, lossless=True, effort=effort or 1)) + output_encoding = {"bgr8": "rgb8", "bgra8": "rgba8"}.get(message.encoding, message.encoding) + else: + raise ValueError(f"unsupported compression format {format!r}") + return CompressedImage( + header=message.header, + format=f"{message.encoding}; {format} compressed {output_encoding}", + data=np.asarray(np.frombuffer(data, dtype=np.uint8).copy(), dtype=np.uint8), + ) + + +def image_from_compressed(message: CompressedImage) -> Image: + """Decode JPEG/PNG or the JXL extension into explicitly encoded generated pixels.""" + + format_name = message.format.lower() + if "jxl" in format_name: + jxl_pixels = imagecodecs.jpegxl_decode(bytes(message.data)) + if jxl_pixels.ndim == 2: + encoding = { + np.dtype("float32"): "32FC1", + np.dtype("uint16"): "mono16", + np.dtype("uint8"): "mono8", + }.get(jxl_pixels.dtype) + elif jxl_pixels.ndim == 3 and jxl_pixels.shape[2] in (3, 4): + encoding = "rgb8" if jxl_pixels.shape[2] == 3 else "rgba8" + else: + encoding = None + if encoding is None: + raise ValueError("unsupported decoded JXL layout") + if encoding == "mono16" and format_name.startswith("16uc1;"): + encoding = "16UC1" + return image_from_array(jxl_pixels, encoding=encoding, header=message.header) + if not any(name in format_name for name in ("jpeg", "jpg", "png")): + raise ValueError(f"unsupported compressed image format {message.format!r}") + pixels = cv2.imdecode(np.frombuffer(bytes(message.data), dtype=np.uint8), cv2.IMREAD_UNCHANGED) + if pixels is None: + raise ValueError("invalid compressed image data") + if pixels.ndim == 2: + encoding = "mono16" if pixels.dtype.itemsize == 2 else "mono8" + elif pixels.ndim == 3 and pixels.shape[2] == 3: + encoding = "bgr8" + elif pixels.ndim == 3 and pixels.shape[2] == 4: + encoding = "bgra8" + else: + raise ValueError("unsupported decoded image layout") + if encoding == "mono16" and format_name.startswith("16uc1;"): + encoding = "16UC1" + return image_from_array(pixels, encoding=encoding, header=message.header) + + +def image_brightness(message: Image) -> float: + """Sample mean pixel intensity in [0, 1] for 8/16-bit image encodings. + + Respect row padding and endian. Floating-point depth has no normalized + intensity scale and is rejected rather than treated as a color image. + """ + pixels = image_view(message) + if pixels.dtype.kind != "u": + raise ValueError("brightness requires unsigned integer pixels") + stride = max(1, max(message.height, message.width) // 256) + return float(pixels[::stride, ::stride].mean() / np.iinfo(pixels.dtype).max) + + +def image_sharpness(message: Image) -> float: + """Laplacian variance of an 8-bit visual image, downsampled to 160 pixels wide.""" + + pixels = image_view(message) + codes = { + "rgb8": cv2.COLOR_RGB2GRAY, + "bgr8": cv2.COLOR_BGR2GRAY, + "rgba8": cv2.COLOR_RGBA2GRAY, + "bgra8": cv2.COLOR_BGRA2GRAY, + } + if message.encoding == "mono8": + gray = pixels + elif message.encoding in codes: + gray = cv2.cvtColor(pixels, codes[message.encoding]) + else: + raise ValueError("Sharpness requires an 8-bit visual image encoding") + if gray.size == 0: + raise ValueError("Sharpness requires a nonempty image") + if message.width > 160: + gray = cv2.resize( + gray, + (160, max(1, round(message.height * 160 / message.width))), + interpolation=cv2.INTER_AREA, + ) + return float(cv2.Laplacian(gray, cv2.CV_64F).var()) + + +def image_to_bgr(message: Image) -> NDArray[np.uint8]: + """Return an independent BGR8 array for drawing or OpenCV color operations.""" + + pixels = image_view(message) + if message.encoding == "bgr8": + return pixels.copy() + codes = { + "rgb8": cv2.COLOR_RGB2BGR, + "rgba8": cv2.COLOR_RGBA2BGR, + "bgra8": cv2.COLOR_BGRA2BGR, + "mono8": cv2.COLOR_GRAY2BGR, + } + if message.encoding not in codes: + raise ValueError(f"Cannot convert {message.encoding!r} to BGR8") + return np.asarray(cv2.cvtColor(pixels, codes[message.encoding]), dtype=np.uint8) + + +def image_to_rgb(message: Image) -> NDArray[np.uint8]: + """Copy a display image as RGB8; 16-bit grayscale uses its high byte. + + Alpha is discarded. Floating-point depth needs an explicit visualization + scale and is rejected rather than assigning pixel values to meters. + """ + + pixels = image_view(message) + if message.encoding == "rgb8": + return pixels.copy() + if message.encoding in ("mono16", "16UC1"): + return np.asarray( + cv2.cvtColor((pixels / 256).astype(np.uint8), cv2.COLOR_GRAY2RGB), dtype=np.uint8 + ) + codes = { + "bgr8": cv2.COLOR_BGR2RGB, + "rgba8": cv2.COLOR_RGBA2RGB, + "bgra8": cv2.COLOR_BGRA2RGB, + "mono8": cv2.COLOR_GRAY2RGB, + } + if message.encoding not in codes: + raise ValueError(f"Cannot convert {message.encoding!r} to RGB8") + return np.asarray(cv2.cvtColor(pixels, codes[message.encoding]), dtype=np.uint8) + + +def image_resize_to_fit(message: Image, max_width: int, max_height: int) -> tuple[Image, float]: + """Downscale a generated image while preserving encoding, aspect and exact header.""" + + if min(max_width, max_height, message.width, message.height) <= 0: + raise ValueError("Image and target dimensions must be positive") + if message.width <= max_width and message.height <= max_height: + return message, 1.0 + scale = min(max_width / message.width, max_height / message.height) + pixels = cv2.resize( + image_view(message), + (max(1, int(message.width * scale)), max(1, int(message.height * scale))), + interpolation=cv2.INTER_LINEAR, + ) + return image_from_array(pixels, encoding=message.encoding, header=message.header), scale + + +def image_from_file(path: str | Path, *, header: Header | None = None) -> Image: + """Decode an image file as generated RGB8 with an explicit optional source header.""" + with PILImage.open(path) as image: + return image_from_array(np.asarray(image.convert("RGB")), encoding="rgb8", header=header) diff --git a/dimos/msgs/pointcloud.py b/dimos/msgs/pointcloud.py new file mode 100644 index 00000000000..b28135a2864 --- /dev/null +++ b/dimos/msgs/pointcloud.py @@ -0,0 +1,380 @@ +# 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. + +"""NumPy conversions for generated PointCloud2 messages.""" + +from copy import deepcopy +import math +from typing import Any + +from dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode +import numpy as np +from numpy.typing import NDArray +import open3d as o3d # type: ignore[import-untyped] + +from dimos.messages.geometry_msgs.msg import TransformStamped +from dimos.messages.sensor_msgs.msg import CameraInfo, Image, PointCloud2, PointField +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.camera_info import intrinsic_matrix +from dimos.msgs.geometry import transform_matrix +from dimos.msgs.image import image_to_rgb, image_view +from dimos.msgs.time import to_nanoseconds + +_FIELD_KINDS = {1: "i1", 2: "u1", 3: "i2", 4: "u2", 5: "i4", 6: "u4", 7: "f4", 8: "f8"} + + +def pointcloud_view(message: PointCloud2) -> NDArray[Any]: + """Borrow read-only structured (height, width) points, including row/field padding. + + Fields retain their declared counts and endianness. The returned array keeps + the message's storage alive. Use ``view.copy()`` for independent mutable data. + """ + if message.width and not message.point_step: + raise ValueError("nonempty point cloud requires a positive point_step") + if message.row_step < message.width * message.point_step: + raise ValueError("point cloud row_step is smaller than its row of points") + if len(message.data) != message.height * message.row_step: + raise ValueError("point cloud data length does not match height * row_step") + names: list[str] = [] + formats: list[Any] = [] + offsets: list[int] = [] + for field in message.fields: + if not field.name or field.name in names: + raise ValueError("point cloud field names must be nonempty and unique") + if field.datatype not in _FIELD_KINDS or not field.count: + raise ValueError(f"invalid point cloud datatype/count for {field.name!r}") + dtype = np.dtype((">" if message.is_bigendian else "<") + _FIELD_KINDS[field.datatype]) + if field.offset + dtype.itemsize * field.count > message.point_step: + raise ValueError(f"point cloud field {field.name!r} exceeds point_step") + names.append(field.name) + formats.append(dtype if field.count == 1 else (dtype, (field.count,))) + offsets.append(field.offset) + dtype = np.dtype( + {"names": names, "formats": formats, "offsets": offsets, "itemsize": message.point_step} + ) + view = np.ndarray( + (message.height, message.width), + dtype=dtype, + buffer=message.data, + strides=(message.row_step, message.point_step), + ) + view.setflags(write=False) + return view + + +def pointcloud_xyz(message: PointCloud2) -> NDArray[np.float64]: + """Copy scalar x/y/z fields into an N-by-3 float64 array, preserving point order.""" + points = pointcloud_view(message) + for name in ("x", "y", "z"): + if name not in (points.dtype.names or ()) or points[name].ndim != 2: + raise ValueError(f"point cloud requires a scalar {name!r} field") + return np.stack([points[name].ravel() for name in ("x", "y", "z")], axis=-1).astype(np.float64) + + +def pointcloud_from_xyz( + points: NDArray[Any], *, header: Header, stamps: NDArray[Any] | None = None +) -> PointCloud2: + """Copy Nx3 or HxWx3 coordinates into a tightly packed little-endian XYZ cloud. + + XYZ fields are float32. NaNs/infinities remain missing-point markers and set + is_dense=False; finite coordinates outside float32 range are rejected. + """ + values = np.asarray(points) + if values.ndim not in (2, 3) or values.shape[-1] != 3: + raise ValueError("XYZ coordinates must have shape (N, 3) or (H, W, 3)") + if values.dtype.kind not in "fiu": + raise ValueError("XYZ coordinates must be numeric") + finite = np.isfinite(values) + if np.any(np.abs(values[finite].astype(np.float64)) > np.finfo(np.float32).max): + raise ValueError("XYZ coordinates exceed float32 range") + packed = np.ascontiguousarray(values, dtype=" NDArray[np.float64] | None: + """Read per-point capture times; absence falls back to the header at the caller.""" + fields = [field for field in message.fields if field.name == "stamps"] + if not fields: + return None + if len(fields) != 1 or fields[0].datatype != PointField.FLOAT64 or fields[0].count != 1: + raise ValueError("Per-point stamps must be a scalar FLOAT64 field") + return np.asarray(pointcloud_view(message)["stamps"].reshape(-1), dtype=np.float64) + + +def select_points(message: PointCloud2, keep: NDArray[np.bool_]) -> PointCloud2: + """Copy selected point records in row order, retaining every field and point byte. + + The result is unorganized with no row padding. Point padding, endianness, + field metadata, source header, and the conservative density flag survive. + """ + pointcloud_view(message) # Validate the full declared layout before selecting bytes. + if keep.dtype != np.bool_ or keep.shape != (message.height * message.width,): + raise ValueError("point selection must be a flat boolean mask matching the point count") + records: NDArray[np.uint8] = np.ndarray( + (message.height, message.width, message.point_step), + dtype=np.uint8, + buffer=message.data, + strides=(message.row_step, message.point_step, 1), + ) + selected = records.reshape(message.height * message.width, message.point_step)[keep] + width = len(selected) + return PointCloud2( + header=deepcopy(message.header), + height=1, + width=width, + fields=deepcopy(message.fields), + is_bigendian=message.is_bigendian, + point_step=message.point_step, + row_step=width * message.point_step, + data=np.asarray(np.frombuffer(selected.tobytes(), dtype=np.uint8).copy(), dtype=np.uint8), + is_dense=message.is_dense, + ) + + +def pointcloud_rgb(message: PointCloud2) -> NDArray[np.uint8] | None: + """Copy RGB bytes from the standard packed rgb/rgba field, if present. + + FLOAT32 fields carry color bits, not numeric floating-point color values. + Both representations preserve the declared byte order and row padding. + """ + points = pointcloud_view(message) + names = points.dtype.names or () + name = next((name for name in ("rgb", "rgba") if name in names), None) + if name is None: + return None + field = points[name] + if field.ndim != 2 or field.dtype.kind not in "fu" or field.dtype.itemsize != 4: + raise ValueError("packed point cloud color must be a scalar FLOAT32 or UINT32 field") + packed = field.view(np.dtype(">u4" if message.is_bigendian else "> shift) & 255 for shift in (16, 8, 0)], axis=-1).astype(np.uint8) + + +def concatenate_clouds(first: PointCloud2, second: PointCloud2) -> PointCloud2: + """Join compatible point records, preserving fields, endian, and point padding. + + Row padding is removed. The first frame and newest exact source stamp survive. + Empty inputs are identities; differing nonempty frames/layouts are rejected. + """ + pointcloud_view(first) + pointcloud_view(second) + if not first.width * first.height: + return cdr_decode(cdr_encode(second), PointCloud2) + if not second.width * second.height: + return cdr_decode(cdr_encode(first), PointCloud2) + if first.header.frame_id != second.header.frame_id: + raise ValueError("cannot concatenate point clouds in different frames") + if ( + first.fields != second.fields + or first.point_step != second.point_step + or first.is_bigendian != second.is_bigendian + ): + raise ValueError("cannot concatenate point clouds with different record layouts") + a = select_points(first, np.ones(first.width * first.height, dtype=np.bool_)) + b = select_points(second, np.ones(second.width * second.height, dtype=np.bool_)) + stamp = max((first.header.stamp, second.header.stamp), key=to_nanoseconds) + return PointCloud2( + header=Header(frame_id=first.header.frame_id, stamp=stamp), + height=1, + width=a.width + b.width, + fields=deepcopy(first.fields), + is_bigendian=first.is_bigendian, + point_step=first.point_step, + row_step=a.row_step + b.row_step, + data=np.asarray(np.concatenate((a.data, b.data)), dtype=np.uint8), + is_dense=first.is_dense and second.is_dense, + ) + + +def transform_cloud(message: PointCloud2, transform: TransformStamped) -> PointCloud2: + """Apply parent←child to XYZ while retaining the complete organized record layout. + + Copy storage before changing coordinates. Keep all other fields, point/row + padding, endian and the cloud's exact source stamp. Frames must match; + integer XYZ is rejected because transformed coordinates would be truncated. + """ + if message.header.frame_id != transform.child_frame_id: + raise ValueError("point cloud frame does not match transform child frame") + view = pointcloud_view(message) + for name in ("x", "y", "z"): + if name not in (view.dtype.names or ()) or view.dtype[name].kind != "f": + raise ValueError("transform requires scalar floating-point XYZ fields") + matrix = transform_matrix(transform.transform) + xyz = pointcloud_xyz(message) + transformed = xyz @ matrix[:3, :3].T + matrix[:3, 3] + for index, name in enumerate(("x", "y", "z")): + values = transformed[:, index] + if np.any(np.abs(values[np.isfinite(values)]) > np.finfo(view.dtype[name]).max): + raise ValueError(f"transformed {name} coordinates exceed field range") + payload = bytearray(bytes(message.data)) + writable = np.ndarray( + (message.height, message.width), + dtype=view.dtype, + buffer=payload, + strides=(message.row_step, message.point_step), + ) + for index, name in enumerate(("x", "y", "z")): + writable[name] = transformed[:, index].reshape(message.height, message.width) + output = cdr_decode(cdr_encode(message), PointCloud2) + output.data = np.frombuffer(payload, dtype=np.uint8).copy() + output.header = Header(stamp=deepcopy(message.header.stamp), frame_id=transform.header.frame_id) + return output + + +def pointcloud_from_xyz_rgb( + points: NDArray[Any], colors: NDArray[np.uint8], *, header: Header +) -> PointCloud2: + """Copy XYZ and RGB8 into a declared XYZ FLOAT32 / packed RGB UINT32 layout.""" + cloud = pointcloud_from_xyz(points, header=header) + rgb = np.asarray(colors) + if rgb.dtype != np.uint8 or rgb.shape != np.asarray(points).shape: + raise ValueError("RGB must be uint8 with the same (..., 3) shape as XYZ") + records = np.empty((cloud.height, cloud.width), dtype=[("xyz", " PointCloud2: + """Project a rectified RGB/depth pair into the depth optical frame. + + Integer depth is multiplied by depth_scale to obtain meters; 32FC1 already + represents meters. Invalid/nonpositive/too-distant samples are removed. + Scale K when calibration resolution differs, retaining the depth header. + """ + if ( + not math.isfinite(depth_scale) + or depth_scale <= 0 + or not math.isfinite(depth_trunc) + or depth_trunc <= 0 + ): + raise ValueError("depth scale and truncation must be finite and positive") + if (color.width, color.height) != (depth.width, depth.height): + raise ValueError("color and depth dimensions do not match") + if depth.encoding not in ("16UC1", "mono16", "32FC1"): + raise ValueError("RGBD requires uint16 or float32 depth") + if calibration.width <= 0 or calibration.height <= 0: + raise ValueError("calibration dimensions must be positive") + matrix = intrinsic_matrix(calibration).copy() + matrix[0, :] *= depth.width / calibration.width + matrix[1, :] *= depth.height / calibration.height + distances = image_view(depth).astype(np.float64) + if depth.encoding != "32FC1": + distances *= depth_scale + valid = np.isfinite(distances) & (distances > 0) & (distances < depth_trunc) + rows = np.broadcast_to(np.arange(depth.height, dtype=np.float64)[:, None], distances.shape) + columns = np.broadcast_to(np.arange(depth.width, dtype=np.float64)[None, :], distances.shape) + z = distances[valid] + xyz = np.column_stack( + ( + (columns[valid] - matrix[0, 2]) * z / matrix[0, 0], + (rows[valid] - matrix[1, 2]) * z / matrix[1, 1], + z, + ) + ) + return pointcloud_from_xyz_rgb(xyz, image_to_rgb(color)[valid], header=depth.header) + + +def voxel_downsample_cloud(message: PointCloud2, voxel_size: float = 0.025) -> PointCloud2: + """Downsample XYZ/RGB clouds with the existing Open3D tensor backend. + + Non-positive sizes and clouds below twenty points retain the existing fast + path. Unsupported extra fields are rejected rather than silently dropped. + Source headers are preserved; generated messages own no numerical methods. + """ + if not math.isfinite(voxel_size): + raise ValueError("voxel_size must be finite") + if voxel_size <= 0 or message.width * message.height < 20: + return message + names = {field.name for field in message.fields} + if names not in ({"x", "y", "z"}, {"x", "y", "z", "rgb"}): + raise ValueError("voxel downsampling supports only XYZ and optional RGB fields") + points = pointcloud_xyz(message).astype(np.float32) + if not np.isfinite(points).all(): + raise ValueError("voxel downsampling requires finite XYZ positions") + colors = pointcloud_rgb(message) + + device = o3d.core.Device("CPU:0") + cloud = o3d.t.geometry.PointCloud(o3d.core.Tensor(points, o3d.core.Dtype.Float32, device)) + if colors is not None: + cloud.point["colors"] = o3d.core.Tensor( + colors.astype(np.float32) / 255.0, o3d.core.Dtype.Float32, device + ) + reduced = cloud.voxel_down_sample(voxel_size) + xyz = reduced.point["positions"].numpy() + if colors is None: + return pointcloud_from_xyz(xyz, header=message.header) + rgb = np.clip(reduced.point["colors"].numpy() * 255.0, 0, 255).astype(np.uint8) + return pointcloud_from_xyz_rgb(xyz, rgb, header=message.header) + + +def pointcloud_to_open3d(message: PointCloud2) -> Any: + """Copy generated XYZ/RGB values into a native geometry algorithm input.""" + + result = o3d.geometry.PointCloud() + result.points = o3d.utility.Vector3dVector(pointcloud_xyz(message)) + colors = pointcloud_rgb(message) + if colors is not None: + result.colors = o3d.utility.Vector3dVector(colors.astype(np.float64) / 255.0) + return result + + +def cloud_bounds_intersect(first: PointCloud2, second: PointCloud2) -> bool: + """Test closed axis-aligned bounds; empty or entirely nonfinite clouds do not intersect.""" + a, b = pointcloud_xyz(first), pointcloud_xyz(second) + a, b = a[np.isfinite(a).all(axis=1)], b[np.isfinite(b).all(axis=1)] + if len(a) == 0 or len(b) == 0: + return False + return bool(np.all(a.min(axis=0) <= b.max(axis=0)) and np.all(a.max(axis=0) >= b.min(axis=0))) diff --git a/dimos/msgs/test_camera_info.py b/dimos/msgs/test_camera_info.py new file mode 100644 index 00000000000..84c8a5d2a48 --- /dev/null +++ b/dimos/msgs/test_camera_info.py @@ -0,0 +1,149 @@ +# 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. + +"""Calibration helpers preserve generated ROS fields and reject malformed input.""" + +import math +from pathlib import Path + +from dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode +import numpy as np +import pytest +import yaml + +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.sensor_msgs.msg import CameraInfo +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.camera_info import ( + camera_info_from_fov, + camera_info_from_intrinsics, + camera_info_from_yaml, + camera_info_with_stamp, + intrinsic_matrix, +) + +_CALIBRATION = Path(__file__).parents[1] / "robot/unitree/go2/front_camera_720.yaml" + + +def test_real_go2_calibration_survives_cdr_with_exact_header(): + header = Header(frame_id="camera_optical", stamp=Time(sec=1700000000, nanosec=123456789)) + source = yaml.safe_load(_CALIBRATION.read_text()) + message = camera_info_from_yaml(_CALIBRATION, header=header) + decoded = cdr_decode(cdr_encode(message), CameraInfo) + assert decoded.header == header + assert (decoded.width, decoded.height) == (1280, 720) + assert decoded.distortion_model == "equidistant" + for field, key in [ + ("k", "camera_matrix"), + ("d", "distortion_coefficients"), + ("r", "rectification_matrix"), + ("p", "projection_matrix"), + ]: + np.testing.assert_array_equal(getattr(decoded, field), source[key]["data"]) + matrix = intrinsic_matrix(decoded) + assert matrix.shape == (3, 3) + matrix[0, 0] = 0 + assert decoded.k[0] == source["camera_matrix"]["data"][0] + decoded.header.frame_id = "changed" + assert header.frame_id == "camera_optical" + + +@pytest.mark.parametrize("axis,focal", [("vertical", 240), ("horizontal", 320)]) +def test_fov_converts_selected_axis_and_builds_projection(axis, focal): + message = camera_info_from_fov( + 90, 640, 480, header=Header(frame_id="optical", stamp=Time(sec=0, nanosec=0)), axis=axis + ) + np.testing.assert_allclose(message.k, [focal, 0, 320, 0, focal, 240, 0, 0, 1]) + np.testing.assert_allclose(message.p, [focal, 0, 320, 0, 0, focal, 240, 0, 0, 0, 1, 0]) + np.testing.assert_array_equal(message.r, np.eye(3).reshape(-1)) + assert message.header.frame_id == "optical" + + +@pytest.mark.parametrize("fov", [0, -1, 180, 181, math.nan, math.inf]) +def test_fov_rejects_nonphysical_angles(fov): + with pytest.raises(ValueError, match="Field of view"): + camera_info_from_fov( + fov, 640, 480, header=Header(stamp=Time(sec=0, nanosec=0), frame_id="") + ) + + +@pytest.mark.parametrize("width,height", [(0, 480), (640, -1), (True, 480), (1.5, 480)]) +def test_intrinsics_reject_invalid_dimensions(width, height): + with pytest.raises(ValueError, match="positive integers"): + camera_info_from_intrinsics( + 300, + 300, + 320, + 240, + width, + height, + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + ) + + +@pytest.mark.parametrize("fx", [0, -1, math.nan, math.inf]) +def test_intrinsics_reject_invalid_focal_length(fx): + with pytest.raises(ValueError, match="positive focal"): + camera_info_from_intrinsics( + fx, 300, 320, 240, 640, 480, header=Header(stamp=Time(sec=0, nanosec=0), frame_id="") + ) + + +@pytest.mark.parametrize("mutation", ["rows", "length", "nonfinite"]) +def test_yaml_rejects_invalid_camera_matrix(tmp_path, mutation): + data = yaml.safe_load(_CALIBRATION.read_text()) + matrix = data["camera_matrix"] + if mutation == "rows": + matrix["rows"] = 1 + elif mutation == "length": + matrix["data"].pop() + else: + matrix["data"][0] = math.nan + path = tmp_path / "invalid.yaml" + path.write_text(yaml.safe_dump(data)) + with pytest.raises(ValueError, match="camera_matrix"): + camera_info_from_yaml(path, header=Header(stamp=Time(sec=0, nanosec=0), frame_id="")) + + +def test_stamp_copy_preserves_calibration_and_does_not_mutate_prior_publish(): + original = camera_info_from_intrinsics( + 500, + 510, + 320, + 240, + 640, + 480, + header=Header(frame_id="optical", stamp=Time(sec=1, nanosec=987654321)), + ) + original.binning_x = 2 + original.roi.x_offset = 12 + stamp = Time(sec=2, nanosec=123456789) + copied = camera_info_with_stamp(original, stamp) + assert copied.header.stamp == stamp + assert copied.header.frame_id == original.header.frame_id + assert list(copied.k) == list(original.k) + assert copied.binning_x == 2 and copied.roi.x_offset == 12 + copied.k[0] = 100 + copied.roi.x_offset = 24 + stamp.nanosec = 0 + assert original.k[0] == 500 and original.roi.x_offset == 12 + assert original.header.stamp == Time(sec=1, nanosec=987654321) + assert copied.header.stamp.nanosec == 123456789 + + +def test_calibration_yaml_rejects_non_mapping_input(tmp_path): + source = tmp_path / "calibration.yaml" + source.write_text("- width\n- height\n") + with pytest.raises(TypeError, match="Calibration YAML must contain a mapping"): + camera_info_from_yaml(source, header=Header(stamp=Time(sec=0, nanosec=0), frame_id="")) diff --git a/dimos/msgs/test_geometry.py b/dimos/msgs/test_geometry.py new file mode 100644 index 00000000000..31c30c53bc2 --- /dev/null +++ b/dimos/msgs/test_geometry.py @@ -0,0 +1,283 @@ +# 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. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +import math + +from dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode +import numpy as np +import pytest + +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + PoseWithCovariance, + Quaternion, + Transform, + TransformStamped, + Twist, + TwistWithCovariance, + Vector3, +) +from dimos.messages.nav_msgs.msg import Odometry +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.geometry import ( + compose_transforms, + inverse_transform, + pose_from_matrix, + pose_from_transform, + pose_matrix, + quaternion_euler, + quaternion_from_euler, + transform_from_odometry, + transform_from_pose, + transform_matrix, + translate_pose_local, + yaw, +) + + +@pytest.mark.parametrize("seconds,nanoseconds", [(0, 0), (-1, 500000000), (1700000000, 123456789)]) +def test_odometry_transform_preserves_frames_stamp_and_pose(seconds, nanoseconds): + message = Odometry( + child_frame_id="robot/lidar", + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + 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), + ), + twist=TwistWithCovariance( + twist=Twist(linear=Vector3(x=0.0, y=0.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.0)), + covariance=np.zeros(36, dtype=np.float64), + ), + ) + message.header.frame_id = "robot/odom" + message.header.stamp = Time(sec=seconds, nanosec=nanoseconds) + message.pose.pose.position = Point(x=1, y=2, z=3) + message.pose.pose.orientation = Quaternion(z=0.6, w=0.8, x=0.0, y=0.0) + + transformed = transform_from_odometry(message) + + assert transformed.header.frame_id == "robot/odom" + assert transformed.child_frame_id == "robot/lidar" + assert transformed.header.stamp.sec == seconds + assert transformed.header.stamp.nanosec == nanoseconds + assert ( + transformed.transform.translation.x, + transformed.transform.translation.y, + transformed.transform.translation.z, + ) == (1, 2, 3) + assert transformed.transform.rotation == message.pose.pose.orientation + transformed.header.frame_id = "changed" + transformed.transform.rotation.w = 0 + assert message.header.frame_id == "robot/odom" + assert message.pose.pose.orientation.w == 0.8 + + +def _edge(parent, child, xyz, rotation, stamp=Time(sec=1700000000, nanosec=123456789)): + return TransformStamped( + header=Header(frame_id=parent, stamp=stamp), + child_frame_id=child, + transform=Transform(translation=Vector3(x=xyz[0], y=xyz[1], z=xyz[2]), rotation=rotation), + ) + + +def test_transform_composition_rotates_translation_and_preserves_exact_source_stamp(): + first = _edge("world", "arm", (1, -1, 0), quaternion_from_euler(0, 0, math.pi / 2)) + second = _edge( + "arm", "tool", (2, 3, 4), Quaternion(w=1, x=0.0, y=0.0, z=0.0), Time(sec=7, nanosec=0) + ) + + result = compose_transforms(first, second) + + assert result.header == first.header + assert result.child_frame_id == "tool" + translation = result.transform.translation + assert (translation.x, translation.y, translation.z) == pytest.approx((-2, 1, 4)) + assert yaw(result.transform.rotation) == pytest.approx(math.pi / 2) + result.header.stamp.nanosec = 0 + result.transform.translation.x = 99 + assert first.header.stamp.nanosec == 123456789 + assert first.transform.translation.x == 1 + assert second.transform.translation.x == 2 + + +def test_inverse_composes_to_identity_and_swaps_frames_without_losing_nanoseconds(): + original = _edge("map", "camera", (2, 3, -4), quaternion_from_euler(0.3, -0.2, 1.1)) + inverse = inverse_transform(original) + result = compose_transforms(original, inverse) + + assert inverse.header.frame_id == "camera" + assert inverse.child_frame_id == "map" + assert inverse.header.stamp == original.header.stamp + translation = result.transform.translation + assert (translation.x, translation.y, translation.z) == pytest.approx((0, 0, 0), abs=1e-12) + rotation = result.transform.rotation + assert (rotation.x, rotation.y, rotation.z, abs(rotation.w)) == pytest.approx((0, 0, 0, 1)) + + +def test_transform_composition_rejects_disconnected_frames(): + first = _edge("a", "b", (0, 0, 0), Quaternion(w=1, x=0.0, y=0.0, z=0.0)) + second = _edge("c", "d", (0, 0, 0), Quaternion(w=1, x=0.0, y=0.0, z=0.0)) + with pytest.raises(ValueError, match="Cannot compose frames"): + compose_transforms(first, second) + + +@pytest.mark.parametrize( + "rotation", + [ + Quaternion(w=0, x=0.0, y=0.0, z=0.0), + Quaternion(x=math.nan, w=1, y=0.0, z=0.0), + Quaternion(w=math.inf, x=0.0, y=0.0, z=0.0), + ], +) +def test_transform_math_rejects_invalid_rotations(rotation): + edge = _edge("a", "b", (0, 0, 0), rotation) + with pytest.raises(ValueError): + inverse_transform(edge) + with pytest.raises(ValueError): + compose_transforms(edge, _edge("b", "c", (0, 0, 0), Quaternion(w=1, x=0.0, y=0.0, z=0.0))) + + +@pytest.mark.parametrize("value", [math.nan, math.inf, -math.inf]) +def test_transform_math_rejects_nonfinite_translations_and_angles(value): + edge = _edge("a", "b", (value, 0, 0), Quaternion(w=1, x=0.0, y=0.0, z=0.0)) + with pytest.raises(ValueError, match="Translation components"): + inverse_transform(edge) + with pytest.raises(ValueError, match="Euler angles"): + quaternion_from_euler(value, 0, 0) + + +def test_pose_conversion_copies_nested_fields_and_preserves_header(): + edge = _edge("map", "robot", (1, 2, 3), Quaternion(z=0.6, w=0.8, x=0.0, y=0.0)) + pose = pose_from_transform(edge) + assert pose.header == edge.header + assert pose.pose.position == Point(x=1, y=2, z=3) + assert pose.pose.orientation == edge.transform.rotation + pose.header.frame_id = "changed" + pose.pose.orientation.w = 0 + assert edge.header.frame_id == "map" + assert edge.transform.rotation.w == 0.8 + + +def test_pose_transform_preserves_header_and_does_not_alias_input(): + source = PoseStamped( + header=Header(frame_id="odom", stamp=Time(sec=-1, nanosec=987654321)), + pose=Pose( + position=Point(x=1, y=2, z=3), orientation=Quaternion(z=0.6, w=0.8, x=0.0, y=0.0) + ), + ) + result = transform_from_pose(source, child_frame_id="robot/base") + decoded = cdr_decode(cdr_encode(result), TransformStamped) + assert decoded.header == source.header + assert decoded.child_frame_id == "robot/base" + assert decoded.transform.translation == Vector3(x=1, y=2, z=3) + assert decoded.transform.rotation == source.pose.orientation + result.header.frame_id = "changed" + result.transform.rotation.w = 0 + assert source.header.frame_id == "odom" + assert source.pose.orientation.w == 0.8 + + +def test_transform_matrix_maps_rotation_and_translation_without_aliasing(): + value = Transform( + translation=Vector3(x=2.0, y=-1.0, z=3.0), + rotation=quaternion_from_euler(0.0, 0.0, math.pi / 2), + ) + matrix = transform_matrix(value) + np.testing.assert_allclose(matrix @ [1.0, 0.0, 0.0, 1.0], [2.0, 0.0, 3.0, 1.0], atol=1e-12) + value.translation.x = 10.0 + assert matrix[0, 3] == 2.0 + + +def test_pose_matrix_roundtrip_preserves_rigid_motion_without_aliasing(): + pose = Pose( + position=Point(x=2, y=-1, z=3), + orientation=quaternion_from_euler(0, 0, math.pi / 2), + ) + matrix = pose_matrix(pose) + np.testing.assert_allclose(matrix @ [1, 0, 0, 1], [2, 0, 3, 1], atol=1e-12) + decoded = cdr_decode(cdr_encode(pose_from_matrix(matrix)), Pose) + np.testing.assert_allclose(pose_matrix(decoded), matrix, atol=1e-12) + matrix[0, 3] = 99 + decoded.position.y = 42 + assert pose.position == Point(x=2, y=-1, z=3) + assert decoded.position.x == 2 + + +@pytest.mark.parametrize( + "matrix", + [ + np.eye(3), + np.full((4, 4), math.nan), + np.diag([1, 1, 1, 2]), + np.diag([2, 1, 1, 1]), + np.diag([-1, 1, 1, 1]), + ], +) +def test_pose_from_matrix_rejects_nonrigid_or_invalid_matrices(matrix): + with pytest.raises(ValueError): + pose_from_matrix(matrix) + + +@pytest.mark.parametrize( + "pose", + [ + Pose( + position=Point(x=math.inf, y=0.0, z=0.0), + orientation=Quaternion(w=1, x=0.0, y=0.0, z=0.0), + ), + Pose(orientation=Quaternion(w=0, x=0.0, y=0.0, z=0.0), position=Point(x=0.0, y=0.0, z=0.0)), + Pose( + orientation=Quaternion(w=math.nan, x=0.0, y=0.0, z=0.0), + position=Point(x=0.0, y=0.0, z=0.0), + ), + ], +) +def test_pose_matrix_rejects_invalid_geometry(pose): + with pytest.raises(ValueError): + pose_matrix(pose) + + +def test_local_offset_uses_pose_axes_and_copies_orientation(): + pose = Pose( + position=Point(x=1, y=2, z=3), + orientation=quaternion_from_euler(0, 0, math.pi / 2), + ) + translated = translate_pose_local(pose, Vector3(x=2, z=-1, y=0.0)) + position = translated.position + assert (position.x, position.y, position.z) == pytest.approx((1, 4, 2)) + assert quaternion_euler(translated.orientation) == pytest.approx((0, 0, math.pi / 2)) + translated.orientation.w = 0 + assert pose.orientation.w == pytest.approx(math.sqrt(0.5)) + assert pose.position == Point(x=1, y=2, z=3) + + +@pytest.mark.parametrize("value", [math.nan, math.inf, -math.inf]) +def test_local_offset_rejects_nonfinite_displacements(value): + with pytest.raises(ValueError, match="Offset components"): + translate_pose_local( + Pose( + orientation=Quaternion(w=1, x=0.0, y=0.0, z=0.0), + position=Point(x=0.0, y=0.0, z=0.0), + ), + Vector3(x=value, y=0.0, z=0.0), + ) diff --git a/dimos/msgs/test_image_compression.py b/dimos/msgs/test_image_compression.py new file mode 100644 index 00000000000..71e8c04aacd --- /dev/null +++ b/dimos/msgs/test_image_compression.py @@ -0,0 +1,187 @@ +# 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 cv2 +from dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode +import numpy as np +import pytest + +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.sensor_msgs.msg import CompressedImage +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.image import ( + compressed_image_from_image, + image_from_array, + image_from_compressed, + image_to_bgr, + image_to_rgb, + image_view, +) +from dimos.msgs.time import time_from_seconds, to_nanoseconds + + +@pytest.fixture() +def rgb_image(): + rng = np.random.RandomState(42) + gradient = np.linspace(0, 255, 640, dtype=np.uint8) + data = np.broadcast_to(gradient, (480, 640)).copy() + data = np.stack([data, data // 2, rng.randint(0, 50, (480, 640), dtype=np.uint8)], axis=-1) + return image_from_array( + data, encoding="rgb8", header=Header(frame_id="cam", stamp=time_from_seconds(1234.5678)) + ) + + +def test_jpeg_roundtrip_preserves_meta_and_pixels(rgb_image) -> None: + ci = compressed_image_from_image(rgb_image, quality=90) + assert "; jpeg compressed " in ci.format + assert ci.header.frame_id == "cam" + assert to_nanoseconds(ci.header.stamp) == to_nanoseconds(rgb_image.header.stamp) + assert 0 < len(ci.data) < image_view(rgb_image).nbytes // 4 + img = image_from_compressed(ci) + assert img.encoding in ("rgb8", "bgr8") + assert img.header.frame_id == "cam" + assert to_nanoseconds(img.header.stamp) == to_nanoseconds(rgb_image.header.stamp) + assert image_view(img).shape == image_view(rgb_image).shape + diff = np.abs(image_to_rgb(img).astype(int) - image_view(rgb_image).astype(int)).mean() + assert diff < 5, f"JPEG q90 mean pixel error too high: {diff}" + + +def test_png_roundtrip_is_lossless_bgr(rgb_image) -> None: + bgr = image_from_array(image_to_bgr(rgb_image), encoding="bgr8", header=rgb_image.header) + ci = compressed_image_from_image(bgr, format="png") + img = image_from_compressed(ci) + assert img.encoding == "bgr8" + assert np.array_equal(image_view(img), image_view(bgr)) + assert to_nanoseconds(img.header.stamp) == to_nanoseconds(bgr.header.stamp) + + +def test_png_roundtrip_is_lossless_gray16() -> None: + data = np.arange(100 * 80, dtype=np.uint16).reshape(100, 80) + src = image_from_array( + data, encoding="mono16", header=Header(frame_id="d", stamp=time_from_seconds(1.0)) + ) + img = image_from_compressed(compressed_image_from_image(src, format="png")) + assert img.encoding == "mono16" + assert np.array_equal(image_view(img), data) + + +def test_png_decode_alpha_is_bgra() -> None: + arr = np.zeros((10, 10, 4), dtype=np.uint8) + arr[..., 3] = 128 + ok, buf = cv2.imencode(".png", arr) + assert ok + img = image_from_compressed( + CompressedImage( + data=np.frombuffer(buf.tobytes(), dtype=np.uint8), + format="png", + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + ) + ) + assert img.encoding == "bgra8" + assert np.array_equal(image_view(img), arr) + + +def test_lcm_roundtrip(rgb_image) -> None: + ci = compressed_image_from_image(rgb_image) + wire = cdr_encode(ci) + out = cdr_decode(wire, CompressedImage) + np.testing.assert_array_equal(out.data, ci.data) + assert "; jpeg compressed " in out.format + assert out.header.frame_id == "cam" + assert abs(to_nanoseconds(out.header.stamp) - to_nanoseconds(ci.header.stamp)) < 1e-06 + + +def test_jpeg_rejects_depth_formats() -> None: + depth = image_from_array( + np.zeros((10, 10), dtype=np.uint16), + encoding="mono16", + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + ) + with pytest.raises(ValueError, match="JPEG requires 8-bit"): + compressed_image_from_image(depth) + + +def test_png_rejects_float_depth() -> None: + depth = image_from_array( + np.zeros((10, 10), dtype=np.float32), + encoding="32FC1", + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + ) + with pytest.raises(ValueError, match="PNG cannot encode"): + compressed_image_from_image(depth, format="png") + + +def test_from_image_rejects_non_image() -> None: + with pytest.raises(TypeError): + compressed_image_from_image(b"not an image") + + +def test_max_width_resizes(rgb_image) -> None: + img = image_from_compressed(compressed_image_from_image(rgb_image, max_width=320)) + assert img.width <= 320 + assert img.height <= 320 + + +def test_jxl_roundtrip_is_lossless_gray16() -> None: + data = np.arange(100 * 80, dtype=np.uint16).reshape(100, 80) + src = image_from_array( + data, encoding="mono16", header=Header(frame_id="d", stamp=time_from_seconds(1.0)) + ) + ci = compressed_image_from_image(src, format="jxl") + assert "; jxl compressed " in ci.format + img = image_from_compressed(ci) + assert img.encoding == "mono16" + assert np.array_equal(image_view(img), data) + + +def test_jxl_roundtrip_is_lossless_float_depth() -> None: + data = np.linspace(0.1, 10.0, 100 * 80, dtype=np.float32).reshape(100, 80) + src = image_from_array( + data, encoding="32FC1", header=Header(frame_id="d", stamp=time_from_seconds(2.0)) + ) + ci = compressed_image_from_image(src, format="jxl") + img = image_from_compressed(ci) + assert img.encoding == "32FC1" + assert np.array_equal(image_view(img), data) + assert to_nanoseconds(img.header.stamp) == to_nanoseconds(src.header.stamp) + assert img.header.frame_id == "d" + + +def test_jxl_lossy_rgb_honors_quality(rgb_image) -> None: + ci = compressed_image_from_image(rgb_image, format="jxl", quality=90) + assert 0 < len(ci.data) < image_view(rgb_image).nbytes // 4 + img = image_from_compressed(ci) + assert img.encoding in ("rgb8", "bgr8") + diff = np.abs(image_to_rgb(img).astype(int) - image_view(rgb_image).astype(int)).mean() + assert diff < 5, f"JXL q90 mean pixel error too high: {diff}" + + +def test_jxl_effort_trades_cpu_for_size(rgb_image) -> None: + fast = compressed_image_from_image(rgb_image, format="jxl", effort=1) + thorough = compressed_image_from_image(rgb_image, format="jxl", effort=7) + assert len(thorough.data) <= len(fast.data) + + +def test_jxl_rejects_float64() -> None: + depth = image_from_array(np.zeros((10, 10), dtype=np.float64), encoding="64FC1") + with pytest.raises(ValueError, match="JXL cannot encode"): + compressed_image_from_image(depth, format="jxl") + + +@pytest.mark.parametrize("codec", ["png", "jxl"]) +def test_compressed_big_endian_depth(codec: str) -> None: + pixels = np.arange(80, dtype=">u2").reshape(10, 8) * 257 + source = image_from_array(pixels.astype(">u2"), encoding="16UC1") + decoded = image_from_compressed(compressed_image_from_image(source, format=codec)) + np.testing.assert_array_equal(image_view(decoded), pixels) diff --git a/dimos/msgs/test_image_views.py b/dimos/msgs/test_image_views.py new file mode 100644 index 00000000000..4873c3a9983 --- /dev/null +++ b/dimos/msgs/test_image_views.py @@ -0,0 +1,233 @@ +# 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. + +"""Borrowed buffers preserve padding, byte order, lifetime, and ROS value semantics.""" + +import gc + +import cv2 +from dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode +import numpy as np +from PIL import Image as PILImage +import pytest + +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.msgs.image import ( + image_brightness, + image_from_array, + image_from_file, + image_sharpness, + image_to_jpeg, + image_to_rgb, + image_view, +) +from dimos.msgs.time import time_from_nanoseconds + + +def test_padded_image_view_retains_owner_and_is_readonly() -> None: + msg = Image( + width=2, + height=2, + step=4, + encoding="mono8", + data=np.array([1, 2, 99, 99, 3, 4, 99, 99], dtype=np.uint8), + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + is_bigendian=0, + ) + pixels = image_view(msg) + np.testing.assert_array_equal(pixels, [[1, 2], [3, 4]]) + assert pixels.strides == (4, 1) + with pytest.raises(ValueError): + pixels[0, 0] = 9 + del msg + gc.collect() + np.testing.assert_array_equal(pixels, [[1, 2], [3, 4]]) + copied = pixels.copy() + copied[0, 0] = 9 + assert pixels[0, 0] == 1 + + +def test_big_endian_depth_view() -> None: + msg = Image( + width=2, + height=1, + step=4, + encoding="16UC1", + is_bigendian=1, + data=np.array([0x01, 0x02, 0x03, 0x04], dtype=np.uint8), + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + ) + np.testing.assert_array_equal(image_view(msg), [[258, 772]]) + + +def test_image_array_copy_preserves_endian_and_handles_strides() -> None: + pixels = np.arange(24, dtype=">u2").reshape(4, 6)[::2, ::2] + msg = image_from_array(pixels, encoding="16UC1") + assert (msg.width, msg.height, msg.step, msg.is_bigendian) == (3, 2, 6, 1) + expected = pixels.copy() + pixels[:] = 0 + np.testing.assert_array_equal(image_view(msg), expected) + + +@pytest.mark.parametrize( + "pixels,encoding", + [(np.zeros((2, 2), dtype=np.float32), "mono8"), (np.zeros((2, 2), dtype=np.uint8), "rgb8")], +) +def test_image_array_requires_matching_encoding(pixels: np.ndarray, encoding: str) -> None: + with pytest.raises(ValueError, match="does not match"): + image_from_array(pixels, encoding=encoding) + + +@pytest.mark.parametrize("step,data", [(1, [1]), (2, [1]), (2, [1, 2, 3])]) +def test_invalid_image_layout(step: int, data: list[int]) -> None: + with pytest.raises(ValueError, match="dimensions"): + image_view( + Image( + width=2, + height=1, + step=step, + data=np.asarray(data, dtype=np.uint8), + encoding="mono8", + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + is_bigendian=0, + ) + ) + + +def test_jpeg_preserves_rgb_color() -> None: + msg = Image( + width=16, + height=16, + step=48, + encoding="rgb8", + data=np.asarray([255, 0, 0] * 256, dtype=np.uint8), + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + is_bigendian=0, + ) + jpeg = image_to_jpeg(msg, quality=95) + bgr = cv2.imdecode(np.frombuffer(jpeg, np.uint8), cv2.IMREAD_COLOR) + assert bgr.shape == (16, 16, 3) + assert bgr[8, 8, 2] > 250 and bgr[8, 8, 0] < 5 + + +@pytest.mark.parametrize("encoding", ["rgb8", "bgr8", "rgba8", "bgra8", "mono8"]) +def test_sharpness_prefers_edges_to_blurred_pixels(encoding): + mono = ((np.indices((64, 64)).sum(axis=0) % 2) * 255).astype(np.uint8) + channels = 1 if encoding == "mono8" else (4 if "a" in encoding else 3) + pixels = mono if channels == 1 else np.repeat(mono[:, :, None], channels, axis=2) + blurred = cv2.GaussianBlur(pixels, (5, 5), 0) + sharp = image_from_array(pixels, encoding=encoding) + soft = image_from_array(blurred, encoding=encoding) + assert image_sharpness(sharp) > image_sharpness(soft) + + +def test_sharpness_rejects_depth_and_empty_images(): + depth = image_from_array(np.zeros((2, 2), dtype=np.float32), encoding="32FC1") + with pytest.raises(ValueError, match="8-bit visual"): + image_sharpness(depth) + with pytest.raises(ValueError, match="nonempty"): + image_sharpness( + Image( + encoding="mono8", + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + height=0, + width=0, + is_bigendian=0, + step=0, + data=np.array([], dtype=np.uint8), + ) + ) + + +@pytest.mark.parametrize( + "encoding,pixel", + [ + ("rgb8", [255, 0, 17]), + ("bgr8", [17, 0, 255]), + ("rgba8", [255, 0, 17, 23]), + ("bgra8", [17, 0, 255, 23]), + ], +) +def test_rgb_model_input_is_color_correct_and_independent(encoding: str, pixel: list[int]) -> None: + source = image_from_array(np.array([[pixel]], dtype=np.uint8), encoding=encoding) + restored = cdr_decode(cdr_encode(source), Image) + rgb = image_to_rgb(restored) + np.testing.assert_array_equal(rgb, [[[255, 0, 17]]]) + rgb[0, 0] = 0 + np.testing.assert_array_equal(image_view(restored), [[pixel]]) + + +def test_rgb_model_input_respects_big_endian_padded_grayscale() -> None: + msg = Image( + width=2, + height=2, + step=6, + encoding="mono16", + is_bigendian=1, + data=np.array([0x12, 0x34, 0xFF, 0xFF, 99, 99, 0x01, 0x00, 0, 0, 99, 99], dtype=np.uint8), + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + ) + np.testing.assert_array_equal( + image_to_rgb(msg), [[[18, 18, 18], [255, 255, 255]], [[1, 1, 1], [0, 0, 0]]] + ) + + +def test_rgb_model_input_requires_explicit_float_depth_scale() -> None: + msg = image_from_array(np.array([[2.5]], dtype=np.float32), encoding="32FC1") + with pytest.raises(ValueError, match="RGB8"): + image_to_rgb(msg) + + +def test_brightness_ignores_padding_and_normalizes_big_endian_u16() -> None: + message = Image( + width=2, + height=1, + step=6, + encoding="mono16", + is_bigendian=1, + data=np.array([0, 0, 255, 255, 255, 255], dtype=np.uint8), + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + ) + assert image_brightness(message) == pytest.approx(0.5) + depth = image_from_array(np.array([[1.0]], dtype=np.float32), encoding="32FC1") + with pytest.raises(ValueError, match="unsigned integer"): + image_brightness(depth) + + +@pytest.mark.parametrize("mode", ["L", "RGBA"]) +def test_image_file_decodes_rgb_and_preserves_source_header(tmp_path, mode): + path = tmp_path / "frame.png" + pixels = np.array([[7, 19]], dtype=np.uint8) + source = PILImage.fromarray(pixels).convert(mode) + source.save(path) + header = Header(stamp=time_from_nanoseconds(1234567890), frame_id="camera") + message = image_from_file(path, header=header) + assert message.encoding == "rgb8" + assert cdr_encode(message.header) == cdr_encode(header) + np.testing.assert_array_equal(image_view(message), [[[7, 7, 7], [19, 19, 19]]]) + + +@pytest.mark.parametrize( + "encoding,dtype,shape", + [("64FC1", ">f8", (3, 4)), ("16SC1", ">i2", (3, 4)), ("32FC3", ">f4", (3, 4, 3))], +) +def test_remaining_raw_encodings_keep_signed_float_and_endian_values(encoding, dtype, shape): + pixels = (np.arange(np.prod(shape)).reshape(shape) - 5).astype(dtype) + source = image_from_array(pixels, encoding=encoding) + decoded = cdr_decode(cdr_encode(source), Image) + assert decoded.encoding == encoding + assert decoded.is_bigendian == 1 + np.testing.assert_array_equal(image_view(decoded), pixels) diff --git a/dimos/msgs/test_pointcloud.py b/dimos/msgs/test_pointcloud.py new file mode 100644 index 00000000000..7c1237ccd78 --- /dev/null +++ b/dimos/msgs/test_pointcloud.py @@ -0,0 +1,462 @@ +# 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. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +import gc +import struct + +from dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode +import numpy as np +import pytest + +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 PointCloud2, PointField +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.image import image_from_array +from dimos.msgs.pointcloud import ( + concatenate_clouds, + pointcloud_from_rgbd, + pointcloud_from_xyz, + pointcloud_from_xyz_rgb, + pointcloud_rgb, + pointcloud_stamps, + pointcloud_view, + pointcloud_xyz, + select_points, + transform_cloud, + voxel_downsample_cloud, +) + + +@pytest.fixture(params=[False, True], ids=["little-endian", "big-endian"]) +def padded_cloud(request): + # Two organized rows, two points per row; fields are deliberately reordered. + endian = ">" if request.param else "<" + payload = bytearray(128) + for row in range(2): + for column in range(2): + value = row * 2 + column + 1 + struct.pack_into( + endian + "df2Hf", + payload, + row * 64 + column * 24, + value * 3.0, + value * 1.0, + value, + value + 10, + value * 2.0, + ) + return PointCloud2( + height=2, + width=2, + point_step=24, + row_step=64, + is_bigendian=request.param, + data=np.frombuffer(bytes(payload), dtype=np.uint8), + fields=[ + PointField(name="z", offset=0, datatype=8, count=1), + PointField(name="x", offset=8, datatype=7, count=1), + PointField(name="tags", offset=12, datatype=4, count=2), + PointField(name="y", offset=16, datatype=7, count=1), + ], + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + is_dense=False, + ) + + +def test_field_counts_padding_and_endianness(padded_cloud): + view = pointcloud_view(padded_cloud) + assert view["tags"].tolist() == [[[1, 11], [2, 12]], [[3, 13], [4, 14]]] + assert pointcloud_xyz(padded_cloud).tolist() == [[1, 2, 3], [2, 4, 6], [3, 6, 9], [4, 8, 12]] + assert not view.flags.writeable + with pytest.raises(ValueError, match="WRITEABLE"): + view.setflags(write=True) + copied = view.copy() + copied["x"][0, 0] = 99 + assert view["x"][0, 0] == 1 + + +def test_view_retains_storage_after_message_is_deleted(): + message = PointCloud2( + height=1, + width=1, + point_step=4, + row_step=4, + fields=[PointField(name="x", datatype=7, count=1, offset=0)], + data=np.frombuffer(struct.pack("u2"), encoding="16UC1", header=header) + # Add nonzero row padding to big-endian depth. + payload = bytes(depth.data) + depth.data = payload[:6] + b"xx" + payload[6:] + b"yy" + depth.step = 8 + calibration = camera_info_from_intrinsics(4, 8, 2, 2, 6, 4, header=header) + cloud = cdr_decode( + cdr_encode( + pointcloud_from_rgbd(color, depth, calibration, depth_scale=0.001, depth_trunc=4) + ), + PointCloud2, + ) + np.testing.assert_allclose( + pointcloud_xyz(cloud), [[-0.5, -0.25, 1], [1, -0.5, 2], [-1.5, 0, 3]] + ) + np.testing.assert_array_equal(pointcloud_rgb(cloud), rgb.reshape(-1, 3)[[0, 2, 3]]) + assert cloud.header == header + + +@pytest.mark.parametrize( + "scale,trunc", [(0, 1), (-1, 1), (float("nan"), 1), (1, 0), (1, float("inf"))] +) +def test_rgbd_rejects_invalid_units(scale, trunc): + header = Header(stamp=Time(sec=0, nanosec=0), frame_id="") + color = image_from_array(np.zeros((1, 1, 3), dtype=np.uint8), encoding="rgb8", header=header) + depth = image_from_array(np.ones((1, 1), dtype=np.uint16), encoding="16UC1", header=header) + calibration = camera_info_from_intrinsics(1, 1, 0, 0, 1, 1, header=header) + with pytest.raises(ValueError, match="finite and positive"): + pointcloud_from_rgbd(color, depth, calibration, depth_scale=scale, depth_trunc=trunc) + + +def test_transform_rejects_coordinate_overflow(): + cloud = pointcloud_from_xyz( + np.array([[3e38, 0, 0]]), header=Header(frame_id="child", stamp=Time(sec=0, nanosec=0)) + ) + transform = TransformStamped( + header=Header(frame_id="parent", stamp=Time(sec=0, nanosec=0)), + child_frame_id="child", + transform=Transform( + translation=Vector3(x=3e38, y=0.0, z=0.0), rotation=Quaternion(w=1, x=0.0, y=0.0, z=0.0) + ), + ) + with pytest.raises(ValueError, match="exceed field range"): + transform_cloud(cloud, transform) + assert np.isfinite(pointcloud_xyz(cloud)).all() + + +def test_voxel_downsample_retains_existing_small_cloud_fast_path(): + message = pointcloud_from_xyz( + np.zeros((19, 3)), header=Header(frame_id="lidar", stamp=Time(sec=0, nanosec=0)) + ) + assert voxel_downsample_cloud(message, 0.5) is message + assert voxel_downsample_cloud(message, 0.0) is message + with pytest.raises(ValueError, match="finite"): + voxel_downsample_cloud(message, float("nan")) + + +def test_voxel_downsample_native_centroids_colors_and_exact_header(): + pytest.importorskip("open3d", reason="native tensor downsampling requires Open3D") + points = np.repeat([[0.1, 0.1, 0.1], [0.3, 0.3, 0.3]], 20, axis=0) + colors = np.repeat([[0, 0, 0], [255, 255, 255]], 20, axis=0).astype(np.uint8) + header = Header(frame_id="optical", stamp=Time(sec=5, nanosec=123456789)) + source = pointcloud_from_xyz_rgb(points, colors, header=header) + result = voxel_downsample_cloud(source, 1.0) + assert result.header == header + assert result.width == 1 + np.testing.assert_allclose(pointcloud_xyz(result), [[0.2, 0.2, 0.2]], atol=1e-6) + np.testing.assert_array_equal(pointcloud_rgb(result), [[127, 127, 127]]) + assert source.width == 40 + + +@pytest.mark.parametrize("shape", [(3, 3), (2, 3, 3)]) +def test_per_point_stamps_survive_cdr_and_point_selection(shape): + points = np.arange(np.prod(shape), dtype=np.float32).reshape(shape) + stamps = np.arange(np.prod(shape[:-1]), dtype=np.float64).reshape(shape[:-1]) + 17.25 + header = Header(stamp=Time(sec=19, nanosec=123), frame_id="map") + cloud = pointcloud_from_xyz(points, header=header, stamps=stamps) + restored = cdr_decode(cdr_encode(cloud), PointCloud2) + np.testing.assert_array_equal(pointcloud_xyz(restored), points.reshape(-1, 3)) + np.testing.assert_array_equal(pointcloud_stamps(restored), stamps.reshape(-1)) + keep = np.arange(stamps.size) % 2 == 0 + selected = select_points(restored, keep) + np.testing.assert_array_equal(pointcloud_stamps(selected), stamps.reshape(-1)[keep]) + assert selected.header == header + assert pointcloud_stamps(pointcloud_from_xyz(points, header=header)) is None + + +@pytest.mark.parametrize("stamps", [np.array([1.0]), np.array([1.0, np.nan])]) +def test_per_point_stamps_reject_wrong_count_or_nonfinite_values(stamps): + with pytest.raises(ValueError, match="Per-point stamps"): + pointcloud_from_xyz( + np.zeros((2, 3)), + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + stamps=stamps, + ) diff --git a/dimos/msgs/test_pointcloud_colors.py b/dimos/msgs/test_pointcloud_colors.py new file mode 100644 index 00000000000..18fe6680e2b --- /dev/null +++ b/dimos/msgs/test_pointcloud_colors.py @@ -0,0 +1,85 @@ +# 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 dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode +import numpy as np +import pytest + +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.sensor_msgs.msg import PointCloud2, PointField +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.pointcloud import pointcloud_rgb + + +@pytest.mark.parametrize("bigendian", [False, True]) +@pytest.mark.parametrize("name", ["rgb", "rgba"]) +@pytest.mark.parametrize("datatype", [PointField.FLOAT32, PointField.UINT32]) +def test_packed_colors_preserve_bits_and_row_padding(bigendian, name, datatype): + order = ">" if bigendian else "<" + data = bytearray(48) + values = np.ndarray((2, 2), dtype=order + "u4", buffer=data, offset=4, strides=(24, 8)) + values[:] = [[0xFF123456, 0x00ABCDEF], [0x00FF0000, 0xFF00FF00]] + cloud = PointCloud2( + height=2, + width=2, + point_step=8, + row_step=24, + is_bigendian=bigendian, + data=np.frombuffer(bytes(data), dtype=np.uint8), + fields=[PointField(name=name, offset=4, count=1, datatype=datatype)], + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + is_dense=False, + ) + decoded = cdr_decode(cdr_encode(cloud), PointCloud2) + np.testing.assert_array_equal( + pointcloud_rgb(decoded), [[0x12, 0x34, 0x56], [0xAB, 0xCD, 0xEF], [255, 0, 0], [0, 255, 0]] + ) + + +def test_missing_colors(): + assert ( + pointcloud_rgb( + 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, + ) + ) + is None + ) + + +@pytest.mark.parametrize( + "datatype,count", [(PointField.FLOAT64, 1), (PointField.INT32, 1), (PointField.UINT32, 2)] +) +def test_invalid_color_layout(datatype, count): + cloud = PointCloud2( + height=1, + width=1, + point_step=8, + row_step=8, + data=np.frombuffer(bytes(8), dtype=np.uint8), + fields=[PointField(name="rgb", offset=0, count=count, datatype=datatype)], + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + is_bigendian=False, + is_dense=False, + ) + with pytest.raises(ValueError, match="packed point cloud color"): + pointcloud_rgb(cloud) diff --git a/dimos/msgs/test_time.py b/dimos/msgs/test_time.py new file mode 100644 index 00000000000..8e3fe0f037e --- /dev/null +++ b/dimos/msgs/test_time.py @@ -0,0 +1,82 @@ +# 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 dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode +import pytest + +from dimos.msgs.time import ( + duration_from_nanoseconds, + duration_from_seconds, + header_now, + time_from_nanoseconds, + time_from_seconds, + to_nanoseconds, + to_seconds, +) + + +@pytest.mark.parametrize("factory", [time_from_nanoseconds, duration_from_nanoseconds]) +@pytest.mark.parametrize("value", [0, -1, -1_250_000_000, 1_700_000_000_123_456_789]) +def test_integer_time_round_trip_preserves_every_nanosecond(factory, value): + stamp = factory(value) + + assert 0 <= stamp.nanosec < 1_000_000_000 + assert to_nanoseconds(stamp) == value + assert to_nanoseconds(cdr_decode(cdr_encode(stamp), stamp.__msgtype__)) == value + + +@pytest.mark.parametrize("factory", [time_from_seconds, duration_from_seconds]) +@pytest.mark.parametrize("value,expected", [(-1.25, (-2, 750_000_000)), (0.9999999996, (1, 0))]) +def test_float_conversion_normalizes_negative_values_and_fractional_carry(factory, value, expected): + stamp = factory(value) + + assert (stamp.sec, stamp.nanosec) == expected + assert to_seconds(stamp) == pytest.approx(value, abs=1e-9) + + +@pytest.mark.parametrize("value", [float("nan"), float("inf"), -float("inf")]) +def test_nonfinite_seconds_are_rejected(value): + with pytest.raises(ValueError, match="finite"): + time_from_seconds(value) + + +def test_invalid_ros_fraction_is_rejected(): + stamp = time_from_nanoseconds(0) + stamp.nanosec = 1_000_000_000 + + with pytest.raises(ValueError, match="nanosec"): + to_nanoseconds(stamp) + + +def test_now_reads_integer_clock_and_preserves_frame(monkeypatch): + monkeypatch.setattr("dimos.msgs.time.time.time_ns", lambda: 1_700_000_000_123_456_789) + + header = header_now("camera") + + assert to_nanoseconds(header.stamp) == 1_700_000_000_123_456_789 + assert header.frame_id == "camera" + + +@pytest.mark.parametrize("factory", [time_from_nanoseconds, duration_from_nanoseconds]) +@pytest.mark.parametrize("seconds", [-(2**31), 2**31 - 1]) +def test_ros_seconds_range_boundaries(factory, seconds): + value = seconds * 1_000_000_000 + 999_999_999 + assert to_nanoseconds(factory(value)) == value + + +@pytest.mark.parametrize("factory", [time_from_nanoseconds, duration_from_nanoseconds]) +@pytest.mark.parametrize("value", [-(2**31) * 1_000_000_000 - 1, 2**31 * 1_000_000_000]) +def test_ros_seconds_overflow_is_explicit(factory, value): + with pytest.raises(ValueError, match="signed 32-bit"): + factory(value) diff --git a/dimos/msgs/time.py b/dimos/msgs/time.py new file mode 100644 index 00000000000..3d9197cb58a --- /dev/null +++ b/dimos/msgs/time.py @@ -0,0 +1,77 @@ +# 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. + +"""Explicit conversions for generated ROS time, duration, and header fields.""" + +import math +import time +from typing import Any + +from dimos.messages.builtin_interfaces.msg import Duration, Time +from dimos.messages.std_msgs.msg import Header + +NANOSECONDS_PER_SECOND = 1_000_000_000 + + +def to_nanoseconds(stamp: Time | Duration) -> int: + """Convert a normalized ROS time or duration without losing integer precision.""" + if not 0 <= stamp.nanosec < NANOSECONDS_PER_SECOND: + raise ValueError("nanosec must be in [0, 1000000000)") + return stamp.sec * NANOSECONDS_PER_SECOND + stamp.nanosec + + +def to_seconds(stamp: Time | Duration) -> float: + """Convert to floating seconds for APIs that require them.""" + return to_nanoseconds(stamp) / NANOSECONDS_PER_SECOND + + +def time_from_nanoseconds(value: int) -> Time: + sec, nanosec = divmod(value, NANOSECONDS_PER_SECOND) + if not -(2**31) <= sec < 2**31: + raise ValueError("seconds must fit the ROS signed 32-bit field") + return Time(sec=sec, nanosec=nanosec) + + +def duration_from_nanoseconds(value: int) -> Duration: + sec, nanosec = divmod(value, NANOSECONDS_PER_SECOND) + if not -(2**31) <= sec < 2**31: + raise ValueError("seconds must fit the ROS signed 32-bit field") + return Duration(sec=sec, nanosec=nanosec) + + +def _seconds_to_nanoseconds(value: float) -> int: + if not math.isfinite(value): + raise ValueError("seconds must be finite") + sec = math.floor(value) + return sec * NANOSECONDS_PER_SECOND + round((value - sec) * NANOSECONDS_PER_SECOND) + + +def time_from_seconds(value: float) -> Time: + """Round to the nearest nanosecond, normalizing negative and carried fractions.""" + return time_from_nanoseconds(_seconds_to_nanoseconds(value)) + + +def duration_from_seconds(value: float) -> Duration: + return duration_from_nanoseconds(_seconds_to_nanoseconds(value)) + + +def header_now(frame_id: str = "") -> Header: + """Construct a wall-clock header; generated defaults themselves remain zero.""" + return Header(stamp=time_from_nanoseconds(time.time_ns()), frame_id=frame_id) + + +def message_header(message: Any) -> Header | None: + """Return source metadata from a generated stamped value.""" + header = getattr(message, "header", None) + return header if isinstance(header, Header) else None diff --git a/dimos/perception/depth2depth_cloud/module.py b/dimos/perception/depth2depth_cloud/module.py index 36d2cdc4d0d..917a3ef2b6c 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: diff --git a/dimos/perception/depth2depth_cloud/rust/Cargo.lock b/dimos/perception/depth2depth_cloud/rust/Cargo.lock index 57ec25d21e6..f33bea2c3f7 100644 --- a/dimos/perception/depth2depth_cloud/rust/Cargo.lock +++ b/dimos/perception/depth2depth_cloud/rust/Cargo.lock @@ -831,8 +831,8 @@ name = "dimos-depth2depth-cloud" version = "0.1.0" dependencies = [ "depth2depth", + "dimos-generated-messages", "dimos-module", - "lcm-msgs", "nalgebra", "rayon", "serde", @@ -842,10 +842,21 @@ dependencies = [ "validator", ] +[[package]] +name = "dimos-generated-messages" +version = "0.0.14" +dependencies = [ + "heapless", + "re_cdr", + "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", @@ -855,13 +866,13 @@ dependencies = [ [[package]] name = "dimos-module" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos?rev=6663bcb6e5e18f43a6cb9cf77ccbec613e310443#6663bcb6e5e18f43a6cb9cf77ccbec613e310443" dependencies = [ "dimos-lcm", "dimos-module-macros", "lcm-msgs", "nalgebra", "rayon", + "re_cdr", "serde", "serde_json", "tokio", @@ -875,7 +886,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 +1626,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" @@ -1660,6 +1679,17 @@ version = "0.17.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ed5909b6e89a2db4456e54cd5f673791d7eca6732202bbf2a9cc504fe2f9b84a" +[[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" @@ -2078,7 +2108,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 +2858,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" @@ -3162,6 +3202,18 @@ dependencies = [ "yasna", ] +[[package]] +name = "re_cdr" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bba10c21433b597481f366b978cdf2617e56785b38cb0dcc3833c6b8ad7e061a" +dependencies = [ + "bytemuck", + "byteorder", + "serde", + "thiserror 2.0.20", +] + [[package]] name = "reborrow" version = "0.5.5" @@ -3284,6 +3336,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 +3635,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" diff --git a/dimos/perception/depth2depth_cloud/rust/Cargo.toml b/dimos/perception/depth2depth_cloud/rust/Cargo.toml index 08b0649468f..0029d419dd0 100644 --- a/dimos/perception/depth2depth_cloud/rust/Cargo.toml +++ b/dimos/perception/depth2depth_cloud/rust/Cargo.toml @@ -21,17 +21,17 @@ 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" [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" } -lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } +dimos-module = { path = "../../../../native/rust/dimos-module", features = ["cdr"] } +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..677538f3a27 100644 --- a/dimos/perception/depth2depth_cloud/rust/crate-hashes.json +++ b/dimos/perception/depth2depth_cloud/rust/crate-hashes.json @@ -1,6 +1,4 @@ { - "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=" } \ 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..2822a8acf08 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,32 @@ 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/b971b729786cc12dcf4769cfbc233b0e36f1feee"; + sha256 = "20abf8673b36a6fdea3c30b6949fe7e618c2836917c34f39425ef466609a5171"; + }; + 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/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..a36b21d1366 100644 --- a/dimos/perception/depth2depth_cloud/rust/src/module.rs +++ b/dimos/perception/depth2depth_cloud/rust/src/module.rs @@ -31,10 +31,15 @@ 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 +47,122 @@ use tracing::info; use crate::undistort::{Lens, UndistortMap}; +fn encode_cloud(cloud: &PointCloud2) -> Vec { + cdr::encode(cloud).expect("serialize a valid generated PointCloud2") +} + +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, + ) +} + +fn decode_tf(bytes: &[u8]) -> std::io::Result> { + use dimos_generated_messages::tf2_msgs::msg::tf_message::TFMessage; + use nalgebra::{Quaternion, Translation3, UnitQuaternion}; + let message: TFMessage = cdr::decode(bytes)?; + message + .transforms + .into_iter() + .map(|edge| { + let p = edge.transform.translation; + let q = edge.transform.rotation; + if ![p.x, p.y, p.z, q.x, q.y, q.z, q.w] + .iter() + .all(|v| v.is_finite()) + { + return Err(std::io::Error::new( + std::io::ErrorKind::InvalidData, + "nonfinite TF components", + )); + } + let rotation = UnitQuaternion::try_new(Quaternion::new(q.w, q.x, q.y, q.z), 1e-9) + .ok_or_else(|| { + std::io::Error::new(std::io::ErrorKind::InvalidData, "zero TF quaternion") + })?; + if edge.header.stamp.nanosec >= 1_000_000_000 { + return Err(std::io::Error::new( + std::io::ErrorKind::InvalidData, + "unnormalized TF stamp", + )); + } + Ok(dimos_module::tf::Transform::from_nanoseconds( + edge.header.frame_id, + edge.child_frame_id, + edge.header.stamp.sec as i64 * 1_000_000_000 + edge.header.stamp.nanosec as i64, + Isometry3::from_parts(Translation3::new(p.x, p.y, p.z), rotation), + )) + }) + .collect() +} + +fn encode_tf(transforms: &[dimos_module::tf::Transform]) -> std::io::Result> { + use dimos_generated_messages::{ + geometry_msgs::msg::{ + quaternion::Quaternion, transform::Transform, transform_stamped::TransformStamped, + vector3::Vector3, + }, + tf2_msgs::msg::tf_message::TFMessage, + }; + let transforms = transforms + .iter() + .map(|value| { + let ns = value.stamp_nanoseconds().ok_or_else(|| { + std::io::Error::new( + std::io::ErrorKind::InvalidInput, + "TF timestamp is outside range", + ) + })?; + let sec = i32::try_from(ns.div_euclid(1_000_000_000)).map_err(|_| { + std::io::Error::new( + std::io::ErrorKind::InvalidInput, + "TF seconds exceed ROS int32", + ) + })?; + let p = value.translation(); + let q = value.rotation(); + Ok(TransformStamped { + header: Header { + stamp: Time { + sec, + nanosec: ns.rem_euclid(1_000_000_000) as u32, + }, + frame_id: value.parent.clone(), + }, + child_frame_id: value.child.clone(), + transform: Transform { + translation: Vector3 { + x: p.x, + y: p.y, + z: p.z, + }, + rotation: Quaternion { + x: q.i, + y: q.j, + z: q.k, + w: q.w, + }, + }, + }) + }) + .collect::>>()?; + cdr::encode(&TFMessage { transforms }) +} + 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; @@ -122,19 +243,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(decode = decode_tf, encode = encode_tf)] tf: Tf, - #[output(encode = PointCloud2::encode)] + #[output(encode = encode_cloud)] depth_cloud: Output, #[config] @@ -418,8 +539,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 @@ -712,10 +833,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,16 +846,15 @@ 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, } @@ -754,7 +874,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 +944,77 @@ 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(&encode_cloud(&cloud)).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 cdr_tf_adapter_preserves_frames_and_rejects_wrong_encoding() { + let transform = dimos_module::tf::Transform::new( + "world", + "camera", + -0.25, + Isometry3::translation(1., 2., 3.), + ); + let decoded = decode_tf(&encode_tf(&[transform]).unwrap()).unwrap(); + assert_eq!(decoded.len(), 1); + assert_eq!(decoded[0].parent, "world"); + assert_eq!(decoded[0].child, "camera"); + assert_eq!(decoded[0].ts, -0.25); + assert_eq!(decoded[0].translation(), nalgebra::Vector3::new(1., 2., 3.)); + assert!(decode_tf(&[0, 3, 0, 0]).is_err()); + } + + #[test] + fn tf_cdr_source_stamps_survive_epoch_nanoseconds_and_negative_times() { + use dimos_generated_messages::tf2_msgs::msg::tf_message::TFMessage; + for ns in [0, -1, 1_700_000_000_123_456_789] { + let value = dimos_module::tf::Transform::from_nanoseconds( + "world", + "camera", + ns, + Isometry3::translation(1., 2., 3.), + ); + let message: TFMessage = cdr::decode(&encode_tf(&[value]).unwrap()).unwrap(); + assert_eq!( + message.transforms[0].header.stamp.sec as i64, + ns.div_euclid(1_000_000_000) + ); + assert_eq!( + message.transforms[0].header.stamp.nanosec as i64, + ns.rem_euclid(1_000_000_000) + ); + for little in [false, true] { + let bytes = cdr::encode_endian(&message, little).unwrap(); + let decoded = decode_tf(&bytes).unwrap(); + assert_eq!(decoded[0].stamp_nanoseconds(), Some(ns)); + let output: TFMessage = cdr::decode(&encode_tf(&decoded).unwrap()).unwrap(); + assert_eq!(output, message); + } + } + } + #[test] fn a_stage_behind_takes_the_newest_item_and_counts_the_ones_it_missed() { let latest = Latest::default(); @@ -845,8 +1036,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 +1049,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 +1061,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..65bd5fd81ae 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.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.msgs.camera_info import camera_info_from_yaml +from dimos.msgs.geometry import transform_from_pose +from dimos.msgs.time import to_nanoseconds +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..5198f7b5d52 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.messages.sensor_msgs.msg import Image from dimos.models.base import HuggingFaceModel, HuggingFaceModelConfig -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_to_rgb +from dimos.msgs.time import to_seconds 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..38fa172815b 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.messages.sensor_msgs.msg import Image from dimos.models.base import HuggingFaceModel, HuggingFaceModelConfig -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_to_rgb +from dimos.msgs.time import to_seconds 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..e5e58dd1549 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.messages.sensor_msgs.msg import Image from dimos.models.base import HuggingFaceModel, HuggingFaceModelConfig -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_to_rgb +from dimos.msgs.time import to_nanoseconds, to_seconds 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..c8eef6f48bf 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.messages.sensor_msgs.msg import Image +from dimos.msgs.image import image_to_bgr 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..42ea4d1473a --- /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.messages.builtin_interfaces.msg import Time +from dimos.messages.std_msgs.msg import Header +from dimos.messages.vision_msgs.msg import Detection2DArray +from dimos.msgs.image import image_from_array, image_view +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..183b8066937 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.messages.sensor_msgs.msg import Image +from dimos.msgs.image import image_to_bgr 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..c51f2ab2a7c 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.messages.sensor_msgs.msg import Image +from dimos.msgs.image import image_to_bgr 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..af8fe3f3e23 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.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.msgs.image import image_sharpness +from dimos.msgs.time import to_seconds 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..c034fd57657 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.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.msgs.time import to_seconds 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..7140484d517 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,48 @@ 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.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.msgs.geometry import compose_transforms, pose_from_transform, transform_from_pose +from dimos.msgs.time import to_nanoseconds 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 +92,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 +109,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 +134,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..c4fb96b9a3c 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.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.geometry import ( + compose_transforms, + inverse_transform, + transform_matrix, +) +from dimos.msgs.image import image_view +from dimos.msgs.time import time_from_seconds 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..17219e38cda 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.messages.sensor_msgs.msg import Image +from dimos.messages.vision_msgs.msg import Detection2DArray +from dimos.msgs.time import to_nanoseconds 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..99b747131c2 --- /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.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.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.image import image_from_array +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..91ef1f9a0f3 100644 --- a/dimos/perception/detection/type/detection2d/bbox.py +++ b/dimos/perception/detection/type/detection2d/bbox.py @@ -18,13 +18,20 @@ 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.messages.vision_msgs.msg import ( BoundingBox2D, Detection2D as ROSDetection2D, ObjectHypothesis, @@ -32,12 +39,9 @@ Point2D, Pose2D, ) -from rich.console import Console -from rich.text import Text - -from dimos.msgs.std_msgs.Header import Header +from dimos.msgs.image import image_from_array, image_to_bgr, image_view +from dimos.msgs.time import to_seconds 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..cbad18c261a 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.messages.sensor_msgs.msg import Image +from dimos.msgs.time import to_seconds 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..5a4c7de28cb 100644 --- a/dimos/perception/detection/type/detection2d/point.py +++ b/dimos/perception/detection/type/detection2d/point.py @@ -17,7 +17,10 @@ from dataclasses import dataclass from typing import TYPE_CHECKING -from dimos_lcm.vision_msgs import ( +import numpy as np + +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 +28,11 @@ Point2D, Pose2D, ) - -from dimos.msgs.std_msgs.Header import Header +from dimos.msgs.image import image_from_array, image_view 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..8d7879c86a5 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.msgs.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..312841ff60a 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.msgs.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..e2d90c3655e --- /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.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.msgs.image import image_from_array, image_view +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..8a0b80cb436 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.msgs.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..9824e2dec1d 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.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.msgs.geometry import point_distance 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..4f3edf1a122 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.msgs.pointcloud import select_points +from dimos.msgs.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..b685b5eac5c 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.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.msgs.geometry import inverse_transform, transform_matrix +from dimos.msgs.image import image_view +from dimos.msgs.pointcloud import ( + concatenate_clouds, + pointcloud_from_xyz, + pointcloud_xyz, + select_points, +) +from dimos.msgs.time import time_from_seconds, to_seconds 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..112baaca89c 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.messages.geometry_msgs.msg import TransformStamped +from dimos.messages.sensor_msgs.msg import CameraInfo, PointCloud2 +from dimos.msgs.geometry import inverse_transform, transform_matrix +from dimos.msgs.pointcloud import pointcloud_xyz, select_points + +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..d046edd4b14 --- /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.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.msgs.image import image_from_array +from dimos.msgs.pointcloud import pointcloud_view +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..787875c208c 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.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.msgs.image import image_from_array +from dimos.msgs.time import time_from_seconds, to_seconds 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..872ef314abe 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.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.msgs.geometry import transform_from_matrix +from dimos.msgs.image import image_from_array +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz +from dimos.msgs.time import time_from_seconds, to_seconds 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..1b52750499a 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.messages.vision_msgs.msg import Detection2DArray +from dimos.msgs.image import image_from_array, image_to_bgr +from dimos.msgs.time import to_seconds 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..8956f3d6469 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.msgs.geometry import transform_matrix +from dimos.msgs.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..af54ccea6ca 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.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.msgs.pointcloud import concatenate_clouds 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..6d06f01ca0a 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.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.msgs.geometry import quaternion_from_matrix, transform_matrix +from dimos.msgs.image import image_to_rgb, image_view +from dimos.msgs.pointcloud import ( + concatenate_clouds, + pointcloud_from_xyz, + pointcloud_from_xyz_rgb, + pointcloud_rgb, + pointcloud_to_open3d, + pointcloud_xyz, + voxel_downsample_cloud, +) +from dimos.msgs.time import time_from_seconds 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..e8cf858350d 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.msgs.pointcloud import pointcloud_from_xyz +from dimos.msgs.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..4397fc79a4c 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.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.msgs.image import image_from_array, image_view +from dimos.msgs.pointcloud import pointcloud_from_xyz, transform_cloud +from dimos.msgs.time import to_seconds 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..f14260a53c0 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.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.msgs.geometry import quaternion_from_euler +from dimos.msgs.image import image_from_array, image_to_rgb, image_view +from dimos.msgs.time import time_from_seconds, to_seconds +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..a83bd0f10c4 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.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.msgs.image import image_from_array, image_to_rgb +from dimos.msgs.time import time_from_seconds 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..9fef533934b 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 + +# 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.msgs.geometry import quaternion_from_euler +from dimos.msgs.image import image_from_array, image_to_rgb, image_view +from dimos.msgs.time import to_seconds 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..7b8a0d00262 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.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.msgs.image import image_sharpness, image_to_bgr, image_to_jpeg +from dimos.msgs.time import to_seconds +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..4953140d19b 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.messages.geometry_msgs.msg import Point +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.msgs.geometry import pose_from_transform, quaternion_euler +from dimos.msgs.image import image_to_bgr 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..148def62653 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.messages.sensor_msgs.msg import Image +from dimos.msgs.image import image_to_rgb 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..dcb1a6e560a 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.msgs.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..ddb08084554 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.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.msgs.image import image_sharpness +from dimos.msgs.time import header_now 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..010845850bd 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.msgs.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..f673073f041 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.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.msgs.image import image_from_array +from dimos.msgs.time import time_from_seconds +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..d3993fbedbb --- /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.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.msgs.image import image_view +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..53cecb53da9 --- /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.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.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.image import image_from_array +from dimos.msgs.pointcloud import pointcloud_from_xyz_rgb, pointcloud_rgb, pointcloud_xyz +from dimos.msgs.time import time_from_seconds +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..540fb4585d2 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.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.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.image import image_from_array +from dimos.msgs.pointcloud import pointcloud_xyz +from dimos.msgs.time import time_from_seconds +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..20e1e719c40 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.messages.std_msgs.msg import Header +from dimos.msgs.image import image_from_array +from dimos.msgs.time import time_from_seconds 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..876b8aab87c 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.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.msgs.image import image_from_array +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..c2939c2263e 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.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.msgs.camera_info import camera_info_from_yaml +from dimos.msgs.geometry import quaternion_from_euler +from dimos.msgs.time import time_from_nanoseconds 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..67f76179906 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.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.msgs.geometry import compose_transforms +from dimos.msgs.image import image_to_bgr +from dimos.msgs.time import to_seconds 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..6a5470aa80e 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.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.msgs.image import image_sharpness +from dimos.msgs.time import to_seconds 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..e0d8a78a571 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.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 +from dimos.msgs.geometry import quaternion_from_matrix _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..a5f977a4f73 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.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.msgs.time import to_nanoseconds 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..6e94bbc0ac7 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.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.geometry import transform_matrix +from dimos.msgs.image import image_from_array +from dimos.msgs.time import time_from_seconds 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..08cb2c9f8e7 --- /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.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.msgs.image import image_from_array +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..933519d61a9 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.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.msgs.image import image_from_array +from dimos.msgs.time import time_from_seconds 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..e0950fa3459 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.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.msgs.time import time_from_seconds, to_seconds 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..74c7be0fec0 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.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.msgs.image import image_from_array 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..d57425b34b8 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.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.msgs.time import time_from_seconds 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..b39701a570c 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.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.msgs.geometry import quaternion_from_euler +from dimos.msgs.time import time_from_seconds 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..1f1c67e6e5a 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.messages.sensor_msgs.msg import CameraInfo, Image +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.image import image_from_array +from dimos.msgs.time import time_from_seconds, to_seconds 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..b06c061cd55 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.messages.sensor_msgs.msg import PointCloud2 +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.geometry import inverse_transform +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz +from dimos.msgs.time import time_from_nanoseconds, to_nanoseconds 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..488b13b50f0 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.msgs.geometry import inverse_transform, vector_array +from dimos.msgs.image import image_brightness, image_sharpness +from dimos.msgs.pointcloud import pointcloud_xyz +from dimos.msgs.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..4b80c11b257 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.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.msgs.pointcloud import pointcloud_xyz +from dimos.msgs.time import time_from_seconds, to_seconds 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..aa8ba58c075 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.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.msgs.geometry import inverse_transform, pose_from_transform, vector_array +from dimos.msgs.image import image_view +from dimos.msgs.pointcloud import ( + pointcloud_from_rgbd, + pointcloud_from_xyz, + pointcloud_xyz, + transform_cloud, +) +from dimos.msgs.time import time_from_nanoseconds, to_nanoseconds 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..432614f0d0f 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.msgs.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..e7774ff4ef2 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.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.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.image import image_from_array +from dimos.msgs.time import time_from_seconds, to_seconds 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..dcb3fd07e6e --- /dev/null +++ b/dimos/perception/localize/test_rig.py @@ -0,0 +1,365 @@ +# 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.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.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.image import image_from_array +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz +from dimos.msgs.time import time_from_seconds, to_seconds +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.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.geometry import inverse_transform +from dimos.msgs.image import image_from_array +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.msgs.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, _camera_span, 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) + 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..5f23a7e6df6 100644 --- a/dimos/perception/localize/tools/tool_localize.py +++ b/dimos/perception/localize/tools/tool_localize.py @@ -47,8 +47,19 @@ 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.messages.geometry_msgs.msg import PoseStamped +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.geometry import ( + inverse_transform, + pose_from_transform, + pose_matrix, + transform_matrix, + vector_array, +) +from dimos.msgs.image import image_from_array, image_to_bgr, image_to_rgb +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz, voxel_downsample_cloud +from dimos.msgs.time import time_from_seconds 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 +166,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 +203,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 +217,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 +232,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 +254,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 +286,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 +301,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 +317,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 +344,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 +378,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 +406,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 +432,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 +449,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 +466,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 +491,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..17137fcabee 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.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.image import image_from_array 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..249f6d8f3f9 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.messages.sensor_msgs.msg import Image as CdrImage +from dimos.msgs.image import image_to_rgb, image_view 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..8985dd5283a 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.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.msgs.geometry import quaternion_from_euler +from dimos.msgs.time import time_from_nanoseconds 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..28e8fab6ae1 --- /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.messages.tf2_msgs.msg import TFMessage +from dimos.msgs.geometry import compose_transforms +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..aac5cd5cadb 100644 --- a/dimos/protocol/tf/test_tf.py +++ b/dimos/protocol/tf/test_tf.py @@ -14,92 +14,115 @@ # 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.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.msgs.geometry import quaternion_from_euler +from dimos.msgs.time import time_from_seconds, to_seconds 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 +130,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 +152,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 +175,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 +183,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 +223,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 +252,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 +277,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 +285,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 +310,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 +389,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 +404,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 +470,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 +499,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 +510,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 +568,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 +595,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 +632,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 +687,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 +705,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 +748,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 +772,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 +785,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 +800,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 +824,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 +837,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 +875,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 +890,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 +930,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,25 +940,95 @@ 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 finally: store.stop() + + +def test_buffer_retains_distinct_nanoseconds_and_owns_its_snapshots(): + buffer = TBuffer() + first = TransformStamped( + header=Header(frame_id="a", stamp=Time(sec=1700000000, nanosec=123456789)), + 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), + ), + ) + second = TransformStamped( + header=Header(frame_id="a", stamp=Time(sec=1700000000, nanosec=123456790)), + 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), + ), + ) + buffer.add(first) + buffer.add(second) + first.header.stamp.nanosec = 0 + assert len(buffer) == 2 + stored_first = buffer.first() + assert stored_first is not None + assert stored_first.header.stamp.nanosec == 123456789 + stored_first.header.stamp.nanosec = 1 + unchanged = buffer.first() + assert unchanged is not None + assert unchanged.header.stamp.nanosec == 123456789 + latest = buffer.get() + assert latest is not None + assert latest.header.stamp.nanosec == 123456790 + + +def test_buffer_drops_out_of_order_data_older_than_retained_window(): + buffer = TBuffer(buffer_size=1) + for sec in (10, 11, 1): + buffer.add( + TransformStamped( + header=Header(stamp=Time(sec=sec, nanosec=0), frame_id=""), + child_frame_id="", + 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), + ), + ) + ) + assert len(buffer) == 2 + first = buffer.first() + assert first is not None + assert first.header.stamp.sec == 10 + + +def test_buffer_preserves_negative_stamp(): + buffer = TBuffer() + buffer.add( + TransformStamped( + header=Header(stamp=Time(sec=-1, nanosec=500000000), frame_id=""), + child_frame_id="", + 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), + ), + ) + ) + result = buffer.get(time_point=-0.5, time_tolerance=0) + assert result is not None + assert result.header.stamp == Time(sec=-1, nanosec=500000000) diff --git a/dimos/protocol/tf/tf.py b/dimos/protocol/tf/tf.py index 03a4706deda..ae1bed1d58c 100644 --- a/dimos/protocol/tf/tf.py +++ b/dimos/protocol/tf/tf.py @@ -15,17 +15,29 @@ # limitations under the License. from collections import deque +from copy import deepcopy from functools import reduce +import math +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 typing import TYPE_CHECKING, Any, Protocol, cast, runtime_checkable + +from sortedcontainers import SortedDict # type: ignore[import-untyped] + +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.msgs.geometry import compose_transforms, inverse_transform, pose_from_transform +from dimos.msgs.time import time_from_seconds, to_nanoseconds, to_seconds 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 +60,64 @@ def get( child_frame: str, time_point: float | None = None, time_tolerance: float | None = None, - ) -> Transform | None: ... + ) -> TransformStamped | None: ... + +class TBuffer: + """A bounded time index of generated transforms, keyed by integer nanoseconds.""" -class TBuffer(InMemoryStore[Transform]): def __init__(self, buffer_size: float = 10.0) -> None: - super().__init__() + if math.isnan(buffer_size) or buffer_size <= 0: + raise ValueError("buffer_size must be positive") self.buffer_size = buffer_size + self._entries: SortedDict = SortedDict() + + def __len__(self) -> int: + return len(self._entries) + + def add(self, transform: TransformStamped) -> None: + stamp = to_nanoseconds(transform.header.stamp) + self._entries[stamp] = deepcopy(transform) + if math.isfinite(self.buffer_size): + oldest = self._entries.peekitem(-1)[0] - round(self.buffer_size * 1_000_000_000) + while self._entries and self._entries.peekitem(0)[0] < oldest: + self._entries.popitem(0) - def add(self, transform: Transform) -> None: - self.save(transform) - self.prune_old(transform.ts - self.buffer_size) + def first(self) -> TransformStamped | None: + return deepcopy(self._entries.peekitem(0)[1]) if self._entries else None - 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.""" + def last(self) -> TransformStamped | None: + return deepcopy(self._entries.peekitem(-1)[1]) if self._entries else None + + def get( + self, time_point: float | None = None, time_tolerance: float = 1.0 + ) -> TransformStamped | None: if time_point is None: return self.last() - return self.find_closest(time_point, time_tolerance) + stamp = to_nanoseconds(time_from_seconds(time_point)) + if math.isnan(time_tolerance) or time_tolerance < 0: + raise ValueError("time_tolerance must be nonnegative") + index = self._entries.bisect_left(stamp) + candidates = [ + self._entries.peekitem(i) for i in (index - 1, index) if 0 <= i < len(self._entries) + ] + if not candidates: + return None + nearest, result = min(candidates, key=lambda item: (abs(item[0] - stamp), -item[0])) + if abs(nearest - stamp) / 1_000_000_000 > time_tolerance: + return None + return deepcopy(cast("TransformStamped", result)) def __str__(self) -> str: - if len(self) == 0: + first, last = self.first(), self.last() + if first is None or last is None: 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)" + start, end = to_seconds(first.header.stamp), to_seconds(last.header.stamp) + return ( + f"TBuffer({first.header.frame_id} -> {first.child_frame_id}, " + f"{len(self)} msgs, {end - start:.2f}s " + f"[{to_human_readable(start)} - {to_human_readable(end)}])" + ) # stores multiple transform buffers @@ -97,10 +128,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 +165,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 +193,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 +203,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 +217,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 +226,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 +247,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 +277,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 +285,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 +294,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 +320,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 +370,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/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..d5615fbdc5c 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.messages.sensor_msgs.msg import Image +from dimos.msgs.image import image_from_array, image_to_bgr, image_to_rgb, image_view +from dimos.msgs.time import header_now 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..5e33aaacb09 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.messages.builtin_interfaces.msg import Time +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.image import image_from_array, image_to_rgb, image_view +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..c66f370f7a7 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.msgs.image import image_from_file +from dimos.msgs.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.msgs.image`, `dimos.msgs.pointcloud` and `dimos.msgs.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.msgs.image import image_from_file +from dimos.msgs.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.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.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..baa5dfce157 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.msgs.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..5c82203b42e 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.msgs.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.msgs.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/native/message-source.nix b/native/message-source.nix new file mode 100644 index 00000000000..31629077e14 --- /dev/null +++ b/native/message-source.nix @@ -0,0 +1,52 @@ +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +# Generate the built-in definitions through the same configuration and core as pip. +# Nix fetches fixed-hash inputs; the generation builder itself is offline. +{ pkgs }: +let + generatorWheel = pkgs.fetchurl { + url = "https://files.pythonhosted.org/packages/1a/05/cb085e18549a5eced615840b2f607a9c342a8a49c69012964f0568aa309b/dimos_cdrgen-0.1.1-py3-none-any.whl"; + sha256 = "f85923440196a3028dbd974a913b0ef9320eb12b57770f722e96bc1645a351a2"; + }; + generator = pkgs.runCommand "dimos-cdrgen-0.1.1-source" { nativeBuildInputs = [ pkgs.unzip ]; } '' + mkdir -p $out + unzip -q ${generatorWheel} -d $out + ''; + python = pkgs.python3.withPackages (p: [ + p.setuptools p.wheel p.pydantic p.empy p.lark p.rosbags p.numpy p.requests + ]); + product = pkgs.lib.fileset.toSource { + root = ../.; + fileset = pkgs.lib.fileset.unions [ + ../pyproject.toml + ../dimos/message_codegen/build.py + ../packages/dimos-generated/schema-sources.json + ../packages/dimos-generated/custom + ]; + }; + schemaLock = builtins.fromJSON (builtins.readFile ../packages/dimos-generated/schema-sources.json); + schemaArchives = map (source: { + name = builtins.hashString "sha256" source.url + ".tar.gz"; + archive = pkgs.fetchurl { inherit (source) url sha256; }; + }) schemaLock.sources; + source = pkgs.runCommand "dimos-generated-message-source" { nativeBuildInputs = [ python ]; } '' + cp -r ${product} product + chmod -R u+w product + mkdir -p cache/archives + ${pkgs.lib.concatMapStringsSep "\n" (item: "cp ${item.archive} cache/archives/${item.name}") schemaArchives} + export PYTHONPATH="$PWD/product:${generator}:$PYTHONPATH" + python3 - <<'PYTHON' +from pathlib import Path +from dimos.message_codegen.build import prepare_builtin_sources +from dimos_cdrgen.source_package import prepare +prepare_builtin_sources(Path("cache"), offline=True) +prepare(Path("product")) +PYTHON + cp -r product/build/message-source $out + test -f $out/cpp/CMakeLists.txt + test -f $out/rust/Cargo.toml + ''; +in { + inherit generator python source; +} diff --git a/native/rust/dimos-module-macros/src/lib.rs b/native/rust/dimos-module-macros/src/lib.rs index af89a0680b4..2ac7083fef4 100644 --- a/native/rust/dimos-module-macros/src/lib.rs +++ b/native/rust/dimos-module-macros/src/lib.rs @@ -194,7 +194,9 @@ enum FieldKind { handler: Ident, }, Config, - Tf, + Tf { + codecs: Option<(Path, Path)>, + }, State, } @@ -333,7 +335,12 @@ fn expand(input: DeriveInput) -> syn::Result { quote!(#name: builder.io(#name_str, #decode, #encode)) } FieldKind::Config => quote!(#name: config), - FieldKind::Tf => quote!(#name: builder.tf()), + FieldKind::Tf { codecs: None } => quote!(#name: builder.tf()), + FieldKind::Tf { + codecs: Some((decode, encode)), + } => { + quote!(#name: builder.tf_with_codec(#decode, #encode)) + } FieldKind::State => quote!(#name: ::core::default::Default::default()), } }); @@ -451,7 +458,7 @@ fn port_decls(classified: &[ClassifiedField], want_input: bool) -> Vec // goes: bake has to put the topic in the host's map or the module // refuses to start. It lists under `inputs` because the graph has // no io kind and a host mostly consumes tf. - FieldKind::Tf => want_input, + FieldKind::Tf { .. } => want_input, _ => false, }) .map(|f| PortDecl { @@ -461,7 +468,7 @@ fn port_decls(classified: &[ClassifiedField], want_input: bool) -> Vec // An explicit `msg` wins: a view type's wire payload and the dimos // message it stands for are allowed to differ. ty: match f.kind { - FieldKind::Tf => TF_PAYLOAD_TYPE.to_string(), + FieldKind::Tf { .. } => TF_PAYLOAD_TYPE.to_string(), _ => f .msg .clone() @@ -537,7 +544,7 @@ fn check_port_table(entry: &toml::Value, kind: &str, declared: &[PortDecl]) -> R let msg = msg .as_str() .ok_or_else(|| format!("`{kind}.{}` must be a string", port.name))?; - let want = msg.rsplit('.').next().unwrap_or(msg); + let want = msg.rsplit(['.', '/']).next().unwrap_or(msg); if want != port.ty { return Err(format!( "port `{}` is declared `{msg}` in `{kind}` but the struct field carries `{}`", @@ -649,7 +656,31 @@ fn classify_field(field: &Field, name: &Ident) -> syn::Result { if found.is_some() { return Err(syn::Error::new_spanned(attr, ONE_ATTR_ONLY)); } - found = Some(FieldKind::Tf); + let codecs = if matches!(&attr.meta, syn::Meta::Path(_)) { + None + } else { + let mut decode: Option = None; + let mut encode: Option = None; + attr.parse_nested_meta(|meta| { + if meta.path.is_ident("decode") { + decode = Some(meta.value()?.parse()?); + } else if meta.path.is_ident("encode") { + encode = Some(meta.value()?.parse()?); + } else { + return Err(meta.error("expected `decode = ...` or `encode = ...`")); + } + Ok(()) + })?; + Some(( + decode.ok_or_else(|| { + syn::Error::new_spanned(attr, "#[tf] requires both decode and encode") + })?, + encode.ok_or_else(|| { + syn::Error::new_spanned(attr, "#[tf] requires both decode and encode") + })?, + )) + }; + found = Some(FieldKind::Tf { codecs }); } } @@ -736,6 +767,38 @@ mod tests { }; use syn::parse_quote; + #[test] + fn tf_accepts_legacy_or_explicit_codec_pair_and_keeps_registry_type() { + for field in [ + parse_quote!(#[tf] tf: Tf), + parse_quote!(#[tf(decode = wire::decode, encode = wire::encode)] tf: Tf), + ] { + let attr = super::classify_field(&field, &parse_quote!(tf)).expect("valid TF field"); + let ty: syn::Type = parse_quote!(Tf); + let name = parse_quote!(tf); + let fields = [ClassifiedField { + name: &name, + ty: &ty, + kind: attr.kind, + msg: attr.msg, + }]; + let ports = super::port_decls(&fields, true); + assert_eq!(ports.len(), 1); + assert_eq!(ports[0].ty, "TFMessage"); + } + } + + #[test] + fn tf_rejects_partial_or_unknown_codec_settings() { + for field in [ + parse_quote!(#[tf(decode = wire::decode)] tf: Tf), + parse_quote!(#[tf(encode = wire::encode)] tf: Tf), + parse_quote!(#[tf(codec = wire::codec)] tf: Tf), + ] { + assert!(super::classify_field(&field, &parse_quote!(tf)).is_err()); + } + } + #[test] fn io_ports_are_rejected_by_the_registry_check() { let name: syn::LitStr = parse_quote!("demo"); @@ -790,6 +853,29 @@ mod tests { .expect("matching ports should pass"); } + #[test] + fn accepts_canonical_cdr_names_and_still_rejects_type_mismatch() { + let manifest = MANIFEST.replace("sensor_msgs.PointCloud2", "sensor_msgs/msg/PointCloud2"); + check_manifest_ports( + &manifest, + "demo", + &[port("lidar", "PointCloud2")], + &[port("global_map", "PointCloud2")], + ) + .expect("canonical ROS message names match their imported Rust type"); + let err = check_manifest_ports( + &manifest, + "demo", + &[port("lidar", "Image")], + &[port("global_map", "PointCloud2")], + ) + .expect_err("canonical names must still enforce the payload type"); + assert!( + err.contains("sensor_msgs/msg/PointCloud2") && err.contains("Image"), + "{err}" + ); + } + #[test] fn rejects_missing_metadata_section() { let err = check_manifest_ports(MANIFEST, "other", &[], &[]) diff --git a/native/rust/dimos-module/Cargo.lock b/native/rust/dimos-module/Cargo.lock index cbd83d0f707..5e580ae8159 100644 --- a/native/rust/dimos-module/Cargo.lock +++ b/native/rust/dimos-module/Cargo.lock @@ -226,6 +226,20 @@ name = "bytemuck" version = "1.25.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "95832e849adfb21180ccb6826a99da14e5d266ae5c2e668e1602cf234f153797" +dependencies = [ + "bytemuck_derive", +] + +[[package]] +name = "bytemuck_derive" +version = "1.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6a1f896587b6f2c069c73d2f0913e2d590c3990285cd2f0b6aa02b786b4c679c" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.5", +] [[package]] name = "byteorder" @@ -582,7 +596,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,6 +612,7 @@ dependencies = [ "lcm-msgs", "nalgebra", "rayon", + "re_cdr", "serde", "serde_json", "tokio", @@ -1385,7 +1400,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", ] @@ -2164,6 +2179,18 @@ dependencies = [ "yasna", ] +[[package]] +name = "re_cdr" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bba10c21433b597481f366b978cdf2617e56785b38cb0dcc3833c6b8ad7e061a" +dependencies = [ + "bytemuck", + "byteorder", + "serde", + "thiserror", +] + [[package]] name = "redox_syscall" version = "0.5.18" diff --git a/native/rust/dimos-module/Cargo.toml b/native/rust/dimos-module/Cargo.toml index b79f793a92c..f71f91de9e8 100644 --- a/native/rust/dimos-module/Cargo.toml +++ b/native/rust/dimos-module/Cargo.toml @@ -7,10 +7,14 @@ license = "Apache-2.0" [workspace] +[features] +cdr = ["dep:re_cdr"] + [dependencies] -dimos-lcm = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } +re_cdr = { version = "=0.1.0", optional = true } +dimos-lcm = { git = "https://github.com/dimensionalOS/dimos-lcm.git", rev = "04d78e8622500244123ba9cefa4c51b4cb454549" } dimos-module-macros = { version = "=0.1.0", path = "../dimos-module-macros" } -lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } +lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", rev = "04d78e8622500244123ba9cefa4c51b4cb454549" } nalgebra = "0.35.0" rayon = "1" tokio = { version = "1", features = ["rt-multi-thread", "macros", "sync", "time", "signal", "io-std", "io-util"] } diff --git a/native/rust/dimos-module/src/cdr.rs b/native/rust/dimos-module/src/cdr.rs new file mode 100644 index 00000000000..7335e795483 --- /dev/null +++ b/native/rust/dimos-module/src/cdr.rs @@ -0,0 +1,95 @@ +// 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. + +//! XCDR1 transport framing around the upstream Serde CDR codec. +use re_cdr::{BigEndian, LittleEndian}; +use serde::{de::DeserializeOwned, Serialize}; +use std::io; + +pub fn encode(message: &T) -> io::Result> { + encode_endian(message, true) +} + +pub fn encode_endian(message: &T, little_endian: bool) -> io::Result> { + let body = if little_endian { + re_cdr::to_vec::<_, LittleEndian>(message) + } else { + re_cdr::to_vec::<_, BigEndian>(message) + } + .map_err(|error| io::Error::new(io::ErrorKind::InvalidInput, error.to_string()))?; + let mut bytes = Vec::with_capacity(4 + body.len()); + bytes.extend_from_slice(&[0, u8::from(little_endian), 0, 0]); + bytes.extend(body); + Ok(bytes) +} + +pub fn decode(bytes: &[u8]) -> io::Result { + if bytes.len() < 4 || bytes[0] != 0 || bytes[1] > 1 || bytes[2] != 0 || bytes[3] != 0 { + return Err(io::Error::new( + io::ErrorKind::InvalidData, + "Expected plain CDR/XCDR1 encapsulation", + )); + } + let (value, consumed) = if bytes[1] == 1 { + re_cdr::from_bytes::(&bytes[4..]) + } else { + re_cdr::from_bytes::(&bytes[4..]) + } + .map_err(|error| io::Error::new(io::ErrorKind::InvalidData, error.to_string()))?; + if consumed != bytes.len() - 4 { + return Err(io::Error::new( + io::ErrorKind::InvalidData, + "Trailing bytes after CDR message", + )); + } + Ok(value) +} + +#[cfg(test)] +mod tests { + use super::*; + use serde::{Deserialize, Serialize}; + + #[derive(Debug, PartialEq, Serialize, Deserialize)] + struct Int32 { + data: i32, + } + + #[test] + fn literal_bytes_match_ros_cdr_in_both_byte_orders() { + let value = Int32 { data: 42 }; + assert_eq!(encode(&value).unwrap(), vec![0, 1, 0, 0, 42, 0, 0, 0]); + assert_eq!( + encode_endian(&value, false).unwrap(), + vec![0, 0, 0, 0, 0, 0, 0, 42] + ); + assert_eq!(decode::(&[0, 1, 0, 0, 42, 0, 0, 0]).unwrap(), value); + assert_eq!(decode::(&[0, 0, 0, 0, 0, 0, 0, 42]).unwrap(), value); + } + + #[test] + fn reject_wrong_encapsulation_truncation_and_trailing_bytes() { + for bytes in [ + vec![], + vec![0, 1, 0, 0, 42], + vec![0, 3, 0, 0, 42, 0, 0, 0], + vec![0, 1, 0, 0, 42, 0, 0, 0, 0], + ] { + assert_eq!( + decode::(&bytes).unwrap_err().kind(), + io::ErrorKind::InvalidData + ); + } + } +} diff --git a/native/rust/dimos-module/src/lib.rs b/native/rust/dimos-module/src/lib.rs index 5816445c3f4..69ef3fcdac5 100644 --- a/native/rust/dimos-module/src/lib.rs +++ b/native/rust/dimos-module/src/lib.rs @@ -17,6 +17,8 @@ #[cfg(test)] extern crate self as dimos_module; +#[cfg(feature = "cdr")] +pub mod cdr; pub mod host; pub mod lcm; pub mod log; diff --git a/native/rust/dimos-module/src/module.rs b/native/rust/dimos-module/src/module.rs index 23fe965ba36..9ee5113ad64 100644 --- a/native/rust/dimos-module/src/module.rs +++ b/native/rust/dimos-module/src/module.rs @@ -419,13 +419,35 @@ impl Builder { /// The graph fills in the background as `tf` messages arrive. Repeated calls /// share one graph. pub fn tf(&mut self) -> crate::tf::Tf { + self.tf_with_codec(crate::tf::decode_legacy, crate::tf::encode_legacy) + } + + /// Select TF wire codecs while retaining the same graph and lookup API. + /// Repeated calls share one graph and must select the same codec pair. + /// + /// # Panics + /// Panics if this module already selected a different pair for its TF topic. + pub fn tf_with_codec( + &mut self, + decode: crate::tf::DecodeTransforms, + encode: crate::tf::EncodeTransforms, + ) -> crate::tf::Tf { if let Some(tf) = &self.tf { + assert!( + tf.uses_codec(decode, encode), + "one TF topic cannot mix codec pairs" + ); return tf.clone(); } let topic = self.topic_for("tf"); let sender = self.add_publisher(&topic); - let (tf, route) = - crate::tf::tf_subscription(topic.clone(), crate::tf::DEFAULT_TF_WINDOW_SECS, sender); + let (tf, route) = crate::tf::tf_subscription_with_codec( + topic.clone(), + crate::tf::DEFAULT_TF_WINDOW_SECS, + sender, + decode, + encode, + ); self.routes.entry(topic).or_default().push(route); self.tf = Some(tf.clone()); tf @@ -1048,6 +1070,82 @@ mod tests { assert_eq!(builder.routes.get("/tf").map(Vec::len), Some(1)); } + fn encode_tagged_tf(values: &[crate::tf::Transform]) -> io::Result> { + let mut bytes = b"test-tf:".to_vec(); + bytes.extend(crate::tf::encode_legacy(values)?); + Ok(bytes) + } + + fn decode_tagged_tf(bytes: &[u8]) -> io::Result> { + crate::tf::decode_legacy(bytes.strip_prefix(b"test-tf:").ok_or_else(|| { + io::Error::new(io::ErrorKind::InvalidData, "missing test codec prefix") + })?) + } + + #[derive(crate::Module)] + struct TaggedTfModule { + #[tf(decode = decode_tagged_tf, encode = encode_tagged_tf)] + tf: crate::tf::Tf, + } + + #[tokio::test] + async fn derived_tf_field_uses_selected_codecs_for_publish_and_receive() { + let mut sender = builder_with_topics(&[("tf", "/robot/tf")]); + let mut receiver = builder_with_topics(&[("tf", "/robot/tf")]); + let producer = TaggedTfModule::build(&mut sender, NoConfig); + let consumer = TaggedTfModule::build(&mut receiver, NoConfig); + let edge = crate::tf::Transform::new( + "map", + "camera", + 3.25, + nalgebra::Isometry3::translation(1.0, 2.0, 3.0), + ); + producer.tf.publish(&[edge]).await.unwrap(); + let bytes = sender.outputs[0].1.recv().await.unwrap(); + assert!(bytes.starts_with(b"test-tf:")); + receiver.routes["/robot/tf"][0].try_dispatch(b"wrong codec"); + assert!(consumer.tf.get_latest("map", "camera").is_none()); + receiver.routes["/robot/tf"][0].try_dispatch(&bytes); + let found = consumer.tf.get_latest("map", "camera").unwrap(); + assert_eq!(found.translation(), nalgebra::Vector3::new(1.0, 2.0, 3.0)); + assert_eq!(found.ts, 3.25); + receiver.tf_with_codec(decode_tagged_tf, encode_tagged_tf); + assert_eq!(receiver.outputs.len(), 1); + assert_eq!(receiver.routes["/robot/tf"].len(), 1); + } + + #[test] + #[should_panic(expected = "one TF topic cannot mix codec pairs")] + fn tf_rejects_mixed_codecs_on_one_topic() { + let mut builder = builder_with_topics(&[("tf", "/tf")]); + builder.tf_with_codec(decode_tagged_tf, encode_tagged_tf); + builder.tf(); + } + + #[tokio::test] + async fn tf_encode_error_does_not_publish_or_change_the_local_graph() { + fn reject(_: &[crate::tf::Transform]) -> io::Result> { + Err(io::Error::new( + io::ErrorKind::InvalidData, + "unencodable transform", + )) + } + let mut builder = builder_with_topics(&[("tf", "/tf")]); + let tf = builder.tf_with_codec(decode_tagged_tf, reject); + let edge = + crate::tf::Transform::new("map", "camera", 3.25, nalgebra::Isometry3::identity()); + let error = tf + .publish(&[edge]) + .await + .expect_err("codec error must reach the caller"); + assert_eq!(error.kind(), io::ErrorKind::InvalidData); + assert!(tf.get_latest("map", "camera").is_none()); + assert!(matches!( + builder.outputs[0].1.try_recv(), + Err(tokio::sync::mpsc::error::TryRecvError::Empty) + )); + } + // recv/publish concurrency #[tokio::test(flavor = "multi_thread", worker_threads = 2)] diff --git a/native/rust/dimos-module/src/pointcloud.rs b/native/rust/dimos-module/src/pointcloud.rs index 31b8ef0aee6..1e6b7ddba7a 100644 --- a/native/rust/dimos-module/src/pointcloud.rs +++ b/native/rust/dimos-module/src/pointcloud.rs @@ -58,45 +58,78 @@ pub fn extract_xyz(msg: &PointCloud2) -> Result, ExtractError> { if step == 0 { return Err(ExtractError("point_step is 0")); } - let n = count(msg.width, "negative width")? - .checked_mul(count(msg.height, "negative height")?) - .ok_or(ExtractError("width*height overflows"))?; - let needed = n + let width = count(msg.width, "negative width")?; + let height = count(msg.height, "negative height")?; + let row_step = width .checked_mul(step) - .ok_or(ExtractError("width*height*point_step overflows"))?; - if msg.data.len() < needed { - return Err(ExtractError( - "data buffer shorter than width*height*point_step", - )); - } - for off in [xo, yo, zo] { - if off.checked_add(4).is_none_or(|end| end > step) { + .ok_or(ExtractError("row size overflows"))?; + extract_xyz_rows( + &msg.data, + [width, height], + [step, row_step], + [xo, yo, zo], + false, + ) +} + +/// Read finite float32 xyz values from validated field offsets, respecting row padding. +/// Message adapters retain their own schema types and pass only the wire layout here. +pub fn extract_xyz_rows( + data: &[u8], + dimensions: [usize; 2], + steps: [usize; 2], + offsets: [usize; 3], + big_endian: bool, +) -> Result, ExtractError> { + let [width, height] = dimensions; + let [step, row_step] = steps; + if step == 0 { + return Err(ExtractError("point_step is 0")); + } + let packed = width + .checked_mul(step) + .ok_or(ExtractError("row size overflows"))?; + if row_step < packed { + return Err(ExtractError("row_step is shorter than a row")); + } + let needed = height + .checked_mul(row_step) + .ok_or(ExtractError("row buffer size overflows"))?; + if data.len() < needed { + return Err(ExtractError("data buffer shorter than rows")); + } + for offset in offsets { + if offset.checked_add(4).is_none_or(|end| end > step) { return Err(ExtractError( "xyz field offsets do not fit within point_step", )); } } - - let mut out = Vec::with_capacity(n); - for base in (0..needed).step_by(step) { - let x = read_f32_le(&msg.data, base + xo); - let y = read_f32_le(&msg.data, base + yo); - let z = read_f32_le(&msg.data, base + zo); - if x.is_finite() && y.is_finite() && z.is_finite() { - out.push([x, y, z]); + let mut out = Vec::new(); + if width == 0 || height == 0 { + return Ok(out); + } + for row in 0..height { + for column in 0..width { + let base = row * row_step + column * step; + let point = offsets.map(|offset| { + let bytes: [u8; 4] = data[base + offset..base + offset + 4] + .try_into() + .expect("bounds checked above"); + if big_endian { + f32::from_be_bytes(bytes) + } else { + f32::from_le_bytes(bytes) + } + }); + if point.iter().all(|value| value.is_finite()) { + out.push(point); + } } } Ok(out) } -#[inline] -fn read_f32_le(buf: &[u8], off: usize) -> f32 { - let bytes: [u8; 4] = buf[off..off + 4] - .try_into() - .expect("bounds checked by caller"); - f32::from_le_bytes(bytes) -} - #[cfg(test)] mod tests { use super::*; @@ -129,6 +162,40 @@ mod tests { } } + #[test] + fn padded_rows_and_both_byte_orders_preserve_finite_points() { + for big_endian in [false, true] { + let mut bytes = Vec::new(); + for xyz in [[1.0f32, 2.0, 3.0], [4.0, 5.0, 6.0]] { + for value in xyz { + bytes.extend(if big_endian { + value.to_be_bytes() + } else { + value.to_le_bytes() + }); + } + bytes.extend([255u8; 4]); + } + assert_eq!( + extract_xyz_rows(&bytes, [1, 2], [12, 16], [0, 4, 8], big_endian).unwrap(), + vec![[1., 2., 3.], [4., 5., 6.]] + ); + } + } + + #[test] + fn malformed_row_layouts_are_rejected_before_allocation_or_indexing() { + for (dimensions, steps, offsets) in [ + ([1, 1], [12, 11], [0, 4, 8]), + ([1, 2], [12, 16], [0, 4, 8]), + ([1, 1], [12, 12], [0, 4, usize::MAX]), + ([usize::MAX, 1], [12, 12], [0, 4, 8]), + ([1, usize::MAX], [12, 16], [0, 4, 8]), + ] { + assert!(extract_xyz_rows(&[0; 16], dimensions, steps, offsets, false).is_err()); + } + } + #[test] fn drops_non_finite_points_and_moves_nothing() { let cloud = cloud_of(&[[1.0, 2.0, 0.3], [f32::NAN, 0.0, 0.0], [0.0, 0.0, 0.0]]); diff --git a/native/rust/dimos-module/src/tf.rs b/native/rust/dimos-module/src/tf.rs index a6c1611f9e3..04865ef86c3 100644 --- a/native/rust/dimos-module/src/tf.rs +++ b/native/rust/dimos-module/src/tf.rs @@ -45,6 +45,7 @@ pub struct Transform { pub parent: String, pub child: String, pub ts: f64, + stamp_ns: Option, iso: Isometry3, } @@ -59,10 +60,40 @@ impl Transform { parent: parent.into(), child: child.into(), ts, + stamp_ns: None, iso, } } + /// Retain a wire timestamp without passing its nanoseconds through a float. + /// The existing `ts` field remains available for seconds-based math. + pub fn from_nanoseconds( + parent: impl Into, + child: impl Into, + stamp_ns: i64, + iso: Isometry3, + ) -> Self { + Self { + parent: parent.into(), + child: child.into(), + ts: stamp_ns as f64 / 1_000_000_000., + stamp_ns: Some(stamp_ns), + iso, + } + } + + /// Exact source time, or rounded seconds for values made with `new`. + /// A caller updating the public `ts` field invalidates saved wire metadata. + pub fn stamp_nanoseconds(&self) -> Option { + if let Some(ns) = self.stamp_ns { + if self.ts == ns as f64 / 1_000_000_000. { + return Some(ns); + } + } + let ns = (self.ts * 1_000_000_000.).round(); + (ns.is_finite() && ns >= i64::MIN as f64 && ns < i64::MAX as f64).then_some(ns as i64) + } + pub fn translation(&self) -> Vector3 { self.iso.translation.vector } @@ -76,6 +107,7 @@ impl Transform { parent: self.child.clone(), child: self.parent.clone(), ts: self.ts, + stamp_ns: self.stamp_ns, iso: self.iso.inverse(), } } @@ -85,6 +117,7 @@ impl Transform { parent: self.parent.clone(), child: other.child.clone(), ts: self.ts, + stamp_ns: self.stamp_ns, iso: self.iso * other.iso, } } @@ -92,6 +125,7 @@ impl Transform { struct Sample { ts: f64, + stamp_ns: Option, iso: Isometry3, } @@ -110,6 +144,10 @@ impl TBuffer { } fn add(&mut self, ts: f64, iso: Isometry3) { + self.add_stamped(ts, iso, None); + } + + fn add_stamped(&mut self, ts: f64, iso: Isometry3, stamp_ns: Option) { // A stamp a whole window behind the newest is a clock reset, not jitter. if self .samples @@ -118,8 +156,15 @@ impl TBuffer { { self.samples.clear(); } - let pos = self.samples.partition_point(|s| s.ts <= ts); - self.samples.insert(pos, Sample { ts, iso }); + let pos = self.samples.partition_point(|s| { + s.ts < ts + || (s.ts == ts + && match (s.stamp_ns, stamp_ns) { + (Some(a), Some(b)) => a <= b, + _ => true, + }) + }); + self.samples.insert(pos, Sample { ts, stamp_ns, iso }); // Anchored to the newest sample so a late message cannot widen the window. let newest = self.samples.back().map_or(ts, |s| s.ts); self.prune(newest - self.window_secs); @@ -175,6 +220,7 @@ impl TBuffer { parent: parent.to_string(), child: child.to_string(), ts: s.ts, + stamp_ns: s.stamp_ns, iso: s.iso, }) } @@ -202,6 +248,23 @@ impl MultiTBuffer { .add(ts, iso); } + fn receive_transform(&mut self, transform: &Transform) { + if transform.stamp_ns.is_none() { + self.receive( + &transform.parent, + &transform.child, + transform.ts, + transform.iso, + ); + return; + } + let window_secs = self.window_secs; + self.buffers + .entry((transform.parent.clone(), transform.child.clone())) + .or_insert_with(|| TBuffer::new(window_secs)) + .add_stamped(transform.ts, transform.iso, transform.stamp_nanoseconds()); + } + fn connections(&self, frame: &str) -> Vec { let mut out = Vec::new(); for (parent, child) in self.buffers.keys() { @@ -227,6 +290,7 @@ impl MultiTBuffer { parent: parent.to_string(), child: child.to_string(), ts: time.unwrap_or_else(now_secs), + stamp_ns: None, iso: Isometry3::identity(), }); } @@ -253,11 +317,17 @@ impl MultiTBuffer { } let path = self.bfs(parent, child, time, tolerance)?; // A composition is only as fresh as its stalest edge. - let oldest = path.iter().map(|t| t.ts).fold(f64::INFINITY, f64::min); + let oldest = path.iter().min_by(|a, b| { + a.ts.total_cmp(&b.ts) + .then_with(|| a.stamp_nanoseconds().cmp(&b.stamp_nanoseconds())) + })?; + let oldest_ts = oldest.ts; + let oldest_ns = oldest.stamp_nanoseconds(); let mut steps = path.into_iter(); let first = steps.next()?; let mut composed = steps.fold(first, |acc, step| acc.compose(&step)); - composed.ts = oldest; + composed.ts = oldest_ts; + composed.stamp_ns = oldest_ns; Some(composed) } @@ -338,6 +408,11 @@ impl Graph { } } +/// Decode a wire packet into the existing transform graph values. +pub type DecodeTransforms = fn(&[u8]) -> io::Result>; +/// Encode graph values for publication on the selected TF wire format. +pub type EncodeTransforms = fn(&[Transform]) -> io::Result>; + /// A cheap-to-clone handle for querying and publishing transforms. /// /// Obtain one from `Builder::tf` (or a `#[tf]` field on a `#[derive(Module)]` @@ -346,9 +421,15 @@ impl Graph { pub struct Tf { graph: Arc, sender: mpsc::Sender>, + decode: DecodeTransforms, + encode: EncodeTransforms, } impl Tf { + pub(crate) fn uses_codec(&self, decode: DecodeTransforms, encode: EncodeTransforms) -> bool { + std::ptr::fn_addr_eq(self.decode, decode) && std::ptr::fn_addr_eq(self.encode, encode) + } + /// Start a lookup of the transform from `parent` to `child`. /// /// Refine it with [`Lookup::at`] and [`Lookup::tolerance`], then finish with @@ -379,15 +460,13 @@ impl Tf { /// The transforms also feed the local graph, so a lookup right after sees /// them without waiting for the transport round trip. pub async fn publish(&self, transforms: &[Transform]) -> io::Result<()> { + let payload = (self.encode)(transforms)?; self.graph.update(|buffer| { for t in transforms { - buffer.receive(&t.parent, &t.child, t.ts, t.iso); + buffer.receive_transform(t); } }); - let msg = lcm_msgs::tf2_msgs::TFMessage { - transforms: transforms.iter().map(to_stamped).collect(), - }; - crate::module::publish_encoded(&self.sender, msg.encode()).await + crate::module::publish_encoded(&self.sender, payload).await } } @@ -515,17 +594,52 @@ fn to_stamped(t: &Transform) -> lcm_msgs::geometry_msgs::TransformStamped { } } -// Decodes /tf messages into the shared graph. Registered as a Route so the -// module's existing recv loop dispatches tf traffic to it. +pub(crate) fn encode_legacy(transforms: &[Transform]) -> io::Result> { + Ok(lcm_msgs::tf2_msgs::TFMessage { + transforms: transforms.iter().map(to_stamped).collect(), + } + .encode()) +} + +pub(crate) fn decode_legacy(data: &[u8]) -> io::Result> { + let msg = lcm_msgs::tf2_msgs::TFMessage::decode(data)?; + let mut transforms = Vec::with_capacity(msg.transforms.len()); + for st in msg.transforms { + let t = &st.transform.translation; + let q = &st.transform.rotation; + let Some(rotation) = UnitQuaternion::try_new(Quaternion::new(q.w, q.x, q.y, q.z), 1e-9) + else { + crate::error_throttled!( + Duration::from_secs(1), + parent = %st.header.frame_id, + child = %st.child_frame_id, + "tf rotation is not a valid quaternion" + ); + continue; + }; + let iso = Isometry3::from_parts(Translation3::new(t.x, t.y, t.z), rotation); + let ts = st.header.stamp.sec as f64 + st.header.stamp.nsec as f64 * 1e-9; + transforms.push(Transform::new( + st.header.frame_id, + st.child_frame_id, + ts, + iso, + )); + } + Ok(transforms) +} + +// The existing receive loop dispatches TF using the codec selected by the module. struct TfRoute { topic: String, graph: Arc, + decode: DecodeTransforms, } impl Route for TfRoute { fn try_dispatch(&self, data: &[u8]) { - let msg = match lcm_msgs::tf2_msgs::TFMessage::decode(data) { - Ok(msg) => msg, + let transforms = match (self.decode)(data) { + Ok(transforms) => transforms, Err(e) => { crate::error_throttled!( Duration::from_secs(1), @@ -537,46 +651,44 @@ impl Route for TfRoute { } }; self.graph.update(|buffer| { - for st in &msg.transforms { - let t = &st.transform.translation; - let q = &st.transform.rotation; - // Normalizing a zero-norm quaternion yields a NaN rotation. - let Some(rotation) = - UnitQuaternion::try_new(Quaternion::new(q.w, q.x, q.y, q.z), 1e-9) - else { - crate::error_throttled!( - Duration::from_secs(1), - topic = %self.topic, - parent = %st.header.frame_id, - child = %st.child_frame_id, - "tf rotation is not a valid quaternion" - ); - continue; - }; - let iso = Isometry3::from_parts(Translation3::new(t.x, t.y, t.z), rotation); - let ts = st.header.stamp.sec as f64 + st.header.stamp.nsec as f64 * 1e-9; - buffer.receive(&st.header.frame_id, &st.child_frame_id, ts, iso); + for t in transforms { + buffer.receive_transform(&t); } }); } } -// Builds the shared graph plus the handle and the route that feeds it. The -// sender carries published messages to the tf topic's publish worker. -pub(crate) fn tf_subscription( +pub(crate) fn tf_subscription_with_codec( topic: String, window_secs: f64, sender: mpsc::Sender>, + decode: DecodeTransforms, + encode: EncodeTransforms, ) -> (Tf, Box) { let graph = Arc::new(Graph::new(window_secs)); let tf = Tf { graph: Arc::clone(&graph), sender, + decode, + encode, }; - let route = Box::new(TfRoute { topic, graph }); + let route = Box::new(TfRoute { + topic, + graph, + decode, + }); (tf, route) } +#[cfg(test)] +pub(crate) fn tf_subscription( + topic: String, + window_secs: f64, + sender: mpsc::Sender>, +) -> (Tf, Box) { + tf_subscription_with_codec(topic, window_secs, sender, decode_legacy, encode_legacy) +} + #[cfg(test)] mod tests { use super::*; @@ -594,6 +706,8 @@ mod tests { Tf { graph: Arc::clone(&graph), sender: tx, + decode: decode_legacy, + encode: encode_legacy, }, rx, MultiHandle { graph }, @@ -1202,4 +1316,93 @@ mod tests { .unwrap_err(); assert_eq!(err.kind(), io::ErrorKind::BrokenPipe); } + #[test] + fn wire_nanoseconds_survive_direct_reverse_and_latest_lookup() { + for ns in [0, -1, 1_700_000_000_123_456_789] { + let (tf, _) = tf_with(DEFAULT_TF_WINDOW_SECS); + let edge = Transform::from_nanoseconds( + "world", + "base", + ns, + Isometry3::translation(1., 2., 3.), + ); + tf.graph.update(|buffer| buffer.receive_transform(&edge)); + let direct = tf.get_latest("world", "base").unwrap(); + let inverse = tf.get_latest("base", "world").unwrap(); + assert_eq!(direct.stamp_nanoseconds(), Some(ns)); + assert_eq!(inverse.stamp_nanoseconds(), Some(ns)); + assert_eq!(inverse.translation(), Vector3::new(-1., -2., -3.)); + } + } + + #[test] + fn sub_float_resolution_samples_are_ordered_by_exact_wire_time() { + let (tf, _) = tf_with(DEFAULT_TF_WINDOW_SECS); + let base = 1_700_000_000_123_456_789; + let newest = + Transform::from_nanoseconds("a", "b", base + 2, Isometry3::translation(2., 0., 0.)); + let late = + Transform::from_nanoseconds("a", "b", base + 1, Isometry3::translation(1., 0., 0.)); + assert_eq!( + newest.ts, late.ts, + "fixture must exercise a float collision" + ); + tf.graph.update(|buffer| { + buffer.receive_transform(&newest); + buffer.receive_transform(&late); + }); + let found = tf.get_latest("a", "b").unwrap(); + assert_eq!(found.stamp_nanoseconds(), Some(base + 2)); + assert_eq!(found.translation().x, 2.); + } + + #[test] + fn composed_wire_time_is_the_exact_stalest_edge() { + let (tf, _) = tf_with(DEFAULT_TF_WINDOW_SECS); + let base = 1_700_000_000_123_456_789; + let first = + Transform::from_nanoseconds("a", "b", base + 2, Isometry3::translation(1., 0., 0.)); + let second = + Transform::from_nanoseconds("b", "c", base + 1, Isometry3::translation(2., 0., 0.)); + tf.graph.update(|buffer| { + buffer.receive_transform(&first); + buffer.receive_transform(&second); + }); + let found = tf.get_latest("a", "c").unwrap(); + assert_eq!(found.stamp_nanoseconds(), Some(base + 1)); + assert_eq!(found.translation().x, 3.); + } + + #[test] + fn public_seconds_updates_invalidate_saved_wire_metadata() { + let mut value = + Transform::from_nanoseconds("a", "b", 1_700_000_000_123_456_789, Isometry3::identity()); + value.ts = 3.5; + assert_eq!(value.stamp_nanoseconds(), Some(3_500_000_000)); + value.ts = f64::NAN; + assert_eq!(value.stamp_nanoseconds(), None); + assert_eq!( + Transform::new("a", "b", 1.25, Isometry3::identity()).stamp_nanoseconds(), + Some(1_250_000_000) + ); + } + + #[tokio::test] + async fn publication_keeps_exact_time_in_the_local_graph() { + let (tf, mut rx, _) = tf_with_publish(DEFAULT_TF_WINDOW_SECS); + let ns = 1_700_000_000_123_456_789; + tf.publish(&[Transform::from_nanoseconds( + "a", + "b", + ns, + Isometry3::identity(), + )]) + .await + .unwrap(); + assert!(rx.recv().await.is_some()); + assert_eq!( + tf.get_latest("a", "b").unwrap().stamp_nanoseconds(), + Some(ns) + ); + } } From 06bf182f8ab3c4e3fb2c6a14247bdf4a282e98b9 Mon Sep 17 00:00:00 2001 From: "autofix-ci[bot]" <114827586+autofix-ci[bot]@users.noreply.github.com> Date: Wed, 21 Oct 2026 05:32:02 +0000 Subject: [PATCH 2/6] [autofix.ci] apply automated fixes --- dimos/experimental/memory/rust/Cargo.lock | 18 ++++++++++++----- dimos/experimental/memory/rust/Cargo.toml | 2 +- .../memory/rust/crate-hashes.json | 7 ++++--- .../sensors/lidar/livox/rust/Cargo.lock | 18 ++++++++++++----- .../sensors/lidar/livox/rust/Cargo.toml | 2 +- .../lidar/livox/rust/crate-hashes.json | 7 ++++--- .../sensors/lidar/pointlio/rust/Cargo.lock | 18 ++++++++++++----- .../sensors/lidar/pointlio/rust/Cargo.toml | 2 +- .../lidar/pointlio/rust/crate-hashes.json | 7 ++++--- .../sensors/lidar/virtual_mid360/Cargo.lock | 8 ++++---- .../sensors/lidar/virtual_mid360/Cargo.toml | 2 +- .../lidar/virtual_mid360/crate-hashes.json | 8 ++++---- dimos/mapping/dim_slam/rust/Cargo.lock | 18 ++++++++++++----- dimos/mapping/dim_slam/rust/Cargo.toml | 2 +- dimos/mapping/dim_slam/rust/crate-hashes.json | 7 ++++--- dimos/mapping/ray_tracing/rust/Cargo.lock | 18 ++++++++++++----- dimos/mapping/ray_tracing/rust/Cargo.toml | 2 +- .../ray_tracing/rust/crate-hashes.json | 7 ++++--- .../mls_planner/rust/Cargo.lock | 18 ++++++++++++----- .../mls_planner/rust/Cargo.toml | 2 +- .../mls_planner/rust/crate-hashes.json | 7 ++++--- .../navigation/local_planner/rust/Cargo.lock | 18 ++++++++++++----- .../navigation/local_planner/rust/Cargo.toml | 2 +- .../local_planner/rust/crate-hashes.json | 7 ++++--- .../trajectory_follower/fancy/rust/Cargo.lock | 20 +++++++++++++------ .../trajectory_follower/fancy/rust/Cargo.toml | 2 +- .../fancy/rust/crate-hashes.json | 7 ++++--- dimos/robot/unitree/go2/dds/rust/Cargo.lock | 10 +++++----- dimos/robot/unitree/go2/dds/rust/Cargo.toml | 2 +- .../unitree/go2/dds/rust/crate-hashes.json | 8 ++++---- examples/native-modules/rust/Cargo.lock | 18 ++++++++++++----- examples/native-modules/rust/Cargo.toml | 2 +- .../native-modules/rust/crate-hashes.json | 7 ++++--- 33 files changed, 182 insertions(+), 101 deletions(-) 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 7ac39440148..0b6b28bdeba 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" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6" } 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/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 2b042fdd770..677cea043c9 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" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6" } 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 a6cb8b21048..7ac008a0ce7 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" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6" } 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 220a2de1080..79bbab54e13 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" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6" } 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 8df62977ea2..3032fb95d83 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" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6" } 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 e96b69ebefa..ff2dfab6cf9 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" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6" } 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/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 9f47c6ad8fa..be416dac50d 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" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6" } 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 6d62af49fd7..6ee46345815 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 } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6", optional = true } 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 ad24d3e149e..40c0dcafbdb 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 } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6", optional = true } 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/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 1a264d70fdf..42f834c8ff6 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" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6" } 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/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 b7d0cf69700..831e09b8af6 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" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "fc6a4e58da89bb073a38dce5ac63728c6559d3a6" } 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 From f758a4f6ed4bc365ed1f83cdd36afef407deee7c Mon Sep 17 00:00:00 2001 From: "autofix-ci[bot]" <114827586+autofix-ci[bot]@users.noreply.github.com> Date: Wed, 21 Oct 2026 05:32:49 +0000 Subject: [PATCH 3/6] [autofix.ci] apply automated fixes (attempt 2/3) --- dimos/perception/localize/test_rig.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/dimos/perception/localize/test_rig.py b/dimos/perception/localize/test_rig.py index dcb3fd07e6e..b8dd53b4449 100644 --- a/dimos/perception/localize/test_rig.py +++ b/dimos/perception/localize/test_rig.py @@ -276,7 +276,7 @@ def test_offline_renderer_uses_generated_images_and_local_camera_pinhole(tmp_pat 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, _camera_span, RigCamera +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: @@ -360,6 +360,8 @@ def test_camera_travel_uses_generated_tf_positions(memory): 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) + 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 From dc6b984d6a2a286a6feb04ea5b4f355177bd4ca7 Mon Sep 17 00:00:00 2001 From: "autofix-ci[bot]" <114827586+autofix-ci[bot]@users.noreply.github.com> Date: Wed, 21 Oct 2026 18:45:59 +0000 Subject: [PATCH 4/6] [autofix.ci] apply automated fixes (attempt 3/3) --- dimos/navigation/trajectory_follower/fancy/rust/flake.lock | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dimos/navigation/trajectory_follower/fancy/rust/flake.lock b/dimos/navigation/trajectory_follower/fancy/rust/flake.lock index 9a4b4df94e3..e89b6611140 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": 1792533441, - "narHash": "sha256-XF2lT9/357inKL+G8SmftVXicRfeMugelCoPDB8XOOc=", + "lastModified": 1792608006, + "narHash": "sha256-IZ006Zhuo4TmN6Vqi0CKg9EH6QIda1lMeFosvQrX18k=", "owner": "dimensionalOS", "repo": "dimos", - "rev": "b173fa6dcd3d7046429e6405b3826f8b96952cf8", + "rev": "f3bfd2c724653bd7badcc9a30c486b0dc97e5da5", "type": "github" }, "original": { From f59b609c27f5cc62124a438e7890c15e1fc8f38c Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 21 Oct 2026 19:24:32 -0700 Subject: [PATCH 5/6] refactor(messages): reuse shared sensor helpers across perception --- .../camera/gstreamer/gstreamer_camera.py | 4 +- .../camera/gstreamer/test_generated_frames.py | 4 +- dimos/hardware/sensors/camera/module.py | 4 +- dimos/hardware/sensors/camera/test_module.py | 2 +- dimos/hardware/sensors/camera/test_webcam.py | 6 +- dimos/hardware/sensors/camera/webcam.py | 6 +- dimos/hardware/sensors/camera/zed/camera.py | 8 +- dimos/hardware/sensors/camera/zed/compat.py | 2 +- dimos/hardware/sensors/camera/zed/messages.py | 2 +- .../sensors/camera/zed/test_messages.py | 4 +- dimos/memory/module.py | 2 +- dimos/{msgs => message_utils}/camera_info.py | 0 dimos/{msgs => message_utils}/geometry.py | 0 dimos/message_utils/image.py | 176 +++++++- dimos/message_utils/pointcloud.py | 294 +++++++++++++- .../test_camera_info.py | 8 +- .../{msgs => message_utils}/test_geometry.py | 28 +- .../test_image_compression.py | 10 +- .../test_image_views.py | 10 +- .../test_pointcloud.py | 14 +- .../test_pointcloud_colors.py | 2 +- dimos/{msgs => message_utils}/test_time.py | 4 +- dimos/{msgs => message_utils}/time.py | 0 dimos/models/embedding/clip.py | 4 +- dimos/models/embedding/dino.py | 4 +- dimos/models/embedding/siglip.py | 4 +- dimos/models/embedding/siglip2.py | 4 +- .../models/embedding/test_generated_images.py | 2 +- dimos/models/embedding/treid.py | 4 +- dimos/models/segmentation/edge_tam.py | 2 +- .../test_generated_edge_tam_images.py | 2 +- dimos/models/segmentation/test_yoloe.py | 4 +- dimos/models/segmentation/yoloe.py | 2 +- dimos/models/vl/base.py | 4 +- dimos/models/vl/qwen.py | 4 +- dimos/models/vl/test_base.py | 4 +- dimos/msgs/image.py | 311 -------------- dimos/msgs/pointcloud.py | 380 ------------------ dimos/perception/detection/conftest.py | 6 +- .../detection/detectors/grounding_dino.py | 4 +- dimos/perception/detection/detectors/omdet.py | 4 +- dimos/perception/detection/detectors/owlv2.py | 4 +- .../detection/detectors/person/yolo.py | 2 +- .../detectors/test_generated_images.py | 2 +- dimos/perception/detection/detectors/yolo.py | 2 +- dimos/perception/detection/detectors/yoloe.py | 2 +- dimos/perception/detection/module2D.py | 4 +- dimos/perception/detection/module3D.py | 2 +- dimos/perception/detection/person_tracker.py | 8 +- dimos/perception/detection/project.py | 10 +- dimos/perception/detection/reid/module.py | 2 +- .../detection/reid/test_generated_module.py | 4 +- .../detection/type/detection2d/bbox.py | 4 +- .../detection/type/detection2d/person.py | 2 +- .../detection/type/detection2d/point.py | 2 +- .../detection/type/detection2d/seg.py | 2 +- .../detection/type/detection2d/test_bbox.py | 2 +- .../type/detection2d/test_generated_bbox.py | 2 +- .../detection/type/detection2d/test_person.py | 2 +- .../detection/type/detection3d/bbox.py | 2 +- .../type/detection3d/imageDetections3DPC.py | 4 +- .../detection/type/detection3d/pointcloud.py | 18 +- .../type/detection3d/pointcloud_filters.py | 4 +- .../detection3d/test_generated_filters.py | 4 +- .../detection3d/test_generated_projection.py | 4 +- .../detection3d/test_imageDetections3D.py | 4 +- .../detection/type/detection3d/test_marker.py | 4 +- .../type/detection3d/test_pointcloud.py | 8 +- .../detection/type/imageDetections.py | 4 +- .../detection/type/test_detection3d.py | 4 +- dimos/perception/experimental/moduleDB.py | 2 +- dimos/perception/experimental/object.py | 24 +- dimos/perception/experimental/objectDB.py | 4 +- .../experimental/object_scene_registration.py | 6 +- .../perception/experimental/object_tracker.py | 6 +- .../experimental/object_tracker_2d.py | 4 +- .../experimental/object_tracker_3d.py | 6 +- .../experimental/perceive_loop_skill.py | 4 +- .../experimental/spatial_perception.py | 4 +- .../temporal_memory/clip_filter.py | 2 +- .../frame_window_accumulator.py | 2 +- .../temporal_memory/temporal_memory.py | 4 +- .../temporal_memory/temporal_utils/helpers.py | 2 +- .../test_temporal_memory_module.py | 4 +- .../experimental/test_generated_tracker.py | 2 +- .../experimental/test_object_generated.py | 8 +- ...test_object_scene_registration_temporal.py | 8 +- .../experimental/test_perceive_loop_skill.py | 4 +- .../experimental/test_spatial_memory.py | 2 +- .../fiducial/blueprints/desk_marker_tf.py | 6 +- dimos/perception/fiducial/marker_detect.py | 6 +- .../marker_detection_stream_module.py | 4 +- dimos/perception/fiducial/marker_pose.py | 2 +- .../perception/fiducial/marker_transformer.py | 2 +- .../fiducial/test_fixture_verification.py | 8 +- .../fiducial/test_generated_marker_detect.py | 2 +- dimos/perception/fiducial/test_helpers.py | 4 +- .../test_marker_detection_stream_module.py | 2 +- dimos/perception/fiducial/test_marker_pose.py | 2 +- .../fiducial/test_marker_tf_integration.py | 2 +- .../fiducial/test_marker_tf_module.py | 4 +- .../fiducial/test_marker_transformer.py | 4 +- dimos/perception/localize/association.py | 6 +- dimos/perception/localize/localize.py | 8 +- dimos/perception/localize/module.py | 4 +- dimos/perception/localize/rig.py | 14 +- dimos/perception/localize/support_plane.py | 2 +- dimos/perception/localize/test_dandetect.py | 6 +- dimos/perception/localize/test_rig.py | 18 +- .../localize/tools/tool_localize.py | 18 +- .../impl/webrtc/providers/test_video_track.py | 2 +- .../impl/webrtc/providers/video_track.py | 2 +- dimos/protocol/tf/static_tf_publisher.py | 4 +- dimos/protocol/tf/test_static_tf_publisher.py | 2 +- dimos/protocol/tf/test_tf.py | 4 +- dimos/protocol/tf/tf.py | 4 +- dimos/teleop/hosted/camera_mux.py | 4 +- dimos/teleop/hosted/test_camera_mux.py | 2 +- docs/capabilities/perception/index.md | 14 +- docs/usage/sensor_streams/index.md | 2 +- docs/usage/transforms.md | 4 +- 121 files changed, 745 insertions(+), 978 deletions(-) rename dimos/{msgs => message_utils}/camera_info.py (100%) rename dimos/{msgs => message_utils}/geometry.py (100%) rename dimos/{msgs => message_utils}/test_camera_info.py (99%) rename dimos/{msgs => message_utils}/test_geometry.py (99%) rename dimos/{msgs => message_utils}/test_image_compression.py (98%) rename dimos/{msgs => message_utils}/test_image_views.py (98%) rename dimos/{msgs => message_utils}/test_pointcloud.py (99%) rename dimos/{msgs => message_utils}/test_pointcloud_colors.py (98%) rename dimos/{msgs => message_utils}/test_time.py (95%) rename dimos/{msgs => message_utils}/time.py (100%) delete mode 100644 dimos/msgs/image.py delete mode 100644 dimos/msgs/pointcloud.py diff --git a/dimos/hardware/sensors/camera/gstreamer/gstreamer_camera.py b/dimos/hardware/sensors/camera/gstreamer/gstreamer_camera.py index 028e22b87f2..113f561b56e 100644 --- a/dimos/hardware/sensors/camera/gstreamer/gstreamer_camera.py +++ b/dimos/hardware/sensors/camera/gstreamer/gstreamer_camera.py @@ -26,10 +26,10 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import Out +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.msgs.image import image_from_array -from dimos.msgs.time import duration_from_seconds, time_from_nanoseconds, to_nanoseconds from dimos.utils.logging_config import setup_logger # Add system path for gi module if needed diff --git a/dimos/hardware/sensors/camera/gstreamer/test_generated_frames.py b/dimos/hardware/sensors/camera/gstreamer/test_generated_frames.py index cf74ebf9afb..0ebfeed75c5 100644 --- a/dimos/hardware/sensors/camera/gstreamer/test_generated_frames.py +++ b/dimos/hardware/sensors/camera/gstreamer/test_generated_frames.py @@ -22,9 +22,9 @@ 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.msgs.image import image_view -from dimos.msgs.time import to_nanoseconds from dimos.protocol.rpc.zenohrpc import ZenohRPC gi = pytest.importorskip("gi", reason="GStreamer acceptance requires the optional GI binding") diff --git a/dimos/hardware/sensors/camera/module.py b/dimos/hardware/sensors/camera/module.py index 74095066a97..37a33781b4c 100644 --- a/dimos/hardware/sensors/camera/module.py +++ b/dimos/hardware/sensors/camera/module.py @@ -26,13 +26,13 @@ from dimos.core.stream import Out from dimos.hardware.sensors.camera.spec import CameraHardware from dimos.hardware.sensors.camera.webcam import Webcam +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.msgs.image import image_sharpness -from dimos.msgs.time import time_from_nanoseconds from dimos.spec import perception from dimos.utils.reactive import quality_barrier from dimos.visualization.vis_module import vis_module diff --git a/dimos/hardware/sensors/camera/test_module.py b/dimos/hardware/sensors/camera/test_module.py index 2a24e5e8037..3d7bc41eab0 100644 --- a/dimos/hardware/sensors/camera/test_module.py +++ b/dimos/hardware/sensors/camera/test_module.py @@ -20,9 +20,9 @@ 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.msgs.geometry import transform_matrix from dimos.protocol.rpc.zenohrpc import ZenohRPC diff --git a/dimos/hardware/sensors/camera/test_webcam.py b/dimos/hardware/sensors/camera/test_webcam.py index aefc45a0e58..48cd1d53256 100644 --- a/dimos/hardware/sensors/camera/test_webcam.py +++ b/dimos/hardware/sensors/camera/test_webcam.py @@ -17,11 +17,11 @@ import pytest from dimos.hardware.sensors.camera.webcam import Webcam +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 -from dimos.msgs.camera_info import camera_info_from_fov -from dimos.msgs.image import image_view def test_webcam_without_intrinsics_reports_a_nominal_pinhole() -> None: @@ -69,7 +69,7 @@ def test_capture_converts_bgr_and_crops_before_cdr_encoding(side, start, mocker) capture = mocker.Mock() capture.read.return_value = (True, pixels) mocker.patch.object(camera, "_capture", capture) - mocker.patch("dimos.msgs.time.time.time_ns", return_value=1700000000123456789) + mocker.patch("dimos.message_utils.time.time.time_ns", return_value=1700000000123456789) try: message = cdr_decode(cdr_encode(camera.capture_frame()), Image) finally: diff --git a/dimos/hardware/sensors/camera/webcam.py b/dimos/hardware/sensors/camera/webcam.py index ce25d2b5644..b4463fc33ea 100644 --- a/dimos/hardware/sensors/camera/webcam.py +++ b/dimos/hardware/sensors/camera/webcam.py @@ -24,12 +24,12 @@ from reactivex.observable import Observable from dimos.hardware.sensors.camera.spec import CameraConfig, CameraHardware +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.msgs.camera_info import camera_info_from_fov -from dimos.msgs.image import image_from_array -from dimos.msgs.time import header_now from dimos.utils.reactive import backpressure diff --git a/dimos/hardware/sensors/camera/zed/camera.py b/dimos/hardware/sensors/camera/zed/camera.py index 6308b700503..0f483a1934a 100644 --- a/dimos/hardware/sensors/camera/zed/camera.py +++ b/dimos/hardware/sensors/camera/zed/camera.py @@ -38,15 +38,15 @@ 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.msgs.camera_info import camera_info_with_stamp -from dimos.msgs.image import image_from_array -from dimos.msgs.pointcloud import pointcloud_from_rgbd, voxel_downsample_cloud -from dimos.msgs.time import time_from_nanoseconds from dimos.spec import perception from dimos.utils.reactive import backpressure diff --git a/dimos/hardware/sensors/camera/zed/compat.py b/dimos/hardware/sensors/camera/zed/compat.py index 2ee92c2341f..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.camera_info 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 index 389d902e468..a23fccd222f 100644 --- a/dimos/hardware/sensors/camera/zed/messages.py +++ b/dimos/hardware/sensors/camera/zed/messages.py @@ -19,11 +19,11 @@ 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 -from dimos.msgs.geometry import compose_transforms, inverse_transform def transform_from_arrays( diff --git a/dimos/hardware/sensors/camera/zed/test_messages.py b/dimos/hardware/sensors/camera/zed/test_messages.py index 07aefe3c18e..117b4d25f95 100644 --- a/dimos/hardware/sensors/camera/zed/test_messages.py +++ b/dimos/hardware/sensors/camera/zed/test_messages.py @@ -24,10 +24,10 @@ 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 -from dimos.msgs.geometry import quaternion_from_euler, transform_matrix -from dimos.msgs.time import time_from_nanoseconds, to_nanoseconds @pytest.mark.parametrize("ns", [0, -1, 1700000000123456789]) diff --git a/dimos/memory/module.py b/dimos/memory/module.py index 06c78cabc9a..8bfdddccfea 100644 --- a/dimos/memory/module.py +++ b/dimos/memory/module.py @@ -35,9 +35,9 @@ from dimos.memory.store.sqlite import SqliteStore from dimos.memory.stream import Stream from dimos.memory.type.recording import OnExisting +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.msgs.time import to_seconds from dimos.utils.data import backup_file from dimos.utils.logging_config import setup_logger diff --git a/dimos/msgs/camera_info.py b/dimos/message_utils/camera_info.py similarity index 100% rename from dimos/msgs/camera_info.py rename to dimos/message_utils/camera_info.py diff --git a/dimos/msgs/geometry.py b/dimos/message_utils/geometry.py similarity index 100% rename from dimos/msgs/geometry.py rename to dimos/message_utils/geometry.py diff --git a/dimos/message_utils/image.py b/dimos/message_utils/image.py index 08cfb6296bf..16548a5f175 100644 --- a/dimos/message_utils/image.py +++ b/dimos/message_utils/image.py @@ -12,13 +12,18 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Read and convert generated image buffers.""" +"""Array views and compression for generated ROS image messages.""" +from __future__ import annotations + +from pathlib import Path from typing import Any +import cv2 import imagecodecs import numpy as np from numpy.typing import NDArray +from PIL import Image as PILImage from dimos.messages.builtin_interfaces.msg import Time from dimos.messages.sensor_msgs.msg import CompressedImage, Image @@ -39,9 +44,7 @@ } -def _image_from_array( - pixels: NDArray[Any], *, encoding: str, header: Header | None = None -) -> Image: +def image_from_array(pixels: NDArray[Any], *, encoding: str, header: Header | None = None) -> Image: """Copy an array into a generated image with an explicit pixel encoding.""" if encoding not in _FORMATS: raise ValueError(f"unsupported image encoding {encoding!r}") @@ -85,9 +88,88 @@ def image_view(msg: Image) -> NDArray[Any]: return view +def image_to_jpeg(msg: Image, quality: int = 75) -> bytes: + """Encode an 8-bit gray or color image using OpenCV's JPEG encoder.""" + + if isinstance(quality, bool) or not 0 <= quality <= 100: + raise ValueError("JPEG quality must be in 0..100") + pixels = image_view(msg) + conversions = { + "rgb8": cv2.COLOR_RGB2BGR, + "rgba8": cv2.COLOR_RGBA2BGR, + "bgra8": cv2.COLOR_BGRA2BGR, + } + if msg.encoding in conversions: + pixels = cv2.cvtColor(pixels, conversions[msg.encoding]) + elif msg.encoding not in ("bgr8", "mono8"): + raise ValueError(f"JPEG requires 8-bit color or gray, got {msg.encoding!r}") + ok, encoded = cv2.imencode(".jpg", pixels, [cv2.IMWRITE_JPEG_QUALITY, quality]) + if not ok: + raise ValueError("JPEG encoding failed") + return bytes(encoded) + + +def compressed_image_from_image( + message: Image, + quality: int = 75, + *, + format: str = "jpeg", + max_width: int | None = None, + effort: int | None = None, +) -> CompressedImage: + """Compress generated pixels, retaining the exact header. + + PNG preserves uint16 depth. JXL preserves uint16/float32 depth losslessly; + it requires imagecodecs and is an explicit codec extension for consumers. + """ + if not isinstance(message, Image): + raise TypeError("compression expects a generated Image") + if max_width is not None: + message, _ = image_resize_to_fit(message, max_width, max_width) + if format == "jpeg": + output_encoding = "mono8" if message.encoding == "mono8" else "bgr8" + data = image_to_jpeg(message, quality=quality) + elif format == "png": + pixels = image_view(message) + if pixels.dtype.kind != "u" or pixels.dtype.itemsize not in (1, 2): + raise ValueError("PNG cannot encode floating-point depth") + if message.encoding in ("rgb8", "rgba8"): + code = cv2.COLOR_RGB2BGR if message.encoding == "rgb8" else cv2.COLOR_RGBA2BGRA + pixels = cv2.cvtColor(pixels, code) + pixels = np.ascontiguousarray(pixels, dtype=pixels.dtype.newbyteorder("=")) + ok, encoded = cv2.imencode(".png", pixels) + if not ok: + raise ValueError("PNG encoding failed") + data = bytes(encoded) + output_encoding = {"rgb8": "bgr8", "rgba8": "bgra8"}.get(message.encoding, message.encoding) + elif format == "jxl": + pixels = image_view(message) + if pixels.dtype.kind not in ("u", "f") or ( + pixels.dtype.kind, + pixels.dtype.itemsize, + ) not in (("u", 1), ("u", 2), ("f", 4)): + raise ValueError(f"JXL cannot encode dtype {pixels.dtype}") + if message.encoding == "bgr8": + pixels = pixels[..., ::-1] + elif message.encoding == "bgra8": + pixels = pixels[..., [2, 1, 0, 3]] + pixels = np.ascontiguousarray(pixels, dtype=pixels.dtype.newbyteorder("=")) + if pixels.dtype == np.uint8: + data = bytes(imagecodecs.jpegxl_encode(pixels, level=quality, effort=effort or 3)) + else: + data = bytes(imagecodecs.jpegxl_encode(pixels, lossless=True, effort=effort or 1)) + output_encoding = {"bgr8": "rgb8", "bgra8": "rgba8"}.get(message.encoding, message.encoding) + else: + raise ValueError(f"unsupported compression format {format!r}") + return CompressedImage( + header=message.header, + format=f"{message.encoding}; {format} compressed {output_encoding}", + data=np.asarray(np.frombuffer(data, dtype=np.uint8).copy(), dtype=np.uint8), + ) + + def image_from_compressed(message: CompressedImage) -> Image: """Decode JPEG/PNG or the JXL extension into explicitly encoded generated pixels.""" - import cv2 format_name = message.format.lower() if "jxl" in format_name: @@ -106,7 +188,7 @@ def image_from_compressed(message: CompressedImage) -> Image: raise ValueError("unsupported decoded JXL layout") if encoding == "mono16" and format_name.startswith("16uc1;"): encoding = "16UC1" - return _image_from_array(jxl_pixels, encoding=encoding, header=message.header) + return image_from_array(jxl_pixels, encoding=encoding, header=message.header) if not any(name in format_name for name in ("jpeg", "jpg", "png")): raise ValueError(f"unsupported compressed image format {message.format!r}") pixels = cv2.imdecode(np.frombuffer(bytes(message.data), dtype=np.uint8), cv2.IMREAD_UNCHANGED) @@ -122,7 +204,64 @@ def image_from_compressed(message: CompressedImage) -> Image: raise ValueError("unsupported decoded image layout") if encoding == "mono16" and format_name.startswith("16uc1;"): encoding = "16UC1" - return _image_from_array(pixels, encoding=encoding, header=message.header) + return image_from_array(pixels, encoding=encoding, header=message.header) + + +def image_brightness(message: Image) -> float: + """Sample mean pixel intensity in [0, 1] for 8/16-bit image encodings. + + Respect row padding and endian. Floating-point depth has no normalized + intensity scale and is rejected rather than treated as a color image. + """ + pixels = image_view(message) + if pixels.dtype.kind != "u": + raise ValueError("brightness requires unsigned integer pixels") + stride = max(1, max(message.height, message.width) // 256) + return float(pixels[::stride, ::stride].mean() / np.iinfo(pixels.dtype).max) + + +def image_sharpness(message: Image) -> float: + """Laplacian variance of an 8-bit visual image, downsampled to 160 pixels wide.""" + + pixels = image_view(message) + codes = { + "rgb8": cv2.COLOR_RGB2GRAY, + "bgr8": cv2.COLOR_BGR2GRAY, + "rgba8": cv2.COLOR_RGBA2GRAY, + "bgra8": cv2.COLOR_BGRA2GRAY, + } + if message.encoding == "mono8": + gray = pixels + elif message.encoding in codes: + gray = cv2.cvtColor(pixels, codes[message.encoding]) + else: + raise ValueError("Sharpness requires an 8-bit visual image encoding") + if gray.size == 0: + raise ValueError("Sharpness requires a nonempty image") + if message.width > 160: + gray = cv2.resize( + gray, + (160, max(1, round(message.height * 160 / message.width))), + interpolation=cv2.INTER_AREA, + ) + return float(cv2.Laplacian(gray, cv2.CV_64F).var()) + + +def image_to_bgr(message: Image) -> NDArray[np.uint8]: + """Return an independent BGR8 array for drawing or OpenCV color operations.""" + + pixels = image_view(message) + if message.encoding == "bgr8": + return pixels.copy() + codes = { + "rgb8": cv2.COLOR_RGB2BGR, + "rgba8": cv2.COLOR_RGBA2BGR, + "bgra8": cv2.COLOR_BGRA2BGR, + "mono8": cv2.COLOR_GRAY2BGR, + } + if message.encoding not in codes: + raise ValueError(f"Cannot convert {message.encoding!r} to BGR8") + return np.asarray(cv2.cvtColor(pixels, codes[message.encoding]), dtype=np.uint8) def image_to_rgb(message: Image) -> NDArray[np.uint8]: @@ -131,7 +270,6 @@ def image_to_rgb(message: Image) -> NDArray[np.uint8]: Alpha is discarded. Floating-point depth needs an explicit visualization scale and is rejected rather than assigning pixel values to meters. """ - import cv2 pixels = image_view(message) if message.encoding == "rgb8": @@ -149,3 +287,25 @@ def image_to_rgb(message: Image) -> NDArray[np.uint8]: if message.encoding not in codes: raise ValueError(f"Cannot convert {message.encoding!r} to RGB8") return np.asarray(cv2.cvtColor(pixels, codes[message.encoding]), dtype=np.uint8) + + +def image_resize_to_fit(message: Image, max_width: int, max_height: int) -> tuple[Image, float]: + """Downscale a generated image while preserving encoding, aspect and exact header.""" + + if min(max_width, max_height, message.width, message.height) <= 0: + raise ValueError("Image and target dimensions must be positive") + if message.width <= max_width and message.height <= max_height: + return message, 1.0 + scale = min(max_width / message.width, max_height / message.height) + pixels = cv2.resize( + image_view(message), + (max(1, int(message.width * scale)), max(1, int(message.height * scale))), + interpolation=cv2.INTER_LINEAR, + ) + return image_from_array(pixels, encoding=message.encoding, header=message.header), scale + + +def image_from_file(path: str | Path, *, header: Header | None = None) -> Image: + """Decode an image file as generated RGB8 with an explicit optional source header.""" + with PILImage.open(path) as image: + return image_from_array(np.asarray(image.convert("RGB")), encoding="rgb8", header=header) diff --git a/dimos/message_utils/pointcloud.py b/dimos/message_utils/pointcloud.py index 1e630828eab..fe34b0d912b 100644 --- a/dimos/message_utils/pointcloud.py +++ b/dimos/message_utils/pointcloud.py @@ -12,14 +12,24 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Read generated point-cloud layouts and extract positions and colors.""" +"""NumPy conversions for generated PointCloud2 messages.""" +from copy import deepcopy +import math from typing import Any +from dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode import numpy as np from numpy.typing import NDArray +import open3d as o3d # type: ignore[import-untyped] -from dimos.messages.sensor_msgs.msg import PointCloud2 +from dimos.message_utils.camera_info import intrinsic_matrix +from dimos.message_utils.geometry import transform_matrix +from dimos.message_utils.image import image_to_rgb, image_view +from dimos.message_utils.time import to_nanoseconds +from dimos.messages.geometry_msgs.msg import TransformStamped +from dimos.messages.sensor_msgs.msg import CameraInfo, Image, PointCloud2, PointField +from dimos.messages.std_msgs.msg import Header _FIELD_KINDS = {1: "i1", 2: "u1", 3: "i2", 4: "u2", 5: "i4", 6: "u4", 7: "f4", 8: "f8"} @@ -72,6 +82,96 @@ def pointcloud_xyz(message: PointCloud2) -> NDArray[np.float64]: return np.stack([points[name].ravel() for name in ("x", "y", "z")], axis=-1).astype(np.float64) +def pointcloud_from_xyz( + points: NDArray[Any], *, header: Header, stamps: NDArray[Any] | None = None +) -> PointCloud2: + """Copy Nx3 or HxWx3 coordinates into a tightly packed little-endian XYZ cloud. + + XYZ fields are float32. NaNs/infinities remain missing-point markers and set + is_dense=False; finite coordinates outside float32 range are rejected. + """ + values = np.asarray(points) + if values.ndim not in (2, 3) or values.shape[-1] != 3: + raise ValueError("XYZ coordinates must have shape (N, 3) or (H, W, 3)") + if values.dtype.kind not in "fiu": + raise ValueError("XYZ coordinates must be numeric") + finite = np.isfinite(values) + if np.any(np.abs(values[finite].astype(np.float64)) > np.finfo(np.float32).max): + raise ValueError("XYZ coordinates exceed float32 range") + packed = np.ascontiguousarray(values, dtype=" NDArray[np.float64] | None: + """Read per-point capture times; absence falls back to the header at the caller.""" + fields = [field for field in message.fields if field.name == "stamps"] + if not fields: + return None + if len(fields) != 1 or fields[0].datatype != PointField.FLOAT64 or fields[0].count != 1: + raise ValueError("Per-point stamps must be a scalar FLOAT64 field") + return np.asarray(pointcloud_view(message)["stamps"].reshape(-1), dtype=np.float64) + + +def select_points(message: PointCloud2, keep: NDArray[np.bool_]) -> PointCloud2: + """Copy selected point records in row order, retaining every field and point byte. + + The result is unorganized with no row padding. Point padding, endianness, + field metadata, source header, and the conservative density flag survive. + """ + pointcloud_view(message) # Validate the full declared layout before selecting bytes. + if keep.dtype != np.bool_ or keep.shape != (message.height * message.width,): + raise ValueError("point selection must be a flat boolean mask matching the point count") + records: NDArray[np.uint8] = np.ndarray( + (message.height, message.width, message.point_step), + dtype=np.uint8, + buffer=message.data, + strides=(message.row_step, message.point_step, 1), + ) + selected = records.reshape(message.height * message.width, message.point_step)[keep] + width = len(selected) + return PointCloud2( + header=deepcopy(message.header), + height=1, + width=width, + fields=deepcopy(message.fields), + is_bigendian=message.is_bigendian, + point_step=message.point_step, + row_step=width * message.point_step, + data=np.asarray(np.frombuffer(selected.tobytes(), dtype=np.uint8).copy(), dtype=np.uint8), + is_dense=message.is_dense, + ) + + def pointcloud_rgb(message: PointCloud2) -> NDArray[np.uint8] | None: """Copy RGB bytes from the standard packed rgb/rgba field, if present. @@ -88,3 +188,193 @@ def pointcloud_rgb(message: PointCloud2) -> NDArray[np.uint8] | None: raise ValueError("packed point cloud color must be a scalar FLOAT32 or UINT32 field") packed = field.view(np.dtype(">u4" if message.is_bigendian else "> shift) & 255 for shift in (16, 8, 0)], axis=-1).astype(np.uint8) + + +def concatenate_clouds(first: PointCloud2, second: PointCloud2) -> PointCloud2: + """Join compatible point records, preserving fields, endian, and point padding. + + Row padding is removed. The first frame and newest exact source stamp survive. + Empty inputs are identities; differing nonempty frames/layouts are rejected. + """ + pointcloud_view(first) + pointcloud_view(second) + if not first.width * first.height: + return cdr_decode(cdr_encode(second), PointCloud2) + if not second.width * second.height: + return cdr_decode(cdr_encode(first), PointCloud2) + if first.header.frame_id != second.header.frame_id: + raise ValueError("cannot concatenate point clouds in different frames") + if ( + first.fields != second.fields + or first.point_step != second.point_step + or first.is_bigendian != second.is_bigendian + ): + raise ValueError("cannot concatenate point clouds with different record layouts") + a = select_points(first, np.ones(first.width * first.height, dtype=np.bool_)) + b = select_points(second, np.ones(second.width * second.height, dtype=np.bool_)) + stamp = max((first.header.stamp, second.header.stamp), key=to_nanoseconds) + return PointCloud2( + header=Header(frame_id=first.header.frame_id, stamp=stamp), + height=1, + width=a.width + b.width, + fields=deepcopy(first.fields), + is_bigendian=first.is_bigendian, + point_step=first.point_step, + row_step=a.row_step + b.row_step, + data=np.asarray(np.concatenate((a.data, b.data)), dtype=np.uint8), + is_dense=first.is_dense and second.is_dense, + ) + + +def transform_cloud(message: PointCloud2, transform: TransformStamped) -> PointCloud2: + """Apply parent←child to XYZ while retaining the complete organized record layout. + + Copy storage before changing coordinates. Keep all other fields, point/row + padding, endian and the cloud's exact source stamp. Frames must match; + integer XYZ is rejected because transformed coordinates would be truncated. + """ + if message.header.frame_id != transform.child_frame_id: + raise ValueError("point cloud frame does not match transform child frame") + view = pointcloud_view(message) + for name in ("x", "y", "z"): + if name not in (view.dtype.names or ()) or view.dtype[name].kind != "f": + raise ValueError("transform requires scalar floating-point XYZ fields") + matrix = transform_matrix(transform.transform) + xyz = pointcloud_xyz(message) + transformed = xyz @ matrix[:3, :3].T + matrix[:3, 3] + for index, name in enumerate(("x", "y", "z")): + values = transformed[:, index] + if np.any(np.abs(values[np.isfinite(values)]) > np.finfo(view.dtype[name]).max): + raise ValueError(f"transformed {name} coordinates exceed field range") + payload = bytearray(bytes(message.data)) + writable = np.ndarray( + (message.height, message.width), + dtype=view.dtype, + buffer=payload, + strides=(message.row_step, message.point_step), + ) + for index, name in enumerate(("x", "y", "z")): + writable[name] = transformed[:, index].reshape(message.height, message.width) + output = cdr_decode(cdr_encode(message), PointCloud2) + output.data = np.frombuffer(payload, dtype=np.uint8).copy() + output.header = Header(stamp=deepcopy(message.header.stamp), frame_id=transform.header.frame_id) + return output + + +def pointcloud_from_xyz_rgb( + points: NDArray[Any], colors: NDArray[np.uint8], *, header: Header +) -> PointCloud2: + """Copy XYZ and RGB8 into a declared XYZ FLOAT32 / packed RGB UINT32 layout.""" + cloud = pointcloud_from_xyz(points, header=header) + rgb = np.asarray(colors) + if rgb.dtype != np.uint8 or rgb.shape != np.asarray(points).shape: + raise ValueError("RGB must be uint8 with the same (..., 3) shape as XYZ") + records = np.empty((cloud.height, cloud.width), dtype=[("xyz", " PointCloud2: + """Project a rectified RGB/depth pair into the depth optical frame. + + Integer depth is multiplied by depth_scale to obtain meters; 32FC1 already + represents meters. Invalid/nonpositive/too-distant samples are removed. + Scale K when calibration resolution differs, retaining the depth header. + """ + if ( + not math.isfinite(depth_scale) + or depth_scale <= 0 + or not math.isfinite(depth_trunc) + or depth_trunc <= 0 + ): + raise ValueError("depth scale and truncation must be finite and positive") + if (color.width, color.height) != (depth.width, depth.height): + raise ValueError("color and depth dimensions do not match") + if depth.encoding not in ("16UC1", "mono16", "32FC1"): + raise ValueError("RGBD requires uint16 or float32 depth") + if calibration.width <= 0 or calibration.height <= 0: + raise ValueError("calibration dimensions must be positive") + matrix = intrinsic_matrix(calibration).copy() + matrix[0, :] *= depth.width / calibration.width + matrix[1, :] *= depth.height / calibration.height + distances = image_view(depth).astype(np.float64) + if depth.encoding != "32FC1": + distances *= depth_scale + valid = np.isfinite(distances) & (distances > 0) & (distances < depth_trunc) + rows = np.broadcast_to(np.arange(depth.height, dtype=np.float64)[:, None], distances.shape) + columns = np.broadcast_to(np.arange(depth.width, dtype=np.float64)[None, :], distances.shape) + z = distances[valid] + xyz = np.column_stack( + ( + (columns[valid] - matrix[0, 2]) * z / matrix[0, 0], + (rows[valid] - matrix[1, 2]) * z / matrix[1, 1], + z, + ) + ) + return pointcloud_from_xyz_rgb(xyz, image_to_rgb(color)[valid], header=depth.header) + + +def voxel_downsample_cloud(message: PointCloud2, voxel_size: float = 0.025) -> PointCloud2: + """Downsample XYZ/RGB clouds with the existing Open3D tensor backend. + + Non-positive sizes and clouds below twenty points retain the existing fast + path. Unsupported extra fields are rejected rather than silently dropped. + Source headers are preserved; generated messages own no numerical methods. + """ + if not math.isfinite(voxel_size): + raise ValueError("voxel_size must be finite") + if voxel_size <= 0 or message.width * message.height < 20: + return message + names = {field.name for field in message.fields} + if names not in ({"x", "y", "z"}, {"x", "y", "z", "rgb"}): + raise ValueError("voxel downsampling supports only XYZ and optional RGB fields") + points = pointcloud_xyz(message).astype(np.float32) + if not np.isfinite(points).all(): + raise ValueError("voxel downsampling requires finite XYZ positions") + colors = pointcloud_rgb(message) + + device = o3d.core.Device("CPU:0") + cloud = o3d.t.geometry.PointCloud(o3d.core.Tensor(points, o3d.core.Dtype.Float32, device)) + if colors is not None: + cloud.point["colors"] = o3d.core.Tensor( + colors.astype(np.float32) / 255.0, o3d.core.Dtype.Float32, device + ) + reduced = cloud.voxel_down_sample(voxel_size) + xyz = reduced.point["positions"].numpy() + if colors is None: + return pointcloud_from_xyz(xyz, header=message.header) + rgb = np.clip(reduced.point["colors"].numpy() * 255.0, 0, 255).astype(np.uint8) + return pointcloud_from_xyz_rgb(xyz, rgb, header=message.header) + + +def pointcloud_to_open3d(message: PointCloud2) -> Any: + """Copy generated XYZ/RGB values into a native geometry algorithm input.""" + + result = o3d.geometry.PointCloud() + result.points = o3d.utility.Vector3dVector(pointcloud_xyz(message)) + colors = pointcloud_rgb(message) + if colors is not None: + result.colors = o3d.utility.Vector3dVector(colors.astype(np.float64) / 255.0) + return result + + +def cloud_bounds_intersect(first: PointCloud2, second: PointCloud2) -> bool: + """Test closed axis-aligned bounds; empty or entirely nonfinite clouds do not intersect.""" + a, b = pointcloud_xyz(first), pointcloud_xyz(second) + a, b = a[np.isfinite(a).all(axis=1)], b[np.isfinite(b).all(axis=1)] + if len(a) == 0 or len(b) == 0: + return False + return bool(np.all(a.min(axis=0) <= b.max(axis=0)) and np.all(a.max(axis=0) >= b.min(axis=0))) diff --git a/dimos/msgs/test_camera_info.py b/dimos/message_utils/test_camera_info.py similarity index 99% rename from dimos/msgs/test_camera_info.py rename to dimos/message_utils/test_camera_info.py index 84c8a5d2a48..bcef99230c3 100644 --- a/dimos/msgs/test_camera_info.py +++ b/dimos/message_utils/test_camera_info.py @@ -22,16 +22,16 @@ import pytest import yaml -from dimos.messages.builtin_interfaces.msg import Time -from dimos.messages.sensor_msgs.msg import CameraInfo -from dimos.messages.std_msgs.msg import Header -from dimos.msgs.camera_info import ( +from dimos.message_utils.camera_info import ( camera_info_from_fov, camera_info_from_intrinsics, camera_info_from_yaml, camera_info_with_stamp, intrinsic_matrix, ) +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.sensor_msgs.msg import CameraInfo +from dimos.messages.std_msgs.msg import Header _CALIBRATION = Path(__file__).parents[1] / "robot/unitree/go2/front_camera_720.yaml" diff --git a/dimos/msgs/test_geometry.py b/dimos/message_utils/test_geometry.py similarity index 99% rename from dimos/msgs/test_geometry.py rename to dimos/message_utils/test_geometry.py index 31c30c53bc2..a26d0a07982 100644 --- a/dimos/msgs/test_geometry.py +++ b/dimos/message_utils/test_geometry.py @@ -21,6 +21,20 @@ import numpy as np import pytest +from dimos.message_utils.geometry import ( + compose_transforms, + inverse_transform, + pose_from_matrix, + pose_from_transform, + pose_matrix, + quaternion_euler, + quaternion_from_euler, + transform_from_odometry, + transform_from_pose, + transform_matrix, + translate_pose_local, + yaw, +) from dimos.messages.builtin_interfaces.msg import Time from dimos.messages.geometry_msgs.msg import ( Point, @@ -36,20 +50,6 @@ ) from dimos.messages.nav_msgs.msg import Odometry from dimos.messages.std_msgs.msg import Header -from dimos.msgs.geometry import ( - compose_transforms, - inverse_transform, - pose_from_matrix, - pose_from_transform, - pose_matrix, - quaternion_euler, - quaternion_from_euler, - transform_from_odometry, - transform_from_pose, - transform_matrix, - translate_pose_local, - yaw, -) @pytest.mark.parametrize("seconds,nanoseconds", [(0, 0), (-1, 500000000), (1700000000, 123456789)]) diff --git a/dimos/msgs/test_image_compression.py b/dimos/message_utils/test_image_compression.py similarity index 98% rename from dimos/msgs/test_image_compression.py rename to dimos/message_utils/test_image_compression.py index 71e8c04aacd..0514d681424 100644 --- a/dimos/msgs/test_image_compression.py +++ b/dimos/message_utils/test_image_compression.py @@ -17,10 +17,7 @@ import numpy as np import pytest -from dimos.messages.builtin_interfaces.msg import Time -from dimos.messages.sensor_msgs.msg import CompressedImage -from dimos.messages.std_msgs.msg import Header -from dimos.msgs.image import ( +from dimos.message_utils.image import ( compressed_image_from_image, image_from_array, image_from_compressed, @@ -28,7 +25,10 @@ image_to_rgb, image_view, ) -from dimos.msgs.time import time_from_seconds, to_nanoseconds +from dimos.message_utils.time import time_from_seconds, to_nanoseconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.sensor_msgs.msg import CompressedImage +from dimos.messages.std_msgs.msg import Header @pytest.fixture() diff --git a/dimos/msgs/test_image_views.py b/dimos/message_utils/test_image_views.py similarity index 98% rename from dimos/msgs/test_image_views.py rename to dimos/message_utils/test_image_views.py index 4873c3a9983..a4d0680bcaf 100644 --- a/dimos/msgs/test_image_views.py +++ b/dimos/message_utils/test_image_views.py @@ -22,10 +22,7 @@ from PIL import Image as PILImage import pytest -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.msgs.image import ( +from dimos.message_utils.image import ( image_brightness, image_from_array, image_from_file, @@ -34,7 +31,10 @@ image_to_rgb, image_view, ) -from dimos.msgs.time import time_from_nanoseconds +from dimos.message_utils.time import time_from_nanoseconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.std_msgs.msg import Header def test_padded_image_view_retains_owner_and_is_readonly() -> None: diff --git a/dimos/msgs/test_pointcloud.py b/dimos/message_utils/test_pointcloud.py similarity index 99% rename from dimos/msgs/test_pointcloud.py rename to dimos/message_utils/test_pointcloud.py index 7c1237ccd78..48411b23d56 100644 --- a/dimos/msgs/test_pointcloud.py +++ b/dimos/message_utils/test_pointcloud.py @@ -22,13 +22,9 @@ import numpy as np import pytest -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 PointCloud2, PointField -from dimos.messages.std_msgs.msg import Header -from dimos.msgs.camera_info import camera_info_from_intrinsics -from dimos.msgs.image import image_from_array -from dimos.msgs.pointcloud import ( +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 ( concatenate_clouds, pointcloud_from_rgbd, pointcloud_from_xyz, @@ -41,6 +37,10 @@ transform_cloud, voxel_downsample_cloud, ) +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 PointCloud2, PointField +from dimos.messages.std_msgs.msg import Header @pytest.fixture(params=[False, True], ids=["little-endian", "big-endian"]) diff --git a/dimos/msgs/test_pointcloud_colors.py b/dimos/message_utils/test_pointcloud_colors.py similarity index 98% rename from dimos/msgs/test_pointcloud_colors.py rename to dimos/message_utils/test_pointcloud_colors.py index 18fe6680e2b..4043a764772 100644 --- a/dimos/msgs/test_pointcloud_colors.py +++ b/dimos/message_utils/test_pointcloud_colors.py @@ -16,10 +16,10 @@ import numpy as np import pytest +from dimos.message_utils.pointcloud import pointcloud_rgb from dimos.messages.builtin_interfaces.msg import Time from dimos.messages.sensor_msgs.msg import PointCloud2, PointField from dimos.messages.std_msgs.msg import Header -from dimos.msgs.pointcloud import pointcloud_rgb @pytest.mark.parametrize("bigendian", [False, True]) diff --git a/dimos/msgs/test_time.py b/dimos/message_utils/test_time.py similarity index 95% rename from dimos/msgs/test_time.py rename to dimos/message_utils/test_time.py index 8e3fe0f037e..9a5f2e8da5a 100644 --- a/dimos/msgs/test_time.py +++ b/dimos/message_utils/test_time.py @@ -15,7 +15,7 @@ from dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode import pytest -from dimos.msgs.time import ( +from dimos.message_utils.time import ( duration_from_nanoseconds, duration_from_seconds, header_now, @@ -60,7 +60,7 @@ def test_invalid_ros_fraction_is_rejected(): def test_now_reads_integer_clock_and_preserves_frame(monkeypatch): - monkeypatch.setattr("dimos.msgs.time.time.time_ns", lambda: 1_700_000_000_123_456_789) + monkeypatch.setattr("dimos.message_utils.time.time.time_ns", lambda: 1_700_000_000_123_456_789) header = header_now("camera") diff --git a/dimos/msgs/time.py b/dimos/message_utils/time.py similarity index 100% rename from dimos/msgs/time.py rename to dimos/message_utils/time.py diff --git a/dimos/models/embedding/clip.py b/dimos/models/embedding/clip.py index f429dc62a93..7c6580d70ae 100644 --- a/dimos/models/embedding/clip.py +++ b/dimos/models/embedding/clip.py @@ -22,11 +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.image import image_to_rgb -from dimos.msgs.time import to_seconds class CLIPModelConfig(HuggingFaceEmbeddingModelConfig): diff --git a/dimos/models/embedding/dino.py b/dimos/models/embedding/dino.py index 022a442ebf7..3af6d4ed331 100644 --- a/dimos/models/embedding/dino.py +++ b/dimos/models/embedding/dino.py @@ -22,11 +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.image import image_to_rgb -from dimos.msgs.time import to_seconds class DINOModelConfig(HuggingFaceEmbeddingModelConfig): diff --git a/dimos/models/embedding/siglip.py b/dimos/models/embedding/siglip.py index 8cabb1afdf4..da5f523d528 100644 --- a/dimos/models/embedding/siglip.py +++ b/dimos/models/embedding/siglip.py @@ -22,11 +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.image import image_to_rgb -from dimos.msgs.time import to_seconds class SigLIPModelConfig(HuggingFaceEmbeddingModelConfig): diff --git a/dimos/models/embedding/siglip2.py b/dimos/models/embedding/siglip2.py index c85932f9176..c6a2c5be572 100644 --- a/dimos/models/embedding/siglip2.py +++ b/dimos/models/embedding/siglip2.py @@ -22,11 +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.image import image_to_rgb -from dimos.msgs.time import to_seconds TEXT_MAX_LENGTH = 64 diff --git a/dimos/models/embedding/test_generated_images.py b/dimos/models/embedding/test_generated_images.py index a51f9778ba4..793e59acb2e 100644 --- a/dimos/models/embedding/test_generated_images.py +++ b/dimos/models/embedding/test_generated_images.py @@ -20,13 +20,13 @@ 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 -from dimos.msgs.image import image_from_array, image_view @pytest.mark.parametrize("model_type", [CLIPModel, DINOModel, SigLIPModel, SigLIP2Model]) diff --git a/dimos/models/embedding/treid.py b/dimos/models/embedding/treid.py index 3ce7331eec3..56e7cd54465 100644 --- a/dimos/models/embedding/treid.py +++ b/dimos/models/embedding/treid.py @@ -23,11 +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.image import image_to_bgr -from dimos.msgs.time import to_seconds from dimos.utils.data import get_data diff --git a/dimos/models/segmentation/edge_tam.py b/dimos/models/segmentation/edge_tam.py index b65977283c0..443fc9114cd 100644 --- a/dimos/models/segmentation/edge_tam.py +++ b/dimos/models/segmentation/edge_tam.py @@ -27,9 +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.image import image_to_bgr, image_to_jpeg 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 diff --git a/dimos/models/segmentation/test_generated_edge_tam_images.py b/dimos/models/segmentation/test_generated_edge_tam_images.py index ac079411bb5..f921fe971bd 100644 --- a/dimos/models/segmentation/test_generated_edge_tam_images.py +++ b/dimos/models/segmentation/test_generated_edge_tam_images.py @@ -19,8 +19,8 @@ import numpy as np import torch +from dimos.message_utils.image import image_from_array from dimos.models.segmentation import edge_tam -from dimos.msgs.image import image_from_array from dimos.perception.detection.type.detection2d.seg import Detection2DSeg diff --git a/dimos/models/segmentation/test_yoloe.py b/dimos/models/segmentation/test_yoloe.py index 5d2efaffa50..c1ed05664b4 100644 --- a/dimos/models/segmentation/test_yoloe.py +++ b/dimos/models/segmentation/test_yoloe.py @@ -19,11 +19,11 @@ 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.image import image_from_array, image_to_bgr -from dimos.msgs.time import time_from_seconds, to_seconds 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 diff --git a/dimos/models/segmentation/yoloe.py b/dimos/models/segmentation/yoloe.py index fd075227d27..47e2574c9f6 100644 --- a/dimos/models/segmentation/yoloe.py +++ b/dimos/models/segmentation/yoloe.py @@ -23,8 +23,8 @@ from numpy.typing import NDArray from ultralytics.models.yolo.yoloe import YOLOEVPSegPredictor +from dimos.message_utils.image import image_to_bgr from dimos.messages.sensor_msgs.msg import Image -from dimos.msgs.image import image_to_bgr 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 diff --git a/dimos/models/vl/base.py b/dimos/models/vl/base.py index 98f9db3fa84..14f0072ba04 100644 --- a/dimos/models/vl/base.py +++ b/dimos/models/vl/base.py @@ -22,9 +22,9 @@ import cv2 from dimos.core.resource import Resource +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.msgs.image import image_from_array, image_view -from dimos.msgs.time import header_now, to_seconds 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 diff --git a/dimos/models/vl/qwen.py b/dimos/models/vl/qwen.py index ea61a786fb2..a63415b9424 100644 --- a/dimos/models/vl/qwen.py +++ b/dimos/models/vl/qwen.py @@ -21,10 +21,10 @@ 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.image import image_from_array, image_to_jpeg -from dimos.msgs.time import header_now # 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 diff --git a/dimos/models/vl/test_base.py b/dimos/models/vl/test_base.py index 69bcff02343..b10a53949a5 100644 --- a/dimos/models/vl/test_base.py +++ b/dimos/models/vl/test_base.py @@ -20,9 +20,9 @@ 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.image import image_from_array -from dimos.msgs.time import header_now from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D # Captured actual response from Qwen API for cafe.jpg with query "humans" diff --git a/dimos/msgs/image.py b/dimos/msgs/image.py deleted file mode 100644 index 16548a5f175..00000000000 --- a/dimos/msgs/image.py +++ /dev/null @@ -1,311 +0,0 @@ -# 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. - -"""Array views and compression for generated ROS image messages.""" - -from __future__ import annotations - -from pathlib import Path -from typing import Any - -import cv2 -import imagecodecs -import numpy as np -from numpy.typing import NDArray -from PIL import Image as PILImage - -from dimos.messages.builtin_interfaces.msg import Time -from dimos.messages.sensor_msgs.msg import CompressedImage, Image -from dimos.messages.std_msgs.msg import Header - -_FORMATS = { - "rgb8": ("u1", 3), - "bgr8": ("u1", 3), - "rgba8": ("u1", 4), - "bgra8": ("u1", 4), - "mono8": ("u1", 1), - "mono16": ("u2", 1), - "16UC1": ("u2", 1), - "32FC1": ("f4", 1), - "64FC1": ("f8", 1), - "16SC1": ("i2", 1), - "32FC3": ("f4", 3), -} - - -def image_from_array(pixels: NDArray[Any], *, encoding: str, header: Header | None = None) -> Image: - """Copy an array into a generated image with an explicit pixel encoding.""" - if encoding not in _FORMATS: - raise ValueError(f"unsupported image encoding {encoding!r}") - kind, channels = _FORMATS[encoding] - expected = np.dtype(kind) - if pixels.dtype.kind != expected.kind or pixels.dtype.itemsize != expected.itemsize: - raise ValueError(f"array dtype does not match {encoding}") - if (channels == 1 and pixels.ndim != 2) or ( - channels != 1 and (pixels.ndim != 3 or pixels.shape[2] != channels) - ): - raise ValueError(f"array shape does not match {encoding}") - contiguous = np.array(pixels, copy=True, order="C") - return Image( - header=header if header is not None else Header(stamp=Time(sec=0, nanosec=0), frame_id=""), - width=pixels.shape[1], - height=pixels.shape[0], - encoding=encoding, - is_bigendian=int(not pixels.dtype.isnative if np.little_endian else pixels.dtype.isnative), - step=pixels.shape[1] * channels * pixels.dtype.itemsize, - data=np.asarray(contiguous.view(np.uint8).ravel(), dtype=np.uint8), - ) - - -def image_view(msg: Image) -> NDArray[Any]: - """Borrow a read-only array, retaining the message and respecting row padding/endian.""" - if msg.encoding not in _FORMATS: - raise ValueError(f"unsupported image encoding {msg.encoding!r}") - kind, channels = _FORMATS[msg.encoding] - dtype = np.dtype((">" if msg.is_bigendian else "<") + kind) - row_bytes = msg.width * channels * dtype.itemsize - if msg.step < row_bytes or len(msg.data) != msg.height * msg.step: - raise ValueError("image data length/step does not match its dimensions") - shape = (msg.height, msg.width) if channels == 1 else (msg.height, msg.width, channels) - strides = ( - (msg.step, dtype.itemsize) - if channels == 1 - else (msg.step, channels * dtype.itemsize, dtype.itemsize) - ) - view = np.ndarray(shape, dtype=dtype, buffer=msg.data, strides=strides) - view.setflags(write=False) - return view - - -def image_to_jpeg(msg: Image, quality: int = 75) -> bytes: - """Encode an 8-bit gray or color image using OpenCV's JPEG encoder.""" - - if isinstance(quality, bool) or not 0 <= quality <= 100: - raise ValueError("JPEG quality must be in 0..100") - pixels = image_view(msg) - conversions = { - "rgb8": cv2.COLOR_RGB2BGR, - "rgba8": cv2.COLOR_RGBA2BGR, - "bgra8": cv2.COLOR_BGRA2BGR, - } - if msg.encoding in conversions: - pixels = cv2.cvtColor(pixels, conversions[msg.encoding]) - elif msg.encoding not in ("bgr8", "mono8"): - raise ValueError(f"JPEG requires 8-bit color or gray, got {msg.encoding!r}") - ok, encoded = cv2.imencode(".jpg", pixels, [cv2.IMWRITE_JPEG_QUALITY, quality]) - if not ok: - raise ValueError("JPEG encoding failed") - return bytes(encoded) - - -def compressed_image_from_image( - message: Image, - quality: int = 75, - *, - format: str = "jpeg", - max_width: int | None = None, - effort: int | None = None, -) -> CompressedImage: - """Compress generated pixels, retaining the exact header. - - PNG preserves uint16 depth. JXL preserves uint16/float32 depth losslessly; - it requires imagecodecs and is an explicit codec extension for consumers. - """ - if not isinstance(message, Image): - raise TypeError("compression expects a generated Image") - if max_width is not None: - message, _ = image_resize_to_fit(message, max_width, max_width) - if format == "jpeg": - output_encoding = "mono8" if message.encoding == "mono8" else "bgr8" - data = image_to_jpeg(message, quality=quality) - elif format == "png": - pixels = image_view(message) - if pixels.dtype.kind != "u" or pixels.dtype.itemsize not in (1, 2): - raise ValueError("PNG cannot encode floating-point depth") - if message.encoding in ("rgb8", "rgba8"): - code = cv2.COLOR_RGB2BGR if message.encoding == "rgb8" else cv2.COLOR_RGBA2BGRA - pixels = cv2.cvtColor(pixels, code) - pixels = np.ascontiguousarray(pixels, dtype=pixels.dtype.newbyteorder("=")) - ok, encoded = cv2.imencode(".png", pixels) - if not ok: - raise ValueError("PNG encoding failed") - data = bytes(encoded) - output_encoding = {"rgb8": "bgr8", "rgba8": "bgra8"}.get(message.encoding, message.encoding) - elif format == "jxl": - pixels = image_view(message) - if pixels.dtype.kind not in ("u", "f") or ( - pixels.dtype.kind, - pixels.dtype.itemsize, - ) not in (("u", 1), ("u", 2), ("f", 4)): - raise ValueError(f"JXL cannot encode dtype {pixels.dtype}") - if message.encoding == "bgr8": - pixels = pixels[..., ::-1] - elif message.encoding == "bgra8": - pixels = pixels[..., [2, 1, 0, 3]] - pixels = np.ascontiguousarray(pixels, dtype=pixels.dtype.newbyteorder("=")) - if pixels.dtype == np.uint8: - data = bytes(imagecodecs.jpegxl_encode(pixels, level=quality, effort=effort or 3)) - else: - data = bytes(imagecodecs.jpegxl_encode(pixels, lossless=True, effort=effort or 1)) - output_encoding = {"bgr8": "rgb8", "bgra8": "rgba8"}.get(message.encoding, message.encoding) - else: - raise ValueError(f"unsupported compression format {format!r}") - return CompressedImage( - header=message.header, - format=f"{message.encoding}; {format} compressed {output_encoding}", - data=np.asarray(np.frombuffer(data, dtype=np.uint8).copy(), dtype=np.uint8), - ) - - -def image_from_compressed(message: CompressedImage) -> Image: - """Decode JPEG/PNG or the JXL extension into explicitly encoded generated pixels.""" - - format_name = message.format.lower() - if "jxl" in format_name: - jxl_pixels = imagecodecs.jpegxl_decode(bytes(message.data)) - if jxl_pixels.ndim == 2: - encoding = { - np.dtype("float32"): "32FC1", - np.dtype("uint16"): "mono16", - np.dtype("uint8"): "mono8", - }.get(jxl_pixels.dtype) - elif jxl_pixels.ndim == 3 and jxl_pixels.shape[2] in (3, 4): - encoding = "rgb8" if jxl_pixels.shape[2] == 3 else "rgba8" - else: - encoding = None - if encoding is None: - raise ValueError("unsupported decoded JXL layout") - if encoding == "mono16" and format_name.startswith("16uc1;"): - encoding = "16UC1" - return image_from_array(jxl_pixels, encoding=encoding, header=message.header) - if not any(name in format_name for name in ("jpeg", "jpg", "png")): - raise ValueError(f"unsupported compressed image format {message.format!r}") - pixels = cv2.imdecode(np.frombuffer(bytes(message.data), dtype=np.uint8), cv2.IMREAD_UNCHANGED) - if pixels is None: - raise ValueError("invalid compressed image data") - if pixels.ndim == 2: - encoding = "mono16" if pixels.dtype.itemsize == 2 else "mono8" - elif pixels.ndim == 3 and pixels.shape[2] == 3: - encoding = "bgr8" - elif pixels.ndim == 3 and pixels.shape[2] == 4: - encoding = "bgra8" - else: - raise ValueError("unsupported decoded image layout") - if encoding == "mono16" and format_name.startswith("16uc1;"): - encoding = "16UC1" - return image_from_array(pixels, encoding=encoding, header=message.header) - - -def image_brightness(message: Image) -> float: - """Sample mean pixel intensity in [0, 1] for 8/16-bit image encodings. - - Respect row padding and endian. Floating-point depth has no normalized - intensity scale and is rejected rather than treated as a color image. - """ - pixels = image_view(message) - if pixels.dtype.kind != "u": - raise ValueError("brightness requires unsigned integer pixels") - stride = max(1, max(message.height, message.width) // 256) - return float(pixels[::stride, ::stride].mean() / np.iinfo(pixels.dtype).max) - - -def image_sharpness(message: Image) -> float: - """Laplacian variance of an 8-bit visual image, downsampled to 160 pixels wide.""" - - pixels = image_view(message) - codes = { - "rgb8": cv2.COLOR_RGB2GRAY, - "bgr8": cv2.COLOR_BGR2GRAY, - "rgba8": cv2.COLOR_RGBA2GRAY, - "bgra8": cv2.COLOR_BGRA2GRAY, - } - if message.encoding == "mono8": - gray = pixels - elif message.encoding in codes: - gray = cv2.cvtColor(pixels, codes[message.encoding]) - else: - raise ValueError("Sharpness requires an 8-bit visual image encoding") - if gray.size == 0: - raise ValueError("Sharpness requires a nonempty image") - if message.width > 160: - gray = cv2.resize( - gray, - (160, max(1, round(message.height * 160 / message.width))), - interpolation=cv2.INTER_AREA, - ) - return float(cv2.Laplacian(gray, cv2.CV_64F).var()) - - -def image_to_bgr(message: Image) -> NDArray[np.uint8]: - """Return an independent BGR8 array for drawing or OpenCV color operations.""" - - pixels = image_view(message) - if message.encoding == "bgr8": - return pixels.copy() - codes = { - "rgb8": cv2.COLOR_RGB2BGR, - "rgba8": cv2.COLOR_RGBA2BGR, - "bgra8": cv2.COLOR_BGRA2BGR, - "mono8": cv2.COLOR_GRAY2BGR, - } - if message.encoding not in codes: - raise ValueError(f"Cannot convert {message.encoding!r} to BGR8") - return np.asarray(cv2.cvtColor(pixels, codes[message.encoding]), dtype=np.uint8) - - -def image_to_rgb(message: Image) -> NDArray[np.uint8]: - """Copy a display image as RGB8; 16-bit grayscale uses its high byte. - - Alpha is discarded. Floating-point depth needs an explicit visualization - scale and is rejected rather than assigning pixel values to meters. - """ - - pixels = image_view(message) - if message.encoding == "rgb8": - return pixels.copy() - if message.encoding in ("mono16", "16UC1"): - return np.asarray( - cv2.cvtColor((pixels / 256).astype(np.uint8), cv2.COLOR_GRAY2RGB), dtype=np.uint8 - ) - codes = { - "bgr8": cv2.COLOR_BGR2RGB, - "rgba8": cv2.COLOR_RGBA2RGB, - "bgra8": cv2.COLOR_BGRA2RGB, - "mono8": cv2.COLOR_GRAY2RGB, - } - if message.encoding not in codes: - raise ValueError(f"Cannot convert {message.encoding!r} to RGB8") - return np.asarray(cv2.cvtColor(pixels, codes[message.encoding]), dtype=np.uint8) - - -def image_resize_to_fit(message: Image, max_width: int, max_height: int) -> tuple[Image, float]: - """Downscale a generated image while preserving encoding, aspect and exact header.""" - - if min(max_width, max_height, message.width, message.height) <= 0: - raise ValueError("Image and target dimensions must be positive") - if message.width <= max_width and message.height <= max_height: - return message, 1.0 - scale = min(max_width / message.width, max_height / message.height) - pixels = cv2.resize( - image_view(message), - (max(1, int(message.width * scale)), max(1, int(message.height * scale))), - interpolation=cv2.INTER_LINEAR, - ) - return image_from_array(pixels, encoding=message.encoding, header=message.header), scale - - -def image_from_file(path: str | Path, *, header: Header | None = None) -> Image: - """Decode an image file as generated RGB8 with an explicit optional source header.""" - with PILImage.open(path) as image: - return image_from_array(np.asarray(image.convert("RGB")), encoding="rgb8", header=header) diff --git a/dimos/msgs/pointcloud.py b/dimos/msgs/pointcloud.py deleted file mode 100644 index b28135a2864..00000000000 --- a/dimos/msgs/pointcloud.py +++ /dev/null @@ -1,380 +0,0 @@ -# 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. - -"""NumPy conversions for generated PointCloud2 messages.""" - -from copy import deepcopy -import math -from typing import Any - -from dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode -import numpy as np -from numpy.typing import NDArray -import open3d as o3d # type: ignore[import-untyped] - -from dimos.messages.geometry_msgs.msg import TransformStamped -from dimos.messages.sensor_msgs.msg import CameraInfo, Image, PointCloud2, PointField -from dimos.messages.std_msgs.msg import Header -from dimos.msgs.camera_info import intrinsic_matrix -from dimos.msgs.geometry import transform_matrix -from dimos.msgs.image import image_to_rgb, image_view -from dimos.msgs.time import to_nanoseconds - -_FIELD_KINDS = {1: "i1", 2: "u1", 3: "i2", 4: "u2", 5: "i4", 6: "u4", 7: "f4", 8: "f8"} - - -def pointcloud_view(message: PointCloud2) -> NDArray[Any]: - """Borrow read-only structured (height, width) points, including row/field padding. - - Fields retain their declared counts and endianness. The returned array keeps - the message's storage alive. Use ``view.copy()`` for independent mutable data. - """ - if message.width and not message.point_step: - raise ValueError("nonempty point cloud requires a positive point_step") - if message.row_step < message.width * message.point_step: - raise ValueError("point cloud row_step is smaller than its row of points") - if len(message.data) != message.height * message.row_step: - raise ValueError("point cloud data length does not match height * row_step") - names: list[str] = [] - formats: list[Any] = [] - offsets: list[int] = [] - for field in message.fields: - if not field.name or field.name in names: - raise ValueError("point cloud field names must be nonempty and unique") - if field.datatype not in _FIELD_KINDS or not field.count: - raise ValueError(f"invalid point cloud datatype/count for {field.name!r}") - dtype = np.dtype((">" if message.is_bigendian else "<") + _FIELD_KINDS[field.datatype]) - if field.offset + dtype.itemsize * field.count > message.point_step: - raise ValueError(f"point cloud field {field.name!r} exceeds point_step") - names.append(field.name) - formats.append(dtype if field.count == 1 else (dtype, (field.count,))) - offsets.append(field.offset) - dtype = np.dtype( - {"names": names, "formats": formats, "offsets": offsets, "itemsize": message.point_step} - ) - view = np.ndarray( - (message.height, message.width), - dtype=dtype, - buffer=message.data, - strides=(message.row_step, message.point_step), - ) - view.setflags(write=False) - return view - - -def pointcloud_xyz(message: PointCloud2) -> NDArray[np.float64]: - """Copy scalar x/y/z fields into an N-by-3 float64 array, preserving point order.""" - points = pointcloud_view(message) - for name in ("x", "y", "z"): - if name not in (points.dtype.names or ()) or points[name].ndim != 2: - raise ValueError(f"point cloud requires a scalar {name!r} field") - return np.stack([points[name].ravel() for name in ("x", "y", "z")], axis=-1).astype(np.float64) - - -def pointcloud_from_xyz( - points: NDArray[Any], *, header: Header, stamps: NDArray[Any] | None = None -) -> PointCloud2: - """Copy Nx3 or HxWx3 coordinates into a tightly packed little-endian XYZ cloud. - - XYZ fields are float32. NaNs/infinities remain missing-point markers and set - is_dense=False; finite coordinates outside float32 range are rejected. - """ - values = np.asarray(points) - if values.ndim not in (2, 3) or values.shape[-1] != 3: - raise ValueError("XYZ coordinates must have shape (N, 3) or (H, W, 3)") - if values.dtype.kind not in "fiu": - raise ValueError("XYZ coordinates must be numeric") - finite = np.isfinite(values) - if np.any(np.abs(values[finite].astype(np.float64)) > np.finfo(np.float32).max): - raise ValueError("XYZ coordinates exceed float32 range") - packed = np.ascontiguousarray(values, dtype=" NDArray[np.float64] | None: - """Read per-point capture times; absence falls back to the header at the caller.""" - fields = [field for field in message.fields if field.name == "stamps"] - if not fields: - return None - if len(fields) != 1 or fields[0].datatype != PointField.FLOAT64 or fields[0].count != 1: - raise ValueError("Per-point stamps must be a scalar FLOAT64 field") - return np.asarray(pointcloud_view(message)["stamps"].reshape(-1), dtype=np.float64) - - -def select_points(message: PointCloud2, keep: NDArray[np.bool_]) -> PointCloud2: - """Copy selected point records in row order, retaining every field and point byte. - - The result is unorganized with no row padding. Point padding, endianness, - field metadata, source header, and the conservative density flag survive. - """ - pointcloud_view(message) # Validate the full declared layout before selecting bytes. - if keep.dtype != np.bool_ or keep.shape != (message.height * message.width,): - raise ValueError("point selection must be a flat boolean mask matching the point count") - records: NDArray[np.uint8] = np.ndarray( - (message.height, message.width, message.point_step), - dtype=np.uint8, - buffer=message.data, - strides=(message.row_step, message.point_step, 1), - ) - selected = records.reshape(message.height * message.width, message.point_step)[keep] - width = len(selected) - return PointCloud2( - header=deepcopy(message.header), - height=1, - width=width, - fields=deepcopy(message.fields), - is_bigendian=message.is_bigendian, - point_step=message.point_step, - row_step=width * message.point_step, - data=np.asarray(np.frombuffer(selected.tobytes(), dtype=np.uint8).copy(), dtype=np.uint8), - is_dense=message.is_dense, - ) - - -def pointcloud_rgb(message: PointCloud2) -> NDArray[np.uint8] | None: - """Copy RGB bytes from the standard packed rgb/rgba field, if present. - - FLOAT32 fields carry color bits, not numeric floating-point color values. - Both representations preserve the declared byte order and row padding. - """ - points = pointcloud_view(message) - names = points.dtype.names or () - name = next((name for name in ("rgb", "rgba") if name in names), None) - if name is None: - return None - field = points[name] - if field.ndim != 2 or field.dtype.kind not in "fu" or field.dtype.itemsize != 4: - raise ValueError("packed point cloud color must be a scalar FLOAT32 or UINT32 field") - packed = field.view(np.dtype(">u4" if message.is_bigendian else "> shift) & 255 for shift in (16, 8, 0)], axis=-1).astype(np.uint8) - - -def concatenate_clouds(first: PointCloud2, second: PointCloud2) -> PointCloud2: - """Join compatible point records, preserving fields, endian, and point padding. - - Row padding is removed. The first frame and newest exact source stamp survive. - Empty inputs are identities; differing nonempty frames/layouts are rejected. - """ - pointcloud_view(first) - pointcloud_view(second) - if not first.width * first.height: - return cdr_decode(cdr_encode(second), PointCloud2) - if not second.width * second.height: - return cdr_decode(cdr_encode(first), PointCloud2) - if first.header.frame_id != second.header.frame_id: - raise ValueError("cannot concatenate point clouds in different frames") - if ( - first.fields != second.fields - or first.point_step != second.point_step - or first.is_bigendian != second.is_bigendian - ): - raise ValueError("cannot concatenate point clouds with different record layouts") - a = select_points(first, np.ones(first.width * first.height, dtype=np.bool_)) - b = select_points(second, np.ones(second.width * second.height, dtype=np.bool_)) - stamp = max((first.header.stamp, second.header.stamp), key=to_nanoseconds) - return PointCloud2( - header=Header(frame_id=first.header.frame_id, stamp=stamp), - height=1, - width=a.width + b.width, - fields=deepcopy(first.fields), - is_bigendian=first.is_bigendian, - point_step=first.point_step, - row_step=a.row_step + b.row_step, - data=np.asarray(np.concatenate((a.data, b.data)), dtype=np.uint8), - is_dense=first.is_dense and second.is_dense, - ) - - -def transform_cloud(message: PointCloud2, transform: TransformStamped) -> PointCloud2: - """Apply parent←child to XYZ while retaining the complete organized record layout. - - Copy storage before changing coordinates. Keep all other fields, point/row - padding, endian and the cloud's exact source stamp. Frames must match; - integer XYZ is rejected because transformed coordinates would be truncated. - """ - if message.header.frame_id != transform.child_frame_id: - raise ValueError("point cloud frame does not match transform child frame") - view = pointcloud_view(message) - for name in ("x", "y", "z"): - if name not in (view.dtype.names or ()) or view.dtype[name].kind != "f": - raise ValueError("transform requires scalar floating-point XYZ fields") - matrix = transform_matrix(transform.transform) - xyz = pointcloud_xyz(message) - transformed = xyz @ matrix[:3, :3].T + matrix[:3, 3] - for index, name in enumerate(("x", "y", "z")): - values = transformed[:, index] - if np.any(np.abs(values[np.isfinite(values)]) > np.finfo(view.dtype[name]).max): - raise ValueError(f"transformed {name} coordinates exceed field range") - payload = bytearray(bytes(message.data)) - writable = np.ndarray( - (message.height, message.width), - dtype=view.dtype, - buffer=payload, - strides=(message.row_step, message.point_step), - ) - for index, name in enumerate(("x", "y", "z")): - writable[name] = transformed[:, index].reshape(message.height, message.width) - output = cdr_decode(cdr_encode(message), PointCloud2) - output.data = np.frombuffer(payload, dtype=np.uint8).copy() - output.header = Header(stamp=deepcopy(message.header.stamp), frame_id=transform.header.frame_id) - return output - - -def pointcloud_from_xyz_rgb( - points: NDArray[Any], colors: NDArray[np.uint8], *, header: Header -) -> PointCloud2: - """Copy XYZ and RGB8 into a declared XYZ FLOAT32 / packed RGB UINT32 layout.""" - cloud = pointcloud_from_xyz(points, header=header) - rgb = np.asarray(colors) - if rgb.dtype != np.uint8 or rgb.shape != np.asarray(points).shape: - raise ValueError("RGB must be uint8 with the same (..., 3) shape as XYZ") - records = np.empty((cloud.height, cloud.width), dtype=[("xyz", " PointCloud2: - """Project a rectified RGB/depth pair into the depth optical frame. - - Integer depth is multiplied by depth_scale to obtain meters; 32FC1 already - represents meters. Invalid/nonpositive/too-distant samples are removed. - Scale K when calibration resolution differs, retaining the depth header. - """ - if ( - not math.isfinite(depth_scale) - or depth_scale <= 0 - or not math.isfinite(depth_trunc) - or depth_trunc <= 0 - ): - raise ValueError("depth scale and truncation must be finite and positive") - if (color.width, color.height) != (depth.width, depth.height): - raise ValueError("color and depth dimensions do not match") - if depth.encoding not in ("16UC1", "mono16", "32FC1"): - raise ValueError("RGBD requires uint16 or float32 depth") - if calibration.width <= 0 or calibration.height <= 0: - raise ValueError("calibration dimensions must be positive") - matrix = intrinsic_matrix(calibration).copy() - matrix[0, :] *= depth.width / calibration.width - matrix[1, :] *= depth.height / calibration.height - distances = image_view(depth).astype(np.float64) - if depth.encoding != "32FC1": - distances *= depth_scale - valid = np.isfinite(distances) & (distances > 0) & (distances < depth_trunc) - rows = np.broadcast_to(np.arange(depth.height, dtype=np.float64)[:, None], distances.shape) - columns = np.broadcast_to(np.arange(depth.width, dtype=np.float64)[None, :], distances.shape) - z = distances[valid] - xyz = np.column_stack( - ( - (columns[valid] - matrix[0, 2]) * z / matrix[0, 0], - (rows[valid] - matrix[1, 2]) * z / matrix[1, 1], - z, - ) - ) - return pointcloud_from_xyz_rgb(xyz, image_to_rgb(color)[valid], header=depth.header) - - -def voxel_downsample_cloud(message: PointCloud2, voxel_size: float = 0.025) -> PointCloud2: - """Downsample XYZ/RGB clouds with the existing Open3D tensor backend. - - Non-positive sizes and clouds below twenty points retain the existing fast - path. Unsupported extra fields are rejected rather than silently dropped. - Source headers are preserved; generated messages own no numerical methods. - """ - if not math.isfinite(voxel_size): - raise ValueError("voxel_size must be finite") - if voxel_size <= 0 or message.width * message.height < 20: - return message - names = {field.name for field in message.fields} - if names not in ({"x", "y", "z"}, {"x", "y", "z", "rgb"}): - raise ValueError("voxel downsampling supports only XYZ and optional RGB fields") - points = pointcloud_xyz(message).astype(np.float32) - if not np.isfinite(points).all(): - raise ValueError("voxel downsampling requires finite XYZ positions") - colors = pointcloud_rgb(message) - - device = o3d.core.Device("CPU:0") - cloud = o3d.t.geometry.PointCloud(o3d.core.Tensor(points, o3d.core.Dtype.Float32, device)) - if colors is not None: - cloud.point["colors"] = o3d.core.Tensor( - colors.astype(np.float32) / 255.0, o3d.core.Dtype.Float32, device - ) - reduced = cloud.voxel_down_sample(voxel_size) - xyz = reduced.point["positions"].numpy() - if colors is None: - return pointcloud_from_xyz(xyz, header=message.header) - rgb = np.clip(reduced.point["colors"].numpy() * 255.0, 0, 255).astype(np.uint8) - return pointcloud_from_xyz_rgb(xyz, rgb, header=message.header) - - -def pointcloud_to_open3d(message: PointCloud2) -> Any: - """Copy generated XYZ/RGB values into a native geometry algorithm input.""" - - result = o3d.geometry.PointCloud() - result.points = o3d.utility.Vector3dVector(pointcloud_xyz(message)) - colors = pointcloud_rgb(message) - if colors is not None: - result.colors = o3d.utility.Vector3dVector(colors.astype(np.float64) / 255.0) - return result - - -def cloud_bounds_intersect(first: PointCloud2, second: PointCloud2) -> bool: - """Test closed axis-aligned bounds; empty or entirely nonfinite clouds do not intersect.""" - a, b = pointcloud_xyz(first), pointcloud_xyz(second) - a, b = a[np.isfinite(a).all(axis=1)], b[np.isfinite(b).all(axis=1)] - if len(a) == 0 or len(b) == 0: - return False - return bool(np.all(a.min(axis=0) <= b.max(axis=0)) and np.all(a.max(axis=0) >= b.min(axis=0))) diff --git a/dimos/perception/detection/conftest.py b/dimos/perception/detection/conftest.py index 65bd5fd81ae..13437cdccba 100644 --- a/dimos/perception/detection/conftest.py +++ b/dimos/perception/detection/conftest.py @@ -27,14 +27,14 @@ import pytest from dimos.core.transport import LCMTransport +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.msgs.camera_info import camera_info_from_yaml -from dimos.msgs.geometry import transform_from_pose -from dimos.msgs.time import to_nanoseconds from dimos.perception.detection.detectors.yolo import Yolo2DDetector from dimos.perception.detection.module2D import Detection2DModule from dimos.perception.detection.module3D import Detection3DModule diff --git a/dimos/perception/detection/detectors/grounding_dino.py b/dimos/perception/detection/detectors/grounding_dino.py index 5198f7b5d52..78c48e89375 100644 --- a/dimos/perception/detection/detectors/grounding_dino.py +++ b/dimos/perception/detection/detectors/grounding_dino.py @@ -22,10 +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.image import image_to_rgb -from dimos.msgs.time import to_seconds from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D diff --git a/dimos/perception/detection/detectors/omdet.py b/dimos/perception/detection/detectors/omdet.py index 38fa172815b..d5befe460d1 100644 --- a/dimos/perception/detection/detectors/omdet.py +++ b/dimos/perception/detection/detectors/omdet.py @@ -22,10 +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.image import image_to_rgb -from dimos.msgs.time import to_seconds from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D diff --git a/dimos/perception/detection/detectors/owlv2.py b/dimos/perception/detection/detectors/owlv2.py index e5e58dd1549..3907665ea16 100644 --- a/dimos/perception/detection/detectors/owlv2.py +++ b/dimos/perception/detection/detectors/owlv2.py @@ -23,10 +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.image import image_to_rgb -from dimos.msgs.time import to_nanoseconds, to_seconds from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D diff --git a/dimos/perception/detection/detectors/person/yolo.py b/dimos/perception/detection/detectors/person/yolo.py index c8eef6f48bf..475067ac91b 100644 --- a/dimos/perception/detection/detectors/person/yolo.py +++ b/dimos/perception/detection/detectors/person/yolo.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos.message_utils.image import image_to_bgr from dimos.messages.sensor_msgs.msg import Image -from dimos.msgs.image import image_to_bgr from dimos.perception.detection.detectors.base import Detector from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.utils.data import resolve_named_path diff --git a/dimos/perception/detection/detectors/test_generated_images.py b/dimos/perception/detection/detectors/test_generated_images.py index 42ea4d1473a..865150f17ab 100644 --- a/dimos/perception/detection/detectors/test_generated_images.py +++ b/dimos/perception/detection/detectors/test_generated_images.py @@ -21,10 +21,10 @@ 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.msgs.image import image_from_array, image_view from dimos.perception.detection.detectors.omdet import OmDetDetector from dimos.perception.detection.detectors.owlv2 import Owlv2Detector diff --git a/dimos/perception/detection/detectors/yolo.py b/dimos/perception/detection/detectors/yolo.py index 183b8066937..bf682da8f52 100644 --- a/dimos/perception/detection/detectors/yolo.py +++ b/dimos/perception/detection/detectors/yolo.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos.message_utils.image import image_to_bgr from dimos.messages.sensor_msgs.msg import Image -from dimos.msgs.image import image_to_bgr from dimos.perception.detection.detectors.base import Detector from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.utils.data import resolve_named_path diff --git a/dimos/perception/detection/detectors/yoloe.py b/dimos/perception/detection/detectors/yoloe.py index c51f2ab2a7c..952bf050b97 100644 --- a/dimos/perception/detection/detectors/yoloe.py +++ b/dimos/perception/detection/detectors/yoloe.py @@ -20,8 +20,8 @@ from numpy.typing import NDArray import torch +from dimos.message_utils.image import image_to_bgr from dimos.messages.sensor_msgs.msg import Image -from dimos.msgs.image import image_to_bgr from dimos.perception.detection.detectors.base import Detector from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.utils.data import get_data diff --git a/dimos/perception/detection/module2D.py b/dimos/perception/detection/module2D.py index af8fe3f3e23..5d1e11c1eba 100644 --- a/dimos/perception/detection/module2D.py +++ b/dimos/perception/detection/module2D.py @@ -23,13 +23,13 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import IO, In, Out +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.msgs.image import image_sharpness -from dimos.msgs.time import to_seconds 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 diff --git a/dimos/perception/detection/module3D.py b/dimos/perception/detection/module3D.py index c034fd57657..0e662ed6087 100644 --- a/dimos/perception/detection/module3D.py +++ b/dimos/perception/detection/module3D.py @@ -19,6 +19,7 @@ from dimos.agents.annotation import skill from dimos.core.core import rpc from dimos.core.stream import In, Out +from dimos.message_utils.time import to_seconds from dimos.messages.geometry_msgs.msg import ( Point, Pose, @@ -30,7 +31,6 @@ 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.msgs.time import to_seconds 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 diff --git a/dimos/perception/detection/person_tracker.py b/dimos/perception/detection/person_tracker.py index 7140484d517..5a6a35be049 100644 --- a/dimos/perception/detection/person_tracker.py +++ b/dimos/perception/detection/person_tracker.py @@ -22,13 +22,17 @@ from dimos.core.core import rpc from dimos.core.module import Module from dimos.core.stream import In, Out +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.msgs.geometry import compose_transforms, pose_from_transform, transform_from_pose -from dimos.msgs.time import to_nanoseconds from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.types.timestamped import TimestampedData, align_timestamped from dimos.utils.reactive import backpressure diff --git a/dimos/perception/detection/project.py b/dimos/perception/detection/project.py index c4fb96b9a3c..01d6432a385 100644 --- a/dimos/perception/detection/project.py +++ b/dimos/perception/detection/project.py @@ -21,15 +21,15 @@ import numpy as np from dimos.memory.transform import Transformer -from dimos.messages.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 -from dimos.messages.std_msgs.msg import Header -from dimos.msgs.geometry import ( +from dimos.message_utils.geometry import ( compose_transforms, inverse_transform, transform_matrix, ) -from dimos.msgs.image import image_view -from dimos.msgs.time import time_from_seconds +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: diff --git a/dimos/perception/detection/reid/module.py b/dimos/perception/detection/reid/module.py index 17219e38cda..eba13fd58ee 100644 --- a/dimos/perception/detection/reid/module.py +++ b/dimos/perception/detection/reid/module.py @@ -18,9 +18,9 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In +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.msgs.time import to_nanoseconds 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 diff --git a/dimos/perception/detection/reid/test_generated_module.py b/dimos/perception/detection/reid/test_generated_module.py index 99b747131c2..6e2e438f208 100644 --- a/dimos/perception/detection/reid/test_generated_module.py +++ b/dimos/perception/detection/reid/test_generated_module.py @@ -22,14 +22,14 @@ 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.msgs.camera_info import camera_info_from_intrinsics -from dimos.msgs.image import image_from_array from dimos.perception.detection.person_tracker import ( PersonTracker, _align_images_and_detections as align_person, diff --git a/dimos/perception/detection/type/detection2d/bbox.py b/dimos/perception/detection/type/detection2d/bbox.py index 91ef1f9a0f3..3c1ee8ea006 100644 --- a/dimos/perception/detection/type/detection2d/bbox.py +++ b/dimos/perception/detection/type/detection2d/bbox.py @@ -31,6 +31,8 @@ from rich.console import Console from rich.text import Text +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, @@ -39,8 +41,6 @@ Point2D, Pose2D, ) -from dimos.msgs.image import image_from_array, image_to_bgr, image_view -from dimos.msgs.time import to_seconds from dimos.perception.detection.type.detection2d.base import Detection2D from dimos.utils.decorators.decorators import simple_mcache diff --git a/dimos/perception/detection/type/detection2d/person.py b/dimos/perception/detection/type/detection2d/person.py index cbad18c261a..415d765471f 100644 --- a/dimos/perception/detection/type/detection2d/person.py +++ b/dimos/perception/detection/type/detection2d/person.py @@ -19,8 +19,8 @@ import numpy as np +from dimos.message_utils.time import to_seconds from dimos.messages.sensor_msgs.msg import Image -from dimos.msgs.time import to_seconds from dimos.perception.detection.type.detection2d.bbox import Bbox, Detection2DBBox from dimos.utils.decorators.decorators import simple_mcache diff --git a/dimos/perception/detection/type/detection2d/point.py b/dimos/perception/detection/type/detection2d/point.py index 5a4c7de28cb..4e77730f25a 100644 --- a/dimos/perception/detection/type/detection2d/point.py +++ b/dimos/perception/detection/type/detection2d/point.py @@ -19,6 +19,7 @@ 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, @@ -28,7 +29,6 @@ Point2D, Pose2D, ) -from dimos.msgs.image import image_from_array, image_view from dimos.perception.detection.type.detection2d.base import Detection2D if TYPE_CHECKING: diff --git a/dimos/perception/detection/type/detection2d/seg.py b/dimos/perception/detection/type/detection2d/seg.py index 8d7879c86a5..61a7d6ec022 100644 --- a/dimos/perception/detection/type/detection2d/seg.py +++ b/dimos/perception/detection/type/detection2d/seg.py @@ -19,7 +19,7 @@ import numpy as np -from dimos.msgs.time import to_seconds +from dimos.message_utils.time import to_seconds from dimos.perception.detection.type.detection2d.bbox import Bbox, Detection2DBBox if TYPE_CHECKING: diff --git a/dimos/perception/detection/type/detection2d/test_bbox.py b/dimos/perception/detection/type/detection2d/test_bbox.py index 312841ff60a..4beda1117d8 100644 --- a/dimos/perception/detection/type/detection2d/test_bbox.py +++ b/dimos/perception/detection/type/detection2d/test_bbox.py @@ -13,7 +13,7 @@ # limitations under the License. import pytest -from dimos.msgs.image import image_view +from dimos.message_utils.image import image_view @pytest.mark.self_hosted diff --git a/dimos/perception/detection/type/detection2d/test_generated_bbox.py b/dimos/perception/detection/type/detection2d/test_generated_bbox.py index e2d90c3655e..777cfc81fea 100644 --- a/dimos/perception/detection/type/detection2d/test_generated_bbox.py +++ b/dimos/perception/detection/type/detection2d/test_generated_bbox.py @@ -15,10 +15,10 @@ 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.msgs.image import image_from_array, image_view 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 diff --git a/dimos/perception/detection/type/detection2d/test_person.py b/dimos/perception/detection/type/detection2d/test_person.py index 8a0b80cb436..201aff833c3 100644 --- a/dimos/perception/detection/type/detection2d/test_person.py +++ b/dimos/perception/detection/type/detection2d/test_person.py @@ -13,7 +13,7 @@ # limitations under the License. import pytest -from dimos.msgs.image import image_from_file +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 diff --git a/dimos/perception/detection/type/detection3d/bbox.py b/dimos/perception/detection/type/detection3d/bbox.py index 9824e2dec1d..0632d606198 100644 --- a/dimos/perception/detection/type/detection3d/bbox.py +++ b/dimos/perception/detection/type/detection3d/bbox.py @@ -20,6 +20,7 @@ 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, @@ -37,7 +38,6 @@ ObjectHypothesis, ObjectHypothesisWithPose, ) -from dimos.msgs.geometry import point_distance from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox diff --git a/dimos/perception/detection/type/detection3d/imageDetections3DPC.py b/dimos/perception/detection/type/detection3d/imageDetections3DPC.py index 4f3edf1a122..2c7737bdb13 100644 --- a/dimos/perception/detection/type/detection3d/imageDetections3DPC.py +++ b/dimos/perception/detection/type/detection3d/imageDetections3DPC.py @@ -18,8 +18,8 @@ import numpy as np -from dimos.msgs.pointcloud import select_points -from dimos.msgs.time import to_seconds +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 diff --git a/dimos/perception/detection/type/detection3d/pointcloud.py b/dimos/perception/detection/type/detection3d/pointcloud.py index b685b5eac5c..c82ac3c5458 100644 --- a/dimos/perception/detection/type/detection3d/pointcloud.py +++ b/dimos/perception/detection/type/detection3d/pointcloud.py @@ -23,6 +23,15 @@ 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, @@ -34,15 +43,6 @@ ) from dimos.messages.sensor_msgs.msg import PointCloud2 from dimos.messages.std_msgs.msg import Header -from dimos.msgs.geometry import inverse_transform, transform_matrix -from dimos.msgs.image import image_view -from dimos.msgs.pointcloud import ( - concatenate_clouds, - pointcloud_from_xyz, - pointcloud_xyz, - select_points, -) -from dimos.msgs.time import time_from_seconds, to_seconds from dimos.perception.detection.type.detection3d.base import Detection3D from dimos.perception.detection.type.detection3d.pointcloud_filters import ( PointCloudFilter, diff --git a/dimos/perception/detection/type/detection3d/pointcloud_filters.py b/dimos/perception/detection/type/detection3d/pointcloud_filters.py index 112baaca89c..f2d95a7c85d 100644 --- a/dimos/perception/detection/type/detection3d/pointcloud_filters.py +++ b/dimos/perception/detection/type/detection3d/pointcloud_filters.py @@ -20,10 +20,10 @@ import numpy as np import open3d as o3d +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 -from dimos.msgs.geometry import inverse_transform, transform_matrix -from dimos.msgs.pointcloud import pointcloud_xyz, select_points if TYPE_CHECKING: from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox diff --git a/dimos/perception/detection/type/detection3d/test_generated_filters.py b/dimos/perception/detection/type/detection3d/test_generated_filters.py index d046edd4b14..7e35242b950 100644 --- a/dimos/perception/detection/type/detection3d/test_generated_filters.py +++ b/dimos/perception/detection/type/detection3d/test_generated_filters.py @@ -16,12 +16,12 @@ 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.msgs.image import image_from_array -from dimos.msgs.pointcloud import pointcloud_view from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection3d.pointcloud_filters import ( height_filter, diff --git a/dimos/perception/detection/type/detection3d/test_generated_projection.py b/dimos/perception/detection/type/detection3d/test_generated_projection.py index d125f4f228e..480b619ef05 100644 --- a/dimos/perception/detection/type/detection3d/test_generated_projection.py +++ b/dimos/perception/detection/type/detection3d/test_generated_projection.py @@ -19,6 +19,8 @@ from reactivex import operators as ops from reactivex.subject import Subject +from dimos.message_utils.image import image_from_array +from dimos.message_utils.pointcloud import pointcloud_from_xyz, pointcloud_view, pointcloud_xyz from dimos.messages.builtin_interfaces.msg import Time from dimos.messages.geometry_msgs.msg import ( Quaternion, @@ -28,8 +30,6 @@ ) from dimos.messages.sensor_msgs.msg import CameraInfo, PointCloud2, PointField, RegionOfInterest from dimos.messages.std_msgs.msg import Header -from dimos.msgs.image import image_from_array -from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_view, pointcloud_xyz from dimos.perception.detection.module3D import _timed_cloud from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D diff --git a/dimos/perception/detection/type/detection3d/test_imageDetections3D.py b/dimos/perception/detection/type/detection3d/test_imageDetections3D.py index 315837a056a..f4220e7cca7 100644 --- a/dimos/perception/detection/type/detection3d/test_imageDetections3D.py +++ b/dimos/perception/detection/type/detection3d/test_imageDetections3D.py @@ -18,12 +18,12 @@ import numpy as np import pytest +from dimos.message_utils.image import image_from_array, image_view +from dimos.message_utils.time import time_from_seconds, to_seconds from dimos.messages.geometry_msgs.msg import Quaternion, Vector3 from dimos.messages.sensor_msgs.msg import Image from dimos.messages.std_msgs.msg import Header from dimos.messages.vision_msgs.msg import Detection3DArray -from dimos.msgs.image import image_from_array, image_view -from dimos.msgs.time import time_from_seconds, to_seconds from dimos.perception.detection.type.detection3d.bbox import Detection3DBBox from dimos.perception.detection.type.detection3d.imageDetections3D import ImageDetections3D from dimos.perception.detection.type.detection3d.marker import Detection3DMarker diff --git a/dimos/perception/detection/type/detection3d/test_marker.py b/dimos/perception/detection/type/detection3d/test_marker.py index 787875c208c..5a688dfc9c0 100644 --- a/dimos/perception/detection/type/detection3d/test_marker.py +++ b/dimos/perception/detection/type/detection3d/test_marker.py @@ -17,11 +17,11 @@ import numpy as np import pytest +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.msgs.image import image_from_array -from dimos.msgs.time import time_from_seconds, to_seconds from dimos.perception.detection.type.detection3d.marker import Detection3DMarker from dimos.perception.fiducial.marker_pose import marker_reprojection_error diff --git a/dimos/perception/detection/type/detection3d/test_pointcloud.py b/dimos/perception/detection/type/detection3d/test_pointcloud.py index 872ef314abe..32ba767a732 100644 --- a/dimos/perception/detection/type/detection3d/test_pointcloud.py +++ b/dimos/perception/detection/type/detection3d/test_pointcloud.py @@ -16,13 +16,13 @@ import numpy as np import pytest +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.msgs.geometry import transform_from_matrix -from dimos.msgs.image import image_from_array -from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz -from dimos.msgs.time import time_from_seconds, to_seconds 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 diff --git a/dimos/perception/detection/type/imageDetections.py b/dimos/perception/detection/type/imageDetections.py index 1b52750499a..9b92c63f82d 100644 --- a/dimos/perception/detection/type/imageDetections.py +++ b/dimos/perception/detection/type/imageDetections.py @@ -22,9 +22,9 @@ else: from typing_extensions import Self +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.msgs.image import image_from_array, image_to_bgr -from dimos.msgs.time import to_seconds from dimos.perception.detection.type.utils import TableStr if TYPE_CHECKING: diff --git a/dimos/perception/detection/type/test_detection3d.py b/dimos/perception/detection/type/test_detection3d.py index 8956f3d6469..deb09dcf232 100644 --- a/dimos/perception/detection/type/test_detection3d.py +++ b/dimos/perception/detection/type/test_detection3d.py @@ -14,8 +14,8 @@ import pytest -from dimos.msgs.geometry import transform_matrix -from dimos.msgs.time import to_seconds +from dimos.message_utils.geometry import transform_matrix +from dimos.message_utils.time import to_seconds pytestmark = pytest.mark.self_hosted diff --git a/dimos/perception/experimental/moduleDB.py b/dimos/perception/experimental/moduleDB.py index af54ccea6ca..c68d83995be 100644 --- a/dimos/perception/experimental/moduleDB.py +++ b/dimos/perception/experimental/moduleDB.py @@ -21,11 +21,11 @@ from dimos.core.core import rpc from dimos.core.stream import In, Out +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.msgs.pointcloud import concatenate_clouds 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 diff --git a/dimos/perception/experimental/object.py b/dimos/perception/experimental/object.py index 6d06f01ca0a..16af65d63cb 100644 --- a/dimos/perception/experimental/object.py +++ b/dimos/perception/experimental/object.py @@ -21,6 +21,18 @@ import numpy as np +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, @@ -40,18 +52,6 @@ ObjectHypothesis, ObjectHypothesisWithPose, ) -from dimos.msgs.geometry import quaternion_from_matrix, transform_matrix -from dimos.msgs.image import image_to_rgb, image_view -from dimos.msgs.pointcloud import ( - concatenate_clouds, - pointcloud_from_xyz, - pointcloud_from_xyz_rgb, - pointcloud_rgb, - pointcloud_to_open3d, - pointcloud_xyz, - voxel_downsample_cloud, -) -from dimos.msgs.time import time_from_seconds from dimos.perception.detection.type.detection2d.seg import Detection2DSeg from dimos.perception.detection.type.detection3d.base import Detection3D diff --git a/dimos/perception/experimental/objectDB.py b/dimos/perception/experimental/objectDB.py index e8cf858350d..5df1fc432cc 100644 --- a/dimos/perception/experimental/objectDB.py +++ b/dimos/perception/experimental/objectDB.py @@ -21,8 +21,8 @@ import numpy as np -from dimos.msgs.pointcloud import pointcloud_from_xyz -from dimos.msgs.time import to_nanoseconds +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: diff --git a/dimos/perception/experimental/object_scene_registration.py b/dimos/perception/experimental/object_scene_registration.py index 4397fc79a4c..a02894856df 100644 --- a/dimos/perception/experimental/object_scene_registration.py +++ b/dimos/perception/experimental/object_scene_registration.py @@ -27,15 +27,15 @@ 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.image import image_from_array, image_view -from dimos.msgs.pointcloud import pointcloud_from_xyz, transform_cloud -from dimos.msgs.time import to_seconds from dimos.perception.detection.detectors.yoloe import Yoloe2DDetector, YoloePromptMode from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.experimental.object import ( diff --git a/dimos/perception/experimental/object_tracker.py b/dimos/perception/experimental/object_tracker.py index f14260a53c0..dceb5ebbbb7 100644 --- a/dimos/perception/experimental/object_tracker.py +++ b/dimos/perception/experimental/object_tracker.py @@ -26,6 +26,9 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out +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, @@ -53,9 +56,6 @@ Point2D, Pose2D, ) -from dimos.msgs.geometry import quaternion_from_euler -from dimos.msgs.image import image_from_array, image_to_rgb, image_view -from dimos.msgs.time import time_from_seconds, to_seconds from dimos.types.timestamped import TimestampedData, align_timestamped from dimos.utils.logging_config import setup_logger diff --git a/dimos/perception/experimental/object_tracker_2d.py b/dimos/perception/experimental/object_tracker_2d.py index a83bd0f10c4..86d1aa20fa0 100644 --- a/dimos/perception/experimental/object_tracker_2d.py +++ b/dimos/perception/experimental/object_tracker_2d.py @@ -25,6 +25,8 @@ 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_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 @@ -39,8 +41,6 @@ Point2D, Pose2D, ) -from dimos.msgs.image import image_from_array, image_to_rgb -from dimos.msgs.time import time_from_seconds from dimos.utils.logging_config import setup_logger logger = setup_logger(level=logging.INFO) diff --git a/dimos/perception/experimental/object_tracker_3d.py b/dimos/perception/experimental/object_tracker_3d.py index 9fef533934b..78ec1968eb8 100644 --- a/dimos/perception/experimental/object_tracker_3d.py +++ b/dimos/perception/experimental/object_tracker_3d.py @@ -20,6 +20,9 @@ from dimos.core.core import rpc from dimos.core.stream import In, Out +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 @@ -43,9 +46,6 @@ ObjectHypothesis, ObjectHypothesisWithPose, ) -from dimos.msgs.geometry import quaternion_from_euler -from dimos.msgs.image import image_from_array, image_to_rgb, image_view -from dimos.msgs.time import to_seconds from dimos.perception.experimental.object_tracker_2d import ObjectTracker2D from dimos.types.timestamped import TimestampedData, align_timestamped from dimos.utils.logging_config import setup_logger diff --git a/dimos/perception/experimental/perceive_loop_skill.py b/dimos/perception/experimental/perceive_loop_skill.py index 7b8a0d00262..ced12861ba1 100644 --- a/dimos/perception/experimental/perceive_loop_skill.py +++ b/dimos/perception/experimental/perceive_loop_skill.py @@ -30,11 +30,11 @@ 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.image import image_sharpness, image_to_bgr, image_to_jpeg -from dimos.msgs.time import to_seconds from dimos.types.timestamped import TimestampedBufferCollection, TimestampedData from dimos.utils.logging_config import setup_logger from dimos.utils.reactive import backpressure diff --git a/dimos/perception/experimental/spatial_perception.py b/dimos/perception/experimental/spatial_perception.py index 4953140d19b..21849b83717 100644 --- a/dimos/perception/experimental/spatial_perception.py +++ b/dimos/perception/experimental/spatial_perception.py @@ -30,11 +30,11 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In +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.msgs.geometry import pose_from_transform, quaternion_euler -from dimos.msgs.image import image_to_bgr 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 diff --git a/dimos/perception/experimental/temporal_memory/clip_filter.py b/dimos/perception/experimental/temporal_memory/clip_filter.py index 148def62653..5f6401f13f3 100644 --- a/dimos/perception/experimental/temporal_memory/clip_filter.py +++ b/dimos/perception/experimental/temporal_memory/clip_filter.py @@ -18,8 +18,8 @@ import numpy as np +from dimos.message_utils.image import image_to_rgb from dimos.messages.sensor_msgs.msg import Image -from dimos.msgs.image import image_to_rgb from dimos.utils.logging_config import setup_logger 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 dcb1a6e560a..6493853fca7 100644 --- a/dimos/perception/experimental/temporal_memory/frame_window_accumulator.py +++ b/dimos/perception/experimental/temporal_memory/frame_window_accumulator.py @@ -24,7 +24,7 @@ import threading from typing import TYPE_CHECKING -from dimos.msgs.time import to_seconds +from dimos.message_utils.time import to_seconds if TYPE_CHECKING: from dimos.messages.sensor_msgs.msg import Image diff --git a/dimos/perception/experimental/temporal_memory/temporal_memory.py b/dimos/perception/experimental/temporal_memory/temporal_memory.py index ddb08084554..55570331004 100644 --- a/dimos/perception/experimental/temporal_memory/temporal_memory.py +++ b/dimos/perception/experimental/temporal_memory/temporal_memory.py @@ -37,12 +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.image import image_sharpness -from dimos.msgs.time import header_now from dimos.perception.experimental.temporal_memory.clip_filter import ( CLIP_AVAILABLE, adaptive_keyframes, diff --git a/dimos/perception/experimental/temporal_memory/temporal_utils/helpers.py b/dimos/perception/experimental/temporal_memory/temporal_utils/helpers.py index 010845850bd..88900424d33 100644 --- a/dimos/perception/experimental/temporal_memory/temporal_utils/helpers.py +++ b/dimos/perception/experimental/temporal_memory/temporal_utils/helpers.py @@ -18,7 +18,7 @@ import numpy as np -from dimos.msgs.image import image_to_rgb +from dimos.message_utils.image import image_to_rgb if TYPE_CHECKING: from dimos.perception.experimental.temporal_memory.frame_window_accumulator import Frame 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 f673073f041..95542fa0cc2 100644 --- a/dimos/perception/experimental/temporal_memory/test_temporal_memory_module.py +++ b/dimos/perception/experimental/temporal_memory/test_temporal_memory_module.py @@ -27,13 +27,13 @@ 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.image import image_from_array -from dimos.msgs.time import time_from_seconds 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 ( diff --git a/dimos/perception/experimental/test_generated_tracker.py b/dimos/perception/experimental/test_generated_tracker.py index d3993fbedbb..fa7e37e7319 100644 --- a/dimos/perception/experimental/test_generated_tracker.py +++ b/dimos/perception/experimental/test_generated_tracker.py @@ -20,6 +20,7 @@ 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 ( @@ -29,7 +30,6 @@ Point2D, Pose2D, ) -from dimos.msgs.image import image_view from dimos.perception.experimental.object_tracker_2d import ObjectTracker2D from dimos.perception.experimental.object_tracker_3d import ObjectTracker3D diff --git a/dimos/perception/experimental/test_object_generated.py b/dimos/perception/experimental/test_object_generated.py index 53cecb53da9..faa2287abd9 100644 --- a/dimos/perception/experimental/test_object_generated.py +++ b/dimos/perception/experimental/test_object_generated.py @@ -17,6 +17,10 @@ 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, @@ -30,10 +34,6 @@ 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.msgs.camera_info import camera_info_from_intrinsics -from dimos.msgs.image import image_from_array -from dimos.msgs.pointcloud import pointcloud_from_xyz_rgb, pointcloud_rgb, pointcloud_xyz -from dimos.msgs.time import time_from_seconds 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 diff --git a/dimos/perception/experimental/test_object_scene_registration_temporal.py b/dimos/perception/experimental/test_object_scene_registration_temporal.py index 540fb4585d2..07719256379 100644 --- a/dimos/perception/experimental/test_object_scene_registration_temporal.py +++ b/dimos/perception/experimental/test_object_scene_registration_temporal.py @@ -23,16 +23,16 @@ import pytest 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.msgs.camera_info import camera_info_from_intrinsics -from dimos.msgs.image import image_from_array -from dimos.msgs.pointcloud import pointcloud_xyz -from dimos.msgs.time import time_from_seconds 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 diff --git a/dimos/perception/experimental/test_perceive_loop_skill.py b/dimos/perception/experimental/test_perceive_loop_skill.py index 20e1e719c40..be0a8553705 100644 --- a/dimos/perception/experimental/test_perceive_loop_skill.py +++ b/dimos/perception/experimental/test_perceive_loop_skill.py @@ -22,9 +22,9 @@ 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.msgs.image import image_from_array -from dimos.msgs.time import time_from_seconds from dimos.perception.experimental import perceive_loop_skill as skill_module from dimos.perception.experimental.perceive_loop_skill import PerceiveLoopSkill diff --git a/dimos/perception/experimental/test_spatial_memory.py b/dimos/perception/experimental/test_spatial_memory.py index 876b8aab87c..94aa3bab684 100644 --- a/dimos/perception/experimental/test_spatial_memory.py +++ b/dimos/perception/experimental/test_spatial_memory.py @@ -24,11 +24,11 @@ from reactivex.scheduler import ThreadPoolScheduler 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.msgs.image import image_from_array from dimos.perception.experimental import spatial_perception from dimos.perception.experimental.spatial_perception import SpatialMemory from dimos.perception.experimental.visual_memory import VisualMemory diff --git a/dimos/perception/fiducial/blueprints/desk_marker_tf.py b/dimos/perception/fiducial/blueprints/desk_marker_tf.py index c2939c2263e..68277c7dd31 100644 --- a/dimos/perception/fiducial/blueprints/desk_marker_tf.py +++ b/dimos/perception/fiducial/blueprints/desk_marker_tf.py @@ -27,15 +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.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.msgs.camera_info import camera_info_from_yaml -from dimos.msgs.geometry import quaternion_from_euler -from dimos.msgs.time import time_from_nanoseconds from dimos.perception.fiducial.marker_detection_stream_module import MarkerDetectionStreamModule from dimos.perception.fiducial.marker_tf_module import MarkerTfModule diff --git a/dimos/perception/fiducial/marker_detect.py b/dimos/perception/fiducial/marker_detect.py index 67f76179906..b89d4c65145 100644 --- a/dimos/perception/fiducial/marker_detect.py +++ b/dimos/perception/fiducial/marker_detect.py @@ -22,12 +22,12 @@ import cv2.aruco import numpy as np +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.msgs.geometry import compose_transforms -from dimos.msgs.image import image_to_bgr -from dimos.msgs.time import to_seconds from dimos.perception.detection.type.detection3d.marker import Detection3DMarker from dimos.perception.fiducial.marker_pose import ( camera_info_to_cv_matrices, diff --git a/dimos/perception/fiducial/marker_detection_stream_module.py b/dimos/perception/fiducial/marker_detection_stream_module.py index 6a5470aa80e..5623e91c724 100644 --- a/dimos/perception/fiducial/marker_detection_stream_module.py +++ b/dimos/perception/fiducial/marker_detection_stream_module.py @@ -33,11 +33,11 @@ from dimos.memory.store.null import NullStore from dimos.memory.stream import Stream from dimos.memory.transform import QualityWindow, SpeedLimit +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.msgs.image import image_sharpness -from dimos.msgs.time import to_seconds 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 diff --git a/dimos/perception/fiducial/marker_pose.py b/dimos/perception/fiducial/marker_pose.py index e0d8a78a571..049292dd181 100644 --- a/dimos/perception/fiducial/marker_pose.py +++ b/dimos/perception/fiducial/marker_pose.py @@ -22,10 +22,10 @@ import cv2.aruco import numpy as np +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 -from dimos.msgs.geometry import quaternion_from_matrix _FISHEYE_MODELS = frozenset({"equidistant", "fisheye", "kannala_brandt"}) diff --git a/dimos/perception/fiducial/marker_transformer.py b/dimos/perception/fiducial/marker_transformer.py index a5f977a4f73..1a2ba8cc520 100644 --- a/dimos/perception/fiducial/marker_transformer.py +++ b/dimos/perception/fiducial/marker_transformer.py @@ -37,11 +37,11 @@ import numpy as np from dimos.memory.transform import Transformer +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.msgs.time import to_nanoseconds 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 ( diff --git a/dimos/perception/fiducial/test_fixture_verification.py b/dimos/perception/fiducial/test_fixture_verification.py index 6e94bbc0ac7..34aa75236f5 100644 --- a/dimos/perception/fiducial/test_fixture_verification.py +++ b/dimos/perception/fiducial/test_fixture_verification.py @@ -18,12 +18,12 @@ import numpy as np import pytest +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.msgs.camera_info import camera_info_from_intrinsics -from dimos.msgs.geometry import transform_matrix -from dimos.msgs.image import image_from_array -from dimos.msgs.time import time_from_seconds from dimos.perception.detection.type.detection3d.imageDetections3D import ImageDetections3D from dimos.perception.fiducial.fixture_verification import ( BoardLayout, diff --git a/dimos/perception/fiducial/test_generated_marker_detect.py b/dimos/perception/fiducial/test_generated_marker_detect.py index 08cb2c9f8e7..45d398f458e 100644 --- a/dimos/perception/fiducial/test_generated_marker_detect.py +++ b/dimos/perception/fiducial/test_generated_marker_detect.py @@ -19,12 +19,12 @@ 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.msgs.image import image_from_array from dimos.perception.fiducial.marker_detect import detect_markers_in_image diff --git a/dimos/perception/fiducial/test_helpers.py b/dimos/perception/fiducial/test_helpers.py index 933519d61a9..c0d00749d11 100644 --- a/dimos/perception/fiducial/test_helpers.py +++ b/dimos/perception/fiducial/test_helpers.py @@ -19,12 +19,12 @@ import cv2 import numpy as np +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 -from dimos.msgs.image import image_from_array -from dimos.msgs.time import time_from_seconds def camera_info(ts: float = 10.0) -> CameraInfo: diff --git a/dimos/perception/fiducial/test_marker_detection_stream_module.py b/dimos/perception/fiducial/test_marker_detection_stream_module.py index e0950fa3459..53038019987 100644 --- a/dimos/perception/fiducial/test_marker_detection_stream_module.py +++ b/dimos/perception/fiducial/test_marker_detection_stream_module.py @@ -24,13 +24,13 @@ from dimos.memory.store.memory import MemoryStore from dimos.memory.transform import SpeedLimit from dimos.memory.type.observation import Observation +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.msgs.time import time_from_seconds, to_seconds 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 diff --git a/dimos/perception/fiducial/test_marker_pose.py b/dimos/perception/fiducial/test_marker_pose.py index 74c7be0fec0..215825c9a6e 100644 --- a/dimos/perception/fiducial/test_marker_pose.py +++ b/dimos/perception/fiducial/test_marker_pose.py @@ -15,10 +15,10 @@ import cv2 import numpy as np +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.msgs.image import image_from_array from dimos.perception.fiducial.marker_pose import ( camera_optical_frame_id, estimate_marker_pose, diff --git a/dimos/perception/fiducial/test_marker_tf_integration.py b/dimos/perception/fiducial/test_marker_tf_integration.py index d57425b34b8..3d4f2aaef37 100644 --- a/dimos/perception/fiducial/test_marker_tf_integration.py +++ b/dimos/perception/fiducial/test_marker_tf_integration.py @@ -23,6 +23,7 @@ import pytest from dimos.core.transport import LCMTransport +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 @@ -34,7 +35,6 @@ ObjectHypothesis, ObjectHypothesisWithPose, ) -from dimos.msgs.time import time_from_seconds from dimos.perception.fiducial.marker_tf_module import MarkerTfModule from dimos.protocol.tf.tf import TF diff --git a/dimos/perception/fiducial/test_marker_tf_module.py b/dimos/perception/fiducial/test_marker_tf_module.py index b39701a570c..f2178dc7bfc 100644 --- a/dimos/perception/fiducial/test_marker_tf_module.py +++ b/dimos/perception/fiducial/test_marker_tf_module.py @@ -18,6 +18,8 @@ import numpy as np import pytest +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 @@ -29,8 +31,6 @@ ObjectHypothesis, ObjectHypothesisWithPose, ) -from dimos.msgs.geometry import quaternion_from_euler -from dimos.msgs.time import time_from_seconds from dimos.perception.fiducial.marker_tf_module import MarkerTfModule from dimos.protocol.tf.tf import TF diff --git a/dimos/perception/fiducial/test_marker_transformer.py b/dimos/perception/fiducial/test_marker_transformer.py index 1f1c67e6e5a..cab69252933 100644 --- a/dimos/perception/fiducial/test_marker_transformer.py +++ b/dimos/perception/fiducial/test_marker_transformer.py @@ -16,10 +16,10 @@ import pytest from dimos.memory.type.observation import Observation +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.msgs.image import image_from_array -from dimos.msgs.time import time_from_seconds, to_seconds 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 ( diff --git a/dimos/perception/localize/association.py b/dimos/perception/localize/association.py index b06c061cd55..935bcb81e66 100644 --- a/dimos/perception/localize/association.py +++ b/dimos/perception/localize/association.py @@ -22,11 +22,11 @@ import numpy as np +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 -from dimos.msgs.geometry import inverse_transform -from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz -from dimos.msgs.time import time_from_nanoseconds, to_nanoseconds if TYPE_CHECKING: from dimos.perception.detection.type.detection3d.pointcloud import Detection3DPC diff --git a/dimos/perception/localize/localize.py b/dimos/perception/localize/localize.py index 488b13b50f0..c8d8a028939 100644 --- a/dimos/perception/localize/localize.py +++ b/dimos/perception/localize/localize.py @@ -33,10 +33,10 @@ from dimos.memory.embed import EmbedImages from dimos.memory.transform import QualityWindow, peaks from dimos.memory.type.observation import Observation -from dimos.msgs.geometry import inverse_transform, vector_array -from dimos.msgs.image import image_brightness, image_sharpness -from dimos.msgs.pointcloud import pointcloud_xyz -from dimos.msgs.time import to_seconds +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 ( diff --git a/dimos/perception/localize/module.py b/dimos/perception/localize/module.py index 4b80c11b257..5903d67ca46 100644 --- a/dimos/perception/localize/module.py +++ b/dimos/perception/localize/module.py @@ -50,6 +50,8 @@ from dimos.memory.blobstore.memory import MemoryBlobStore from dimos.memory.observationstore.memory import ListObservationStore from dimos.memory.store.memory import MemoryStore +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 @@ -61,8 +63,6 @@ ObjectHypothesis, ObjectHypothesisWithPose, ) -from dimos.msgs.pointcloud import pointcloud_xyz -from dimos.msgs.time import time_from_seconds, to_seconds from dimos.perception.localize.association import Groups from dimos.perception.localize.dandetect import DanDetector from dimos.perception.localize.rig import ( diff --git a/dimos/perception/localize/rig.py b/dimos/perception/localize/rig.py index aa8ba58c075..2074e8b438a 100644 --- a/dimos/perception/localize/rig.py +++ b/dimos/perception/localize/rig.py @@ -23,18 +23,18 @@ import numpy as np from dimos.memory.tf import StreamTF -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.msgs.geometry import inverse_transform, pose_from_transform, vector_array -from dimos.msgs.image import image_view -from dimos.msgs.pointcloud import ( +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.msgs.time import time_from_nanoseconds, to_nanoseconds +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, diff --git a/dimos/perception/localize/support_plane.py b/dimos/perception/localize/support_plane.py index 432614f0d0f..a747768001d 100644 --- a/dimos/perception/localize/support_plane.py +++ b/dimos/perception/localize/support_plane.py @@ -21,7 +21,7 @@ import numpy as np -from dimos.msgs.pointcloud import pointcloud_xyz, voxel_downsample_cloud +from dimos.message_utils.pointcloud import pointcloud_xyz, voxel_downsample_cloud from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: diff --git a/dimos/perception/localize/test_dandetect.py b/dimos/perception/localize/test_dandetect.py index e7774ff4ef2..d1df5b05b10 100644 --- a/dimos/perception/localize/test_dandetect.py +++ b/dimos/perception/localize/test_dandetect.py @@ -22,13 +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.camera_info import camera_info_from_intrinsics -from dimos.msgs.image import image_from_array -from dimos.msgs.time import time_from_seconds, to_seconds from dimos.perception.localize import dandetect from dimos.perception.localize.dandetect import DanDetector, NoSegmenter, Option from dimos.perception.localize.localize import embed_index diff --git a/dimos/perception/localize/test_rig.py b/dimos/perception/localize/test_rig.py index b8dd53b4449..6dd21140078 100644 --- a/dimos/perception/localize/test_rig.py +++ b/dimos/perception/localize/test_rig.py @@ -24,15 +24,15 @@ 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.msgs.camera_info import camera_info_from_intrinsics -from dimos.msgs.image import image_from_array -from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz -from dimos.msgs.time import time_from_seconds, to_seconds 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 @@ -268,11 +268,11 @@ def test_offline_renderer_uses_generated_images_and_local_camera_pinhole(tmp_pat from dimos.messages.std_msgs.msg import Header from dimos.memory.store.memory import MemoryStore from dimos.memory.type.observation import Observation -from dimos.msgs.camera_info import camera_info_from_intrinsics -from dimos.msgs.geometry import inverse_transform -from dimos.msgs.image import image_from_array -from dimos.msgs.pointcloud import pointcloud_from_xyz -from dimos.msgs.time import time_from_seconds +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 diff --git a/dimos/perception/localize/tools/tool_localize.py b/dimos/perception/localize/tools/tool_localize.py index 5f23a7e6df6..cf9ed17ccf7 100644 --- a/dimos/perception/localize/tools/tool_localize.py +++ b/dimos/perception/localize/tools/tool_localize.py @@ -47,19 +47,23 @@ from dimos.memory.store.sqlite import SqliteStore from dimos.memory.transform import throttle -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.msgs.geometry import ( +from dimos.message_utils.geometry import ( inverse_transform, pose_from_transform, pose_matrix, transform_matrix, vector_array, ) -from dimos.msgs.image import image_from_array, image_to_bgr, image_to_rgb -from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz, voxel_downsample_cloud -from dimos.msgs.time import time_from_seconds +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 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 17137fcabee..52f6da8891e 100644 --- a/dimos/protocol/pubsub/impl/webrtc/providers/test_video_track.py +++ b/dimos/protocol/pubsub/impl/webrtc/providers/test_video_track.py @@ -25,10 +25,10 @@ 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.image import image_from_array from dimos.msgs.sensor_msgs.Image import Image, ImageFormat from dimos.teleop.hosted.camera_mux import CameraMuxModule diff --git a/dimos/protocol/pubsub/impl/webrtc/providers/video_track.py b/dimos/protocol/pubsub/impl/webrtc/providers/video_track.py index 249f6d8f3f9..4cc37eb74cf 100644 --- a/dimos/protocol/pubsub/impl/webrtc/providers/video_track.py +++ b/dimos/protocol/pubsub/impl/webrtc/providers/video_track.py @@ -29,8 +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.image import image_to_rgb, image_view from dimos.msgs.sensor_msgs.Image import Image, ImageFormat from dimos.utils.logging_config import setup_logger diff --git a/dimos/protocol/tf/static_tf_publisher.py b/dimos/protocol/tf/static_tf_publisher.py index 8985dd5283a..8f42a27e15c 100644 --- a/dimos/protocol/tf/static_tf_publisher.py +++ b/dimos/protocol/tf/static_tf_publisher.py @@ -32,12 +32,12 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import Out +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.msgs.geometry import quaternion_from_euler -from dimos.msgs.time import time_from_nanoseconds from dimos.utils.logging_config import setup_logger logger = setup_logger() diff --git a/dimos/protocol/tf/test_static_tf_publisher.py b/dimos/protocol/tf/test_static_tf_publisher.py index 28e8fab6ae1..f27dc4055c1 100644 --- a/dimos/protocol/tf/test_static_tf_publisher.py +++ b/dimos/protocol/tf/test_static_tf_publisher.py @@ -19,8 +19,8 @@ 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.msgs.geometry import compose_transforms from dimos.protocol.tf.static_tf_publisher import frames_to_edge_transforms diff --git a/dimos/protocol/tf/test_tf.py b/dimos/protocol/tf/test_tf.py index aac5cd5cadb..c4af9048ac2 100644 --- a/dimos/protocol/tf/test_tf.py +++ b/dimos/protocol/tf/test_tf.py @@ -25,12 +25,12 @@ from dimos.memory.store.memory import MemoryStore from dimos.memory.store.sqlite import SqliteStore from dimos.memory.tf import StreamTF +from dimos.message_utils.geometry import quaternion_from_euler +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.std_msgs.msg import Header from dimos.messages.tf2_msgs.msg import TFMessage -from dimos.msgs.geometry import quaternion_from_euler -from dimos.msgs.time import time_from_seconds, to_seconds from dimos.protocol.tf.tf import TF, MultiTBuffer, TBuffer, TFLookup diff --git a/dimos/protocol/tf/tf.py b/dimos/protocol/tf/tf.py index ae1bed1d58c..143a92486d7 100644 --- a/dimos/protocol/tf/tf.py +++ b/dimos/protocol/tf/tf.py @@ -25,6 +25,8 @@ from sortedcontainers import SortedDict # type: ignore[import-untyped] +from dimos.message_utils.geometry import compose_transforms, inverse_transform, pose_from_transform +from dimos.message_utils.time import time_from_seconds, to_nanoseconds, to_seconds from dimos.messages.geometry_msgs.msg import ( PoseStamped, Quaternion, @@ -34,8 +36,6 @@ ) from dimos.messages.std_msgs.msg import Header from dimos.messages.tf2_msgs.msg import TFMessage -from dimos.msgs.geometry import compose_transforms, inverse_transform, pose_from_transform -from dimos.msgs.time import time_from_seconds, to_nanoseconds, to_seconds from dimos.types.timestamped import to_human_readable from dimos.utils.logging_config import setup_logger diff --git a/dimos/teleop/hosted/camera_mux.py b/dimos/teleop/hosted/camera_mux.py index d5615fbdc5c..aa5fbabb82f 100644 --- a/dimos/teleop/hosted/camera_mux.py +++ b/dimos/teleop/hosted/camera_mux.py @@ -36,9 +36,9 @@ 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_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.msgs.image import image_from_array, image_to_bgr, image_to_rgb, image_view -from dimos.msgs.time import header_now from dimos.utils.logging_config import setup_logger logger = setup_logger() diff --git a/dimos/teleop/hosted/test_camera_mux.py b/dimos/teleop/hosted/test_camera_mux.py index 5e33aaacb09..e0a65c9428d 100644 --- a/dimos/teleop/hosted/test_camera_mux.py +++ b/dimos/teleop/hosted/test_camera_mux.py @@ -26,10 +26,10 @@ import numpy as np import pytest +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.msgs.image import image_from_array, image_to_rgb, image_view from dimos.teleop.hosted.blueprints.cloudflare import ( teleop_hosted_go2_multicam, teleop_hosted_go2_transport, diff --git a/docs/capabilities/perception/index.md b/docs/capabilities/perception/index.md index c66f370f7a7..90718e61922 100644 --- a/docs/capabilities/perception/index.md +++ b/docs/capabilities/perception/index.md @@ -11,8 +11,8 @@ 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.msgs.image import image_from_file -from dimos.msgs.time import header_now +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 @@ -37,7 +37,7 @@ complete, select the explicit CDR transport when wiring these ports; see 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.msgs.image`, `dimos.msgs.pointcloud` and `dimos.msgs.geometry`. +`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, @@ -57,8 +57,8 @@ With the existing ML dependencies, the same generated image can be embedded: ```python from dimos.models.embedding.clip import CLIPModel -from dimos.msgs.image import image_from_file -from dimos.msgs.time import header_now +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")) @@ -88,8 +88,8 @@ 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.msgs.camera_info import camera_info_from_intrinsics -from dimos.msgs.image import image_from_array +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) diff --git a/docs/usage/sensor_streams/index.md b/docs/usage/sensor_streams/index.md index baa5dfce157..4dbc219fdc0 100644 --- a/docs/usage/sensor_streams/index.md +++ b/docs/usage/sensor_streams/index.md @@ -57,7 +57,7 @@ and recording: ```python from dimos.messages.sensor_msgs.msg import Image -from dimos.msgs.image import image_view +from dimos.message_utils.image import image_view def mean_red(image: Image) -> float: diff --git a/docs/usage/transforms.md b/docs/usage/transforms.md index 5c82203b42e..f1c825415f4 100644 --- a/docs/usage/transforms.md +++ b/docs/usage/transforms.md @@ -10,7 +10,7 @@ chain then places those points in the robot or world frame. 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.msgs.geometry`. +Geometry operations live in `dimos.message_utils.geometry`. ## Define a mount @@ -39,7 +39,7 @@ receive them. Compose connected edges in parent-to-child order. Inversion swaps the frames. ```python -from dimos.msgs.geometry import compose_transforms, inverse_transform +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([ From a56ed51da1ab35f4902e6a4ec015ba5e2948e33e Mon Sep 17 00:00:00 2001 From: "autofix-ci[bot]" <114827586+autofix-ci[bot]@users.noreply.github.com> Date: Fri, 23 Oct 2026 09:20:01 +0000 Subject: [PATCH 6/6] [autofix.ci] apply automated fixes --- dimos/navigation/trajectory_follower/fancy/rust/flake.lock | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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": {