diff --git a/.github/scripts/test_relock_shared_flakes.py b/.github/scripts/test_relock_shared_flakes.py new file mode 100644 index 00000000000..4bd1b8b8250 --- /dev/null +++ b/.github/scripts/test_relock_shared_flakes.py @@ -0,0 +1,70 @@ +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +"""Exact SDK API pins must survive automatic shared-source refreshes.""" + +from importlib.machinery import SourceFileLoader +from importlib.util import module_from_spec, spec_from_loader +from pathlib import Path +from tempfile import TemporaryDirectory +import unittest +from unittest.mock import patch + +ROOT = Path(__file__).resolve().parents[2] +loader = SourceFileLoader("relock_shared_flakes", str(ROOT / "bin/relock-shared-flakes")) +spec = spec_from_loader(loader.name, loader) +relock = module_from_spec(spec) +loader.exec_module(relock) + + +class ExactSdkVersionTests(unittest.TestCase): + def stale(self, requirement: str, revision_matches: bool = True) -> list[str]: + revision = "a" * 40 + manifest_revision = revision if revision_matches else "b" * 40 + with TemporaryDirectory() as directory: + manifest = Path(directory) / "Cargo.toml" + manifest.write_text( + '[dependencies]\ndimos-module = { git = "' + + relock.GIT_URL + + '", rev = "' + + manifest_revision + + '"' + + requirement + + " }\n" + ) + with ( + patch.object( + relock, + "_pinned_crates", + return_value=[ + ("dimos-module", "0.1.0", revision), + ("dimos-module-macros", "0.1.0", revision), + ], + ), + patch.object( + relock, + "_crate_dirs", + return_value={ + "dimos-module": "native/rust/dimos-module", + "dimos-module-macros": "native/rust/dimos-module-macros", + }, + ), + patch.object(relock, "_git", return_value='[package]\nversion = "0.2.0"'), + patch.object(relock, "tree_at", side_effect=lambda rev, _: rev), + ): + return relock._stale_pin(manifest, manifest.with_name("Cargo.lock")) + + def test_exact_legacy_api_keeps_its_sdk_and_transitive_macro_revision(self): + self.assertEqual(self.stale(', version = "=0.1.0"'), []) + + def test_unconstrained_dependencies_still_refresh_changed_sources(self): + self.assertEqual(len(self.stale("")), 2) + + def test_exact_constraint_does_not_hide_a_mismatched_lock_revision(self): + failures = self.stale(', version = "=0.1.0"', revision_matches=False) + self.assertEqual(len(failures), 2) + self.assertTrue(all("Cargo.lock pins" in error for error in failures)) + + +if __name__ == "__main__": + unittest.main() diff --git a/.github/workflows/autofix.yml b/.github/workflows/autofix.yml index d3c5c7f3660..4370527300b 100644 --- a/.github/workflows/autofix.yml +++ b/.github/workflows/autofix.yml @@ -47,7 +47,9 @@ jobs: run: bash docker/ros/install-nix.sh - name: Relock the modules onto this revision if: steps.shared.outputs.changed == 'true' - run: python3 bin/relock-shared-flakes + run: | + python3 .github/scripts/test_relock_shared_flakes.py + python3 bin/relock-shared-flakes - name: autofix.ci uses: autofix-ci/action@v1 diff --git a/.github/workflows/cdr-native.yml b/.github/workflows/cdr-native.yml new file mode 100644 index 00000000000..21b5b1eed64 --- /dev/null +++ b/.github/workflows/cdr-native.yml @@ -0,0 +1,47 @@ +name: CDR native interoperability + +on: + pull_request: + workflow_dispatch: + push: + branches: [main] + +permissions: + contents: read + +jobs: + native: + runs-on: ubuntu-latest + timeout-minutes: 30 + env: + DIMOS_ALLOW_MISSING_COCKPIT: "1" + CARGO_TARGET_DIR: ${{ github.workspace }}/target + steps: + - uses: actions/checkout@v7 + with: + lfs: false + persist-credentials: false + - uses: astral-sh/setup-uv@v10.2.0 + with: + python-version: '3.12' + enable-cache: true + - name: Install build and media dependencies + run: | + sudo apt-get update + sudo apt-get install -y cmake pkg-config clang libclang-dev portaudio19-dev libturbojpeg ffmpeg + - name: Install Python project and test dependencies + run: uv sync --frozen --group tests + - name: Test generated value adapters + run: | + uv run --no-sync pytest --noconftest --import-mode=importlib -o addopts='' -q \ + dimos/message_utils/test_image_views.py \ + dimos/message_utils/test_image_compression.py \ + dimos/message_utils/test_camera_info.py \ + dimos/message_utils/test_time.py \ + dimos/message_utils/test_geometry.py \ + dimos/message_utils/test_pointcloud.py \ + dimos/message_utils/test_pointcloud_colors.py + - name: Test native SDK + run: | + cargo test --locked --manifest-path native/rust/dimos-module/Cargo.toml + cargo test --locked --manifest-path native/rust/dimos-module-macros/Cargo.toml diff --git a/.github/workflows/message-codegen.yml b/.github/workflows/message-codegen.yml deleted file mode 100644 index c86d4ea9ce1..00000000000 --- a/.github/workflows/message-codegen.yml +++ /dev/null @@ -1,41 +0,0 @@ -name: message-codegen -# CI integration check for the CDR message codegen layer. It runs the one test -# file under dimos/message_codegen/ against the published dimos-cdrgen library, -# on Linux and macOS, in a clean virtualenv without the dimOS dev environment. - -on: - pull_request: - paths: - - dimos/message_codegen/** - - native/cpp/include/dimos/native/cdr_codec.hpp - - docs/development/message-source-packages.md - - pyproject.toml - - uv.lock - - .github/workflows/message-codegen.yml - workflow_dispatch: - -permissions: - contents: read - -jobs: - integration: - strategy: - fail-fast: false - matrix: - os: [ubuntu-latest, macos-14] - runs-on: ${{ matrix.os }} - steps: - - uses: actions/checkout@v7 - - uses: astral-sh/setup-uv@v10.0.1 - - name: Install published generator and consumer test tools - run: | - uv venv .venv --python 3.12 - uv pip install --python .venv/bin/python 'dimos-cdrgen==0.1.0' numpy build pytest pytest-asyncio pytest-env - - name: Verify SDK discovery and installed Python consumer without pip - run: | - .venv/bin/python -c 'import importlib.util; assert importlib.util.find_spec("pip") is None' - .venv/bin/python -m pytest --noconftest -o addopts='' -o 'markers=native_e2e: compiled native consumer acceptance' --strict-markers -m 'not native_e2e' -q dimos/message_codegen/test_integration.py - - name: Verify compiled C++ consumer - run: | - .venv/bin/python -m pytest --noconftest -o addopts='' -o 'markers=native_e2e: compiled native consumer acceptance' --strict-markers -m native_e2e -q dimos/message_codegen/test_integration.py - .venv/bin/python -c 'import importlib.util; assert importlib.util.find_spec("pip") is None; assert importlib.util.find_spec("catkin_pkg") is None' diff --git a/.gitignore b/.gitignore index 181e3ae4bbc..2644e75f80b 100644 --- a/.gitignore +++ b/.gitignore @@ -106,3 +106,6 @@ optuna.db # policy blobs are loaded from outside the repository *.bin + +# Prepared from schema-sources.json; keep downloaded schemas out of Git. +/packages/dimos-generated/interfaces/ diff --git a/MANIFEST.in b/MANIFEST.in index 161b5666977..7d68ef988e6 100644 --- a/MANIFEST.in +++ b/MANIFEST.in @@ -47,3 +47,8 @@ prune dimos/web/command-center-extension global-exclude test_*.py global-exclude conftest.py + +# Built-in message inputs are prepared automatically during source builds; the sdist is self-contained. +include packages/dimos-generated/schema-sources.json +recursive-include packages/dimos-generated/custom *.msg LICENSE +recursive-include packages/dimos-generated/interfaces *.msg *LICENSE* diff --git a/bin/relock-shared-flakes b/bin/relock-shared-flakes index aa91d582147..aeb630dc04c 100755 --- a/bin/relock-shared-flakes +++ b/bin/relock-shared-flakes @@ -22,6 +22,8 @@ rewrites a pull request's merge ref, so a pin on it would rot. What counts as stale is the pinned *content*, not the revision. A pin may name an older commit quite legitimately -- what matters is that the shared tree at that commit is the tree in this one. +An explicit exact Cargo version keeps its pinned revision, including transitive +SDK crates. Updating that API is a caller migration rather than an automatic relock. Stdlib only, so it runs on a bare `python3` like the other bin/ scripts. """ @@ -296,10 +298,19 @@ def _hashes_file(manifest: Path) -> Path: def _stale_pin(manifest: Path, lock: Path) -> list[str]: """Why the pin on this repository's crates is not this commit's; empty when it is.""" revs = {m.group("rev") for m in _GIT_DEP.finditer(manifest.read_text())} + dependencies = tomllib.loads(manifest.read_text()).get("dependencies", {}) + packages = _pinned_crates(lock) + fixed_revs = set() + for name, version, rev in packages: + dependency = dependencies.get(name, {}) + if isinstance(dependency, dict) and dependency.get("version") == f"={version}": + fixed_revs.add(rev) stale = [] - for name, _, rev in _pinned_crates(lock): + for name, _, rev in packages: if rev not in revs: stale.append(f"{name}: Cargo.lock pins {rev[:10]}, Cargo.toml another revision") + if rev in fixed_revs: + continue subdir = _crate_dirs().get(name) pinned = tree_at(rev, subdir) if subdir else None if subdir is None or (pinned is not None and pinned != tree_at("HEAD", subdir)): @@ -345,7 +356,9 @@ def relock_cargo(manifest: Path) -> tuple[Path, bool, list[str]]: return manifest, False, [_unpushed(manifest, " and ".join(subdirs))] manifest.write_text( _GIT_DEP.sub( - lambda m: f'{m.group("name")} = {{ git = "{GIT_URL}", rev = "{rev}"{m.group("rest")}}}', + lambda m: ( + f'{m.group("name")} = {{ git = "{GIT_URL}", rev = "{rev}"{m.group("rest")}}}' + ), before[manifest], ) ) diff --git a/dimos/core/coordination/test_blueprints.py b/dimos/core/coordination/test_blueprints.py index 043e3bf0a4f..72e544c1e79 100644 --- a/dimos/core/coordination/test_blueprints.py +++ b/dimos/core/coordination/test_blueprints.py @@ -38,7 +38,7 @@ from dimos.core.core import rpc from dimos.core.module import Module from dimos.core.stream import In, Out -from dimos.core.transport import LCMTransport +from dimos.core.transport import pLCMTransport from dimos.spec.utils import Spec @@ -139,7 +139,7 @@ def test_config_rejects_unknown_fields_and_sections() -> None: def test_transports() -> None: - custom_transport = LCMTransport("/custom_topic", Data1) + custom_transport = pLCMTransport("/custom_topic") blueprint_set = autoconnect(ModuleA.blueprint(), ModuleB.blueprint()).transports( {("data1", Data1): custom_transport} ) @@ -414,12 +414,12 @@ def test_explicit_instance_name_sets_blueprint_identity() -> None: def test_namespace_prefixes_pinned_transports() -> None: blueprint = ( autoconnect(ModuleA.blueprint(), ModuleB.blueprint()) - .transports({("data1", Data1): LCMTransport("/custom_topic", Data1)}) + .transports({("data1", Data1): pLCMTransport("/custom_topic")}) .namespace("robot0") ) transport = blueprint.transport_map[("robot0/data1", Data1)] - assert transport.topic.pattern == "/robot0/custom_topic" + assert transport.topic == "/robot0/custom_topic" assert ("data1", Data1) not in blueprint.transport_map diff --git a/dimos/core/global_config.py b/dimos/core/global_config.py index 7b657260a16..355d0016dfe 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -47,6 +47,7 @@ class GlobalConfig(BaseSettings): replay_db: str = "go2_short" record: Literal["", "sqlite", "mcap"] = "" record_engine: Literal["python", "rust"] = Field(default="python", validate_default=True) + record_executable: str | None = None # Optional prebuilt Rust recorder path. record_topics: str = "*" # comma-separated globs on the topic slug (/a/b -> a_b) record_encoding_threads: int | None = Field(default=None, ge=1) # How every zenoh session this process opens joins the network. diff --git a/dimos/core/test_dds_transport.py b/dimos/core/test_dds_transport.py new file mode 100644 index 00000000000..14a5f2b6bd3 --- /dev/null +++ b/dimos/core/test_dds_transport.py @@ -0,0 +1,37 @@ +# 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 dataclasses import dataclass + +import pytest + +from dimos.core.transport import DDSTransport + +idl = pytest.importorskip("cyclonedds.idl") + + +@dataclass +class NativeSample(idl.IdlStruct): + value: int + + +def test_dds_transport_accepts_native_idl_type(): + transport = DDSTransport("native_sample", NativeSample) + try: + assert transport.topic.name == "native_sample" + assert transport.topic.data_type is NativeSample + sample = NativeSample(value=42) + assert transport.topic.data_type.deserialize(sample.serialize()) == sample + finally: + transport.stop() diff --git a/dimos/core/test_transport_factory.py b/dimos/core/test_transport_factory.py index 42504371c96..822f2282c79 100644 --- a/dimos/core/test_transport_factory.py +++ b/dimos/core/test_transport_factory.py @@ -18,7 +18,9 @@ from dimos.core.global_config import GlobalConfig from dimos.core.transport import ( + DDSTransport, LCMTransport, + ROSTransport, ZenohTransport, pLCMTransport, pZenohTransport, @@ -31,6 +33,8 @@ session_config, transport_topic, ) +from dimos.messages.sensor_msgs.msg import Image as CdrImage +from dimos.messages.std_msgs.msg import Int32 from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.sensor_msgs.Image import Image @@ -82,8 +86,9 @@ def test_make_transport_zenoh_pickled() -> None: assert t.topic == "dimos/human_input" -def test_default_zenoh_qos_high_rate_sensor_types_drop() -> None: - assert default_zenoh_qos("/camera/color", Image) == QOS_LATEST_WINS +@pytest.mark.parametrize("message_type", [Image, CdrImage]) +def test_default_zenoh_qos_high_rate_sensor_types_drop(message_type) -> None: + assert default_zenoh_qos("/camera/color", message_type) == QOS_LATEST_WINS def test_default_zenoh_qos_agent_channels_never_drop() -> None: @@ -155,3 +160,33 @@ def test_apply_transport_arg_bad_value_exits(argv: list[str]) -> None: apply_transport_arg(argv, g=g) assert exc.value.code == 2 assert g.transport == "lcm" # config left untouched + + +@pytest.mark.parametrize("config", [LCM, ZENOH]) +def test_factory_binds_generated_messages(config): + transport = make_transport("generated", Int32, g=config) + assert transport.topic.message.encoding == "cdr" + assert transport.topic.message.message_type is Int32 + + +@pytest.mark.parametrize("config", [LCM, ZENOH]) +@pytest.mark.parametrize("message_type", [dict, int, str, bool]) +def test_factory_preserves_pickle_for_python_types(config, message_type): + transport = make_transport("python", message_type, g=config) + expected = pLCMTransport if config.transport == "lcm" else pZenohTransport + assert isinstance(transport, expected) + + +@pytest.mark.parametrize("transport_type", [ROSTransport, DDSTransport]) +def test_generated_messages_require_ros_dds_type_adapters(transport_type): + with pytest.raises(TypeError, match="type adapter"): + transport_type("generated", Int32) + + +@pytest.mark.parametrize("config", [LCM, ZENOH]) +def test_factory_does_not_pickle_unregistered_cdr_types(config): + class Unregistered: + __msgtype__ = "test_msgs/msg/Unregistered" + + with pytest.raises(TypeError): + make_transport("invalid", Unregistered, g=config) diff --git a/dimos/core/test_zenoh_transport.py b/dimos/core/test_zenoh_transport.py index 31c1532292c..a141fa3030b 100644 --- a/dimos/core/test_zenoh_transport.py +++ b/dimos/core/test_zenoh_transport.py @@ -48,14 +48,6 @@ from dimos.protocol.service.zenohservice import ZenohSessionPool -class TypedMsg: - """A fake typed message with lcm_encode for testing.""" - - @staticmethod - def lcm_encode() -> bytes: - return b"" - - class UntypedMsg: """A message without lcm_encode. Triggers pickle transport.""" @@ -63,12 +55,12 @@ class UntypedMsg: class ProducerModule(Module): - typed_data: Out[TypedMsg] + typed_data: Out[Image] untyped_data: Out[UntypedMsg] class ConsumerModule(Module): - typed_data: In[TypedMsg] + typed_data: In[Image] untyped_data: In[UntypedMsg] @@ -147,7 +139,7 @@ def test_invalid_transport_is_rejected_on_update() -> None: def test_lcm_transport_returned_when_transport_is_lcm(use_lcm, blueprint) -> None: - transport = _get_transport_for(blueprint, "typed_data", TypedMsg) + transport = _get_transport_for(blueprint, "typed_data", Image) assert isinstance(transport, LCMTransport) @@ -157,7 +149,7 @@ def test_lcm_pickle_transport_returned_for_untyped_when_lcm(use_lcm, blueprint) def test_zenoh_transport_returned_when_transport_is_zenoh(use_zenoh, blueprint) -> None: - transport = _get_transport_for(blueprint, "typed_data", TypedMsg) + transport = _get_transport_for(blueprint, "typed_data", Image) assert isinstance(transport, ZenohTransport) diff --git a/dimos/core/transport.py b/dimos/core/transport.py index ffef78418dd..7f844669d87 100644 --- a/dimos/core/transport.py +++ b/dimos/core/transport.py @@ -29,6 +29,7 @@ from dimos.core.stream import Stream, Transport from dimos.msgs.protocol import DimosMsg +from dimos.protocol.pubsub.codec import describe_message from dimos.protocol.pubsub.impl.lcmpubsub import LCM, PickleLCM, Topic as LCMTopic from dimos.protocol.pubsub.impl.webrtc.providers.broker import BrokerConfig from dimos.protocol.pubsub.impl.webrtc.providers.spec import AudioProvider, ProviderConfig @@ -198,6 +199,11 @@ class ROSTransport(PubSubTransport[DimosMsg]): _ros: DimosROS | None = None def __init__(self, topic: str, msg_type: type[DimosMsg], **kwargs: Any) -> None: + # TODO: Revisit generated-message adapters; ROS also uses CDR encoding. + if hasattr(msg_type, "__msgtype__") and describe_message(msg_type).encoding == "cdr": + raise TypeError( + "Generated messages require a ROS type adapter; raw CDR is not a ROS message" + ) # rclpy (about 0.1 s with rospubsub): only loaded when a ROSTransport exists. from dimos.protocol.pubsub.impl.rospubsub import ROSTopic @@ -241,6 +247,11 @@ def stop(self) -> None: class DDSTransport(PubSubTransport[T]): def __init__(self, topic: str, type: type, **kwargs) -> None: # type: ignore[no-untyped-def] + # TODO: Revisit generated-message adapters; DDS also uses CDR encoding. + if hasattr(type, "__msgtype__") and describe_message(type).encoding == "cdr": + raise TypeError( + "Generated messages require a DDS type adapter; raw CDR is not a DDS type" + ) # cyclonedds: see DDS_AVAILABLE. from dimos.protocol.pubsub.impl.ddspubsub import DDS, Topic as DDSTopic @@ -280,7 +291,7 @@ def subscribe( return self.dds.subscribe(self.topic, lambda msg, topic: callback(msg)) -M = TypeVar("M", bound=DimosMsg) +M = TypeVar("M") def _rebuild_webrtc_transport( @@ -296,10 +307,9 @@ class WebRTCTransport(PubSubTransport[M]): also be used directly with an explicit ``config``. Two modes: * **Raw bytes** (``msg_type=None``): messages pass through as ``bytes``. - * **Typed LCM** (``msg_type=SomeMsg``): LCM-encoded on ``broadcast()``, - LCM-decoded on ``subscribe()`` (foreign types on the shared channel are skipped) — so multiple - transports sharing one multiplexed DataChannel each receive only - their own message type. + * **Typed messages**: bind a shared codec from ``msg_type``. Generated + messages use raw CDR; legacy messages keep their LCM wire format. + Peers must agree on the schema; remote schema validation is deferred. The transport itself holds no connection: the picklable ``config`` resolves to a per-process singleton provider on first use, so transports @@ -322,6 +332,7 @@ def __init__( ) -> None: super().__init__(topic) self._msg_type = msg_type + self._message = describe_message(msg_type) if msg_type is not None else None self._config = config or self._config_cls(**config_kwargs) self._pubsub: WebRTCPubSub | None = None # Guards first-use init: concurrent subscribe()/broadcast() must not @@ -337,7 +348,7 @@ def broadcast(self, _: Stream[M] | None, msg: M) -> None: if not self._started: self.start() assert self._pubsub is not None - data = msg.lcm_encode() if self._msg_type is not None else msg + data = self._message.codec.encode(msg) if self._message is not None else cast("bytes", msg) self._pubsub.publish(self.topic, data) # type: ignore[arg-type] def subscribe( @@ -347,18 +358,15 @@ def subscribe( self.start() assert self._pubsub is not None - if self._msg_type is not None: - msg_type = self._msg_type + if self._message is not None: + descriptor = self._message def _typed_cb(data: bytes, _topic: str) -> None: - # The channel is multiplexed (e.g. the browser sends Twists and - # Poses on cmd_unreliable); lcm_decode verifies the wire - # fingerprint and raises on other types — skip those. try: - msg = msg_type.lcm_decode(data) + msg = descriptor.codec.decode(data) except ValueError: return - callback(msg) # type: ignore[arg-type] + callback(msg) return self._pubsub.subscribe(self.topic, _typed_cb) return self._pubsub.subscribe(self.topic, lambda msg, _topic: callback(msg)) # type: ignore[arg-type] @@ -503,7 +511,7 @@ class CloudflareAudioTransport(WebRTCAudioTransport): class ZenohTransport(PubSubTransport[T]): - """Zenoh transport with LCM encoding for typed DimosMsg. + """Zenoh transport with a codec bound to the declared message type. Accepts either a plain topic string plus message type, or a full `ZenohTopic` carrying per-topic settings: `ZenohTransport(ZenohTopic("bla", @@ -546,7 +554,7 @@ def stop(self) -> None: def broadcast(self, _: Stream[T] | None, msg: T) -> None: if not self._started: self.start() - self.zenoh.publish(self.topic, cast("DimosMsg", msg)) + self.zenoh.publish(self.topic, msg) def subscribe( self, callback: Callable[[T], None], selfstream: Stream[T] | None = None diff --git a/dimos/core/transport_factory.py b/dimos/core/transport_factory.py index b86b271682a..5a3e88f352b 100644 --- a/dimos/core/transport_factory.py +++ b/dimos/core/transport_factory.py @@ -27,6 +27,7 @@ pLCMTransport, pZenohTransport, ) +from dimos.protocol.pubsub.codec import describe_message from dimos.protocol.pubsub.impl.zenohpubsub import ( QOS_LATEST_WINS, QOS_NEVER_DROP, @@ -91,7 +92,10 @@ def default_zenoh_qos_for(name: str, msg_name: str) -> ZenohQoS | None: def default_zenoh_qos(name: str, msg_type: type | None = None) -> ZenohQoS | None: """Default publisher QoS for a logical channel; None = zenoh defaults.""" - return default_zenoh_qos_for(name, getattr(msg_type, "msg_name", "")) + type_name = getattr(msg_type, "msg_name", "") + if msg_type is not None and hasattr(msg_type, "__msgtype__"): + type_name = describe_message(msg_type).name.replace("/msg/", ".") + return default_zenoh_qos_for(name, type_name) def make_transport( @@ -99,8 +103,8 @@ def make_transport( ) -> PubSubTransport[Any]: """Construct the active-backend pub/sub transport for a logical channel. - A pickled (self-describing) transport is used when no `msg_type` is given or - the type has no `lcm_encode`. Otherwise a typed transport is used. + Generated messages use CDR and legacy messages keep LCM encoding. + Other Python types and undeclared types preserve the existing pickle transport. A channel name alone doesn't fully define a topic: backends have per-topic settings (Zenoh publisher QoS, for one) that live on their Topic objects. @@ -110,7 +114,11 @@ def make_transport( transport map. LCM (UDP multicast) has no per-topic settings. """ - use_pickled = msg_type is None or getattr(msg_type, "lcm_encode", None) is None + use_pickled = msg_type is None or ( + not hasattr(msg_type, "__msgtype__") and getattr(msg_type, "lcm_encode", None) is None + ) + if msg_type is not None and not use_pickled: + describe_message(msg_type) topic = transport_topic(name, g) if g.transport == "zenoh": ztopic = ZenohTopic( diff --git a/dimos/experimental/memory/cdr_schema.py b/dimos/experimental/memory/cdr_schema.py new file mode 100644 index 00000000000..624576014c4 --- /dev/null +++ b/dimos/experimental/memory/cdr_schema.py @@ -0,0 +1,32 @@ +# 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. + +"""Resolve schema metadata once when registering a raw CDR recording stream.""" + +from typing import Any + +from dimos_cdrgen.registry import message_types, schema + + +def cdr_name(message_type: type[Any]) -> str | None: + if not hasattr(message_type, "__msgtype__"): + return None + return next((name for name, cls in message_types().items() if cls is message_type), None) + + +def recording_schema(message_type: type[Any]) -> dict[str, Any]: + name = cdr_name(message_type) + if name is None: + raise TypeError("CDR recording requires an installed, registry-owned message type") + return {"name": name, "data": schema(name)} diff --git a/dimos/experimental/memory/rust/Cargo.toml b/dimos/experimental/memory/rust/Cargo.toml index 7ac39440148..08964427d05 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 = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } lz4_flex = "0.14.0" mcap = { version = "0.25.0", default-features = false, features = ["zstd"] } diff --git a/dimos/experimental/memory/rust/src/cdr.rs b/dimos/experimental/memory/rust/src/cdr.rs new file mode 100644 index 00000000000..b06527c0500 --- /dev/null +++ b/dimos/experimental/memory/rust/src/cdr.rs @@ -0,0 +1,39 @@ +// 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. + +//! Schema metadata for opaque CDR recording. + +use anyhow::{ensure, Result}; +use serde::{Deserialize, Serialize}; + +#[derive(Clone, Debug, Deserialize, Serialize)] +#[serde(deny_unknown_fields)] +pub struct CdrSchema { + pub name: String, + pub data: String, +} + +impl CdrSchema { + pub fn validate(&self) -> Result<()> { + ensure!( + self.name.split('/').count() == 3 && self.name.contains("/msg/"), + "CDR schema requires package/msg/Type identity" + ); + ensure!( + !self.data.trim().is_empty(), + "CDR recording requires complete ros2msg schema data" + ); + Ok(()) + } +} diff --git a/dimos/experimental/memory/rust/src/decoding.rs b/dimos/experimental/memory/rust/src/decoding.rs index 79b6b98a5c1..adb51abef6b 100644 --- a/dimos/experimental/memory/rust/src/decoding.rs +++ b/dimos/experimental/memory/rust/src/decoding.rs @@ -30,6 +30,8 @@ use crate::{Codec, StreamConfig, IMAGE_PAYLOAD_TYPE}; /// Transport-decoded payload consumed by storage codecs. #[derive(Debug)] pub(crate) enum DecodedPayload { + /// Plain CDR is preserved byte-for-byte, including its original Header. + Cdr(Vec), /// An LCM payload whose storage codecs operate on its canonical wire form. Lcm(Vec), /// UTF-8 JSON after removing the existing String transport envelope. @@ -51,6 +53,12 @@ pub(crate) fn decode( data: &[u8], reception_ts: f64, ) -> Result> { + if stream.codec == Codec::Cdr { + return Ok(vec![DecodedObservation { + ts: reception_ts, + payload: DecodedPayload::Cdr(data.to_vec()), + }]); + } if stream.codec == Codec::Json { anyhow::ensure!( stream.payload_type == "dimos.msgs.std_msgs.String.String", @@ -194,6 +202,7 @@ mod tests { fn json_stream(field: Option<&str>) -> StreamConfig { StreamConfig { + cdr_schema: None, port: "events".into(), name: "events".into(), payload_type: "dimos.msgs.std_msgs.String.String".into(), diff --git a/dimos/experimental/memory/rust/src/encoding.rs b/dimos/experimental/memory/rust/src/encoding.rs index 00eacc3823b..8e016741acf 100644 --- a/dimos/experimental/memory/rust/src/encoding.rs +++ b/dimos/experimental/memory/rust/src/encoding.rs @@ -35,6 +35,10 @@ pub(crate) fn encode( observation: DecodedObservation, ) -> Result { let data = match stream.codec { + Codec::Cdr => match observation.payload { + DecodedPayload::Cdr(data) => data, + _ => return Err(anyhow!("CDR storage requires a CDR transport payload")), + }, Codec::Lcm => lcm_encode(observation.payload)?, Codec::Json => match observation.payload { DecodedPayload::Json(data) => data, @@ -53,7 +57,9 @@ fn lcm_encode(payload: DecodedPayload) -> Result> { Ok(match payload { DecodedPayload::Lcm(data) => data, DecodedPayload::Image(image) => image.encode(), - DecodedPayload::Json(_) => return Err(anyhow!("LCM codec cannot store JSON payload")), + DecodedPayload::Json(_) | DecodedPayload::Cdr(_) => { + return Err(anyhow!("LCM codec cannot store non-LCM payload")) + } }) } diff --git a/dimos/experimental/memory/rust/src/lib.rs b/dimos/experimental/memory/rust/src/lib.rs index 74df3f3f9f9..c05bdaabc40 100644 --- a/dimos/experimental/memory/rust/src/lib.rs +++ b/dimos/experimental/memory/rust/src/lib.rs @@ -31,6 +31,7 @@ use tracing::{debug, info}; use crate::encoding::StoredObservation; use crate::store::{Observation, RecordingStore}; +pub mod cdr; mod decoding; mod encoding; pub mod store; @@ -44,6 +45,7 @@ const FLUSH_INTERVAL: Duration = Duration::from_millis(100); #[derive(Clone, Copy, Debug, Deserialize, Serialize, PartialEq, Eq)] #[serde(rename_all = "lowercase")] pub enum Codec { + Cdr, Lcm, Json, Jpeg, @@ -54,6 +56,7 @@ pub enum Codec { impl Codec { pub(crate) fn id(self) -> &'static str { match self { + Self::Cdr => "cdr", Self::Lcm => "lcm", Self::Json => "json", Self::Jpeg => "jpeg", @@ -73,11 +76,13 @@ pub struct StreamConfig { pub timestamp_field: Option, #[serde(default)] pub json_schema: Option, + #[serde(default, skip_serializing_if = "Option::is_none")] + pub cdr_schema: Option, } impl StreamConfig { pub fn is_tf(&self) -> bool { - self.payload_type == TF_PAYLOAD_TYPE + self.codec != Codec::Cdr && self.payload_type == TF_PAYLOAD_TYPE } } @@ -152,6 +157,18 @@ pub struct RecorderEngine { impl RecorderEngine { pub fn start(config: RecorderConfig) -> Result { for stream in &config.streams { + if stream.codec == Codec::Cdr { + stream + .cdr_schema + .as_ref() + .context("CDR codec requires its resolved schema")? + .validate()?; + } else { + anyhow::ensure!( + stream.cdr_schema.is_none(), + "Only the cdr codec accepts a CDR schema" + ); + } anyhow::ensure!( stream.codec == Codec::Json || (stream.timestamp_field.is_none() && stream.json_schema.is_none()), @@ -467,6 +484,7 @@ mod tests { fn stream(name: &str, codec: Codec, is_tf: bool) -> Arc { Arc::new(StreamConfig { + cdr_schema: None, port: name.to_string(), name: name.to_string(), payload_type: if is_tf { @@ -482,6 +500,7 @@ mod tests { fn typed_stream(name: &str, payload_type: &str, codec: Codec) -> Arc { Arc::new(StreamConfig { + cdr_schema: None, port: name.to_string(), name: name.to_string(), payload_type: payload_type.to_string(), @@ -562,6 +581,7 @@ mod tests { let mut message = Imu::default(); message.header.stamp = Time { sec: 42, nsec: 25 }; let stream = Arc::new(StreamConfig { + cdr_schema: None, port: "imu".to_string(), name: "imu".to_string(), payload_type: "dimos.msgs.sensor_msgs.Imu.Imu".to_string(), @@ -817,6 +837,7 @@ mod tests { let message = lcm_msgs::std_msgs::String { data: text.into() }; let schema = serde_json::json!({"type":"object","properties":{"sent":{"type":"number"}}}); let stream = Arc::new(StreamConfig { + cdr_schema: None, port: "events".into(), name: "events".into(), payload_type: "dimos.msgs.std_msgs.String.String".into(), @@ -870,3 +891,95 @@ mod tests { } } } + +#[cfg(test)] +mod cdr_recording_tests { + use super::*; + use crate::cdr::CdrSchema; + use tempfile::tempdir; + + #[test] + fn cdr_raw_storage_keeps_schema_and_opaque_payload_with_reception_time() { + let directory = tempdir().unwrap(); + let path = directory.path().join("cdr.mcap"); + let schema_text = "int32 sec\nuint32 nanosec\nstring label\n"; + let stream = Arc::new(StreamConfig { + port: "events".into(), + name: "events".into(), + payload_type: "test_msgs.StampedEvent".into(), + codec: Codec::Cdr, + timestamp_field: None, + json_schema: None, + cdr_schema: Some(CdrSchema { + name: "test_msgs/msg/StampedEvent".into(), + data: schema_text.into(), + }), + }); + let payload = vec![0, 1, 0, 0, 255, 123, 45]; + let encoded = process(&stream, &payload, 99.0).unwrap().pop().unwrap(); + assert_eq!(encoded.data, payload); + assert_eq!(encoded.ts, 99.0); + let config = store::RecordingStoreConfig::Mcap { + path: path.to_str().unwrap().into(), + }; + let mut output = store::open(&config, &[(*stream).clone()], 1).unwrap(); + output + .write_batch(&[Observation { + stream, + source_ts: encoded.ts, + reception_ts: 99.0, + data: encoded.data, + }]) + .unwrap(); + output.finish().unwrap(); + let bytes = std::fs::read(path).unwrap(); + let messages = mcap::MessageStream::new(&bytes) + .unwrap() + .collect::, _>>() + .unwrap(); + assert_eq!(messages.len(), 1); + let message = &messages[0]; + assert_eq!(message.publish_time, 99_000_000_000); + assert_eq!(message.log_time, 99_000_000_000); + assert_eq!(message.channel.message_encoding, "cdr"); + assert_eq!(message.data.as_ref(), payload); + let actual_schema = message.channel.schema.as_ref().unwrap(); + assert_eq!(actual_schema.name, "test_msgs/msg/StampedEvent"); + assert_eq!(actual_schema.encoding, "ros2msg"); + assert_eq!(actual_schema.data.as_ref(), schema_text.as_bytes()); + } + + #[test] + fn cdr_stream_preflight_rejects_missing_or_cross_codec_schema() { + let directory = tempdir().unwrap(); + let mut stream = StreamConfig { + port: "sample".into(), + name: "sample".into(), + payload_type: "test.Sample".into(), + codec: Codec::Cdr, + cdr_schema: None, + timestamp_field: None, + json_schema: None, + }; + let config = |stream| RecorderConfig { + store: store::RecordingStoreConfig::Mcap { + path: directory + .path() + .join("should-not-exist.mcap") + .to_str() + .unwrap() + .into(), + }, + encoding_threads: 1, + streams: vec![stream], + }; + assert!(RecorderEngine::start(config(stream.clone())).is_err()); + stream.cdr_schema = Some(CdrSchema { + name: "test_msgs/msg/Clock".into(), + data: "float64 ts\n".into(), + }); + stream.codec = Codec::Lcm; + assert!(RecorderEngine::start(config(stream)).is_err()); + assert!(!directory.path().join("should-not-exist.mcap").exists()); + } +} diff --git a/dimos/experimental/memory/rust/src/store/mcap.rs b/dimos/experimental/memory/rust/src/store/mcap.rs index 23213affe24..cb4a43d81b6 100644 --- a/dimos/experimental/memory/rust/src/store/mcap.rs +++ b/dimos/experimental/memory/rust/src/store/mcap.rs @@ -32,9 +32,26 @@ pub struct McapRecordingStore { impl McapRecordingStore { pub fn open(path: &str, streams: &[StreamConfig], compression_threads: usize) -> Result { + for stream in streams { + if stream.codec == crate::Codec::Cdr { + stream + .cdr_schema + .as_ref() + .context("CDR MCAP requires its complete schema")? + .validate()?; + } + } let file = File::create(path).with_context(|| format!("failed to create {path}"))?; + let profile = if streams + .iter() + .all(|stream| stream.codec == crate::Codec::Cdr) + { + "ros2" + } else { + "dimos" + }; let options = WriteOptions::new() - .profile("dimos") + .profile(profile) .library("dimos-memory-recorder") .compression(Some(Compression::Zstd)) .compression_threads(compression_threads.try_into().unwrap_or(u32::MAX)); @@ -50,10 +67,17 @@ impl McapRecordingStore { ("dimos.port".to_string(), stream.port.clone()), ( "dimos.observation_time".to_string(), - "publish_time".to_string(), + if stream.cdr_schema.is_some() { + "log_time" + } else { + "publish_time" + } + .to_string(), ), ]); - let schema_id = if let Some(schema) = &stream.json_schema { + let schema_id = if let Some(schema) = &stream.cdr_schema { + writer.add_schema(&schema.name, "ros2msg", schema.data.as_bytes())? + } else if let Some(schema) = &stream.json_schema { writer.add_schema(&stream.name, "jsonschema", &serde_json::to_vec(schema)?)? } else { 0 @@ -145,6 +169,7 @@ mod tests { fn mcap_rejects_negative_source_time_instead_of_writing_zero() { let file = NamedTempFile::new().unwrap(); let stream = Arc::new(StreamConfig { + cdr_schema: None, port: "events".into(), name: "events".into(), payload_type: "dimos.msgs.std_msgs.String.String".into(), @@ -190,6 +215,7 @@ mod tests { fn mcap_preserves_zero_source_time_and_rejects_invalid_reception_time() { let file = NamedTempFile::new().unwrap(); let stream = Arc::new(StreamConfig { + cdr_schema: None, port: "events".into(), name: "events".into(), payload_type: "dimos.msgs.std_msgs.String.String".into(), diff --git a/dimos/experimental/memory/rust_cli_recorder.py b/dimos/experimental/memory/rust_cli_recorder.py index 2214635c4e2..060424400aa 100644 --- a/dimos/experimental/memory/rust_cli_recorder.py +++ b/dimos/experimental/memory/rust_cli_recorder.py @@ -30,6 +30,7 @@ from dimos.core.global_config import global_config from dimos.core.transport import LCMTransport, ZenohTransport from dimos.core.transport_factory import session_config +from dimos.experimental.memory.cdr_schema import cdr_name, recording_schema from dimos.experimental.memory.rust_recorder import RustRecorderConfig, RustStreamSpec from dimos.memory.store.sqlite import SqliteStore from dimos.memory.tap import matching, recording_dir @@ -57,6 +58,14 @@ class RustRecordingPlan: def prepare_rust_recorder() -> Path: """Build the native recorder when it is unavailable or explicitly requested.""" + if global_config.record_executable is not None: + executable = Path(global_config.record_executable).expanduser().resolve() + if not executable.is_file(): + raise FileNotFoundError(f"Rust recorder executable not found: {executable}") + if not os.access(executable, os.X_OK): + raise PermissionError(f"Rust recorder is not executable: {executable}") + return executable + config = RustRecorderConfig() rust_dir, executable_path = config.resolve_paths() executable = Path(executable_path) @@ -103,9 +112,12 @@ def make_plan(transports: dict[tuple[str, type], Any]) -> RustRecordingPlan: for index, ((name, payload_type), transport) in enumerate(transports.items()): if name not in selected: continue - if not hasattr(payload_type, "lcm_encode") or not hasattr(payload_type, "lcm_decode"): + generated = cdr_name(payload_type) is not None + if not generated and ( + not hasattr(payload_type, "lcm_encode") or not hasattr(payload_type, "lcm_decode") + ): logger.info( - "--record: stream is not a DimOS LCM message type; skipped", + "--record: stream is not a supported message type; skipped", stream=name, payload_type=str(payload_type), ) @@ -131,7 +143,10 @@ def make_plan(transports: dict[tuple[str, type], Any]) -> RustRecordingPlan: port=port, name=name, payload_type=f"{payload_type.__module__}.{payload_type.__qualname__}", - codec="jpeg" if issubclass(payload_type, Image) else "lcm", + codec="cdr" + if generated + else ("jpeg" if issubclass(payload_type, Image) else "lcm"), + cdr_schema=recording_schema(payload_type) if generated else None, ) ) topics[port] = transport.channel @@ -299,4 +314,6 @@ def stop(self) -> None: thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) if process.returncode not in (None, 0): logger.warning("Rust recorder stopped with an error", status=process.returncode) + if process.returncode == 0: + logger.info("Recording saved", artifact_path=str(self.plan.path)) self._process = None diff --git a/dimos/experimental/memory/rust_recorder.py b/dimos/experimental/memory/rust_recorder.py index 8324c0469f8..f77e625ba57 100644 --- a/dimos/experimental/memory/rust_recorder.py +++ b/dimos/experimental/memory/rust_recorder.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Experimental native recorder for high-throughput LCM-backed memory streams.""" +"""Experimental native recorder for legacy LCM and generated CDR streams.""" from __future__ import annotations @@ -27,6 +27,7 @@ from dimos.core.module import Module from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import In +from dimos.experimental.memory.cdr_schema import cdr_name, recording_schema from dimos.memory.store.sqlite import SqliteStore from dimos.memory.type.recording import OnExisting from dimos.msgs.sensor_msgs.Image import Image @@ -37,7 +38,7 @@ logger = setup_logger() -_SUPPORTED_NATIVE_CODECS = {"lcm", "jpeg", "lz4+lcm", "json"} +_SUPPORTED_NATIVE_CODECS = {"lcm", "jpeg", "lz4+lcm", "json", "cdr"} class RustStreamSpec(BaseModel): @@ -49,6 +50,7 @@ class RustStreamSpec(BaseModel): codec: str timestamp_field: str | None = None json_schema: dict[str, Any] | None = None + cdr_schema: dict[str, Any] | None = None @model_validator(mode="after") def _json_options(self) -> RustStreamSpec: @@ -56,6 +58,10 @@ def _json_options(self) -> RustStreamSpec: self.timestamp_field is not None or self.json_schema is not None ): raise ValueError("JSON options require the json codec") + if (self.codec == "cdr") != (self.cdr_schema is not None): + raise ValueError( + "The cdr codec requires its resolved schema; other codecs must not carry one" + ) return self @@ -220,7 +226,9 @@ def _stream_specs(self) -> list[RustStreamSpec]: for port_name, port in self.inputs.items(): if getattr(port, "_transport", None) is None: continue - if port is self.tf: + if port is self.tf and not self.config.record_tf: + continue + if port is self.tf and cdr_name(port.type) is None: if self.config.record_tf: specs.append( RustStreamSpec( @@ -243,6 +251,7 @@ def _stream_specs(self) -> list[RustStreamSpec]: codec=codec, timestamp_field=self.config.stream_timestamp_fields.get(stream_name), json_schema=self.config.stream_json_schemas.get(stream_name), + cdr_schema=recording_schema(port.type) if codec == "cdr" else None, ) ) @@ -261,12 +270,14 @@ def _collect_topics(self) -> dict[str, str]: @staticmethod def _default_codec(payload_type: type[Any]) -> str: + if cdr_name(payload_type) is not None: + return "cdr" if issubclass(payload_type, Image): return "jpeg" if hasattr(payload_type, "lcm_encode") and hasattr(payload_type, "lcm_decode"): return "lcm" raise TypeError( - f"RustRecorder only supports LCM-backed messages, got {payload_type.__qualname__}" + f"RustRecorder supports legacy LCM or registered CDR messages, got {payload_type.__qualname__}" ) @staticmethod @@ -276,6 +287,10 @@ def _validate_codec(stream_name: str, payload_type: type[Any], codec: str) -> No f"Unsupported native codec {codec!r} for stream {stream_name!r}; " f"choose one of {sorted(_SUPPORTED_NATIVE_CODECS)}" ) + if codec == "cdr": + if cdr_name(payload_type) is None: + raise TypeError("The cdr codec requires a generated, registry-owned message type") + return if codec == "json" and payload_type is not String: raise TypeError("JSON codec requires std_msgs.String") if codec == "jpeg" and not issubclass(payload_type, Image): diff --git a/dimos/experimental/memory/test_cdr_schema.py b/dimos/experimental/memory/test_cdr_schema.py new file mode 100644 index 00000000000..4c81d3ef2e3 --- /dev/null +++ b/dimos/experimental/memory/test_cdr_schema.py @@ -0,0 +1,31 @@ +# 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. + +"""Recording reuses registered schemas without inspecting payload fields.""" + +from dimos_cdrgen.registry import schema +import pytest + +from dimos.experimental.memory.cdr_schema import cdr_name, recording_schema +from dimos.messages.geometry_msgs.msg import PoseStamped + + +def test_registry_owner_and_complete_schema_are_reused(): + assert cdr_name(PoseStamped) == "geometry_msgs/msg/PoseStamped" + assert recording_schema(PoseStamped) == { + "name": "geometry_msgs/msg/PoseStamped", + "data": schema("geometry_msgs/msg/PoseStamped"), + } + with pytest.raises(TypeError, match="registry-owned"): + recording_schema(bytes) diff --git a/dimos/experimental/memory/test_rust_cli_recorder.py b/dimos/experimental/memory/test_rust_cli_recorder.py index 1b23bb3145e..447c3df7abc 100644 --- a/dimos/experimental/memory/test_rust_cli_recorder.py +++ b/dimos/experimental/memory/test_rust_cli_recorder.py @@ -27,9 +27,11 @@ from dimos.core.global_config import global_config from dimos.core.transport import LCMTransport, ZenohTransport from dimos.experimental.memory import rust_cli_recorder +from dimos.experimental.memory.cdr_schema import recording_schema from dimos.experimental.memory.rust_cli_recorder import RustRecordingPlan, RustRecordingSession from dimos.experimental.memory.rust_recorder import RustStreamSpec from dimos.memory.store.sqlite import SqliteStore +from dimos.messages.geometry_msgs.msg import PoseStamped as GeneratedPoseStamped from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.Image import Image from dimos.protocol.pubsub.impl.zenohpubsub import Topic as ZenohTopic @@ -359,3 +361,40 @@ def build(*args, **kwargs): assert rust_cli_recorder.prepare_rust_recorder() == executable assert popen.call_args.kwargs["cwd"] == str(tmp_path / "dimos/experimental/memory/rust") assert popen.call_args.args[0] == "nix build -L path:." + + +@pytest.mark.parametrize("backend", ["lcm", "zenoh"]) +def test_plan_records_generated_messages_with_schema_on_plain_topic(backend): + transport = ( + LCMTransport("pose", GeneratedPoseStamped, url="memq://") + if backend == "lcm" + else ZenohTransport("pose", GeneratedPoseStamped) + ) + try: + plan = rust_cli_recorder.make_plan({("pose", GeneratedPoseStamped): transport}) + assert plan.backend == backend + assert plan.topics == {"stream_0": "pose"} + assert plan.streams[0].codec == "cdr" + assert plan.streams[0].cdr_schema == recording_schema(GeneratedPoseStamped) + finally: + transport.stop() + + +def test_prepare_uses_explicit_executable_without_build(tmp_path, monkeypatch, mocker): + executable = tmp_path / "recorder" + executable.write_text("#!/bin/sh\nexit 0\n") + executable.chmod(0o755) + monkeypatch.setattr(global_config, "record_executable", str(executable)) + build = mocker.patch.object(rust_cli_recorder.subprocess, "Popen") + assert rust_cli_recorder.prepare_rust_recorder() == executable + build.assert_not_called() + + +@pytest.mark.parametrize("exists", [False, True]) +def test_prepare_rejects_unusable_explicit_executable(tmp_path, monkeypatch, exists): + executable = tmp_path / "recorder" + if exists: + executable.touch() + monkeypatch.setattr(global_config, "record_executable", str(executable)) + with pytest.raises(PermissionError if exists else FileNotFoundError): + rust_cli_recorder.prepare_rust_recorder() diff --git a/dimos/experimental/memory/test_rust_recorder.py b/dimos/experimental/memory/test_rust_recorder.py index f61140f2ee8..ae044f109ea 100644 --- a/dimos/experimental/memory/test_rust_recorder.py +++ b/dimos/experimental/memory/test_rust_recorder.py @@ -16,12 +16,15 @@ from pathlib import Path from typing import Any, TypeVar +from unittest.mock import Mock import pytest from dimos.core.global_config import GlobalConfig from dimos.core.module import Module from dimos.core.stream import In +from dimos.core.transport import PubSubTransport +from dimos.experimental.memory.cdr_schema import recording_schema from dimos.experimental.memory.rust_recorder import ( RustMcapStoreConfig, RustRecorder, @@ -31,8 +34,10 @@ ) from dimos.memory.store.sqlite import SqliteStore from dimos.memory.type.recording import OnExisting +from dimos.messages.geometry_msgs.msg import PoseStamped as NativePoseStamped from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.Image import Image +from dimos.protocol.rpc.zenohrpc import ZenohRPC class SampleRustRecorder(RustRecorder): @@ -48,19 +53,14 @@ class UnsupportedRustRecorder(RustRecorder): values: In[Unsupported] -class FakeTransport: - def __init__(self, channel: str) -> None: - self.channel = channel - - def stop(self) -> None: - pass - - TRecorder = TypeVar("TRecorder", bound=RustRecorder) @pytest.fixture -def make_recorder() -> Any: +def make_recorder(mocker) -> Any: + mocker.patch.object(ZenohRPC, "start") + mocker.patch.object(ZenohRPC, "serve_module_rpc") + mocker.patch("dimos.core.module.get_loop", return_value=(None, None)) recorders: list[RustRecorder] = [] def make(recorder_type: type[TRecorder], **kwargs: Any) -> TRecorder: @@ -75,7 +75,7 @@ def make(recorder_type: type[TRecorder], **kwargs: Any) -> TRecorder: def connect(recorder: RustRecorder, **channels: str) -> None: for name, channel in channels.items(): - getattr(recorder, name).transport = FakeTransport(channel) # type: ignore[assignment] + getattr(recorder, name).transport = Mock(spec=PubSubTransport, channel=channel) def test_specs_use_native_defaults_remapping_and_configured_workers( @@ -209,7 +209,7 @@ def test_unsupported_python_payload_fails_before_native_process_starts( ) connect(recorder, values="/values") - with pytest.raises(TypeError, match="only supports LCM-backed messages"): + with pytest.raises(TypeError, match="supports legacy LCM or registered CDR messages"): recorder._stream_specs() @@ -369,3 +369,49 @@ def test_replay_never_prepares_native_sources_or_process(tmp_path, make_recorder spawn.assert_not_called() start.assert_called_once_with(recorder) assert not path.exists() + + +class GeneratedRustRecorder(RustRecorder): + pose: In[NativePoseStamped] + + +def test_generated_values_resolve_cdr_and_full_schema_without_legacy_type_copy( + tmp_path, make_recorder +): + recorder = make_recorder( + GeneratedRustRecorder, + store=RustSqliteStoreConfig(path=str(tmp_path / "native.db")), + record_tf=False, + ) + connect(recorder, pose="example/pose") + specs = recorder._stream_specs() + assert len(specs) == 1 + assert specs[0].codec == "cdr" + assert specs[0].cdr_schema == recording_schema(NativePoseStamped) + assert ( + specs[0].payload_type == f"{NativePoseStamped.__module__}.{NativePoseStamped.__qualname__}" + ) + recorder._prepare_store(specs) + with SqliteStore(path=str(tmp_path / "native.db")) as store: + assert store.stream("pose").data_type is NativePoseStamped + + +def test_generated_values_cannot_silently_select_a_legacy_wire_codec(tmp_path, make_recorder): + recorder = make_recorder( + GeneratedRustRecorder, + store=RustSqliteStoreConfig(path=str(tmp_path / "native.db")), + record_tf=False, + stream_codecs={"pose": "lcm"}, + ) + connect(recorder, pose="example/pose") + with pytest.raises(TypeError, match="LCM-backed"): + recorder._stream_specs() + assert not (tmp_path / "native.db").exists() + + +def test_cdr_stream_schema_is_required_and_cannot_leak_into_other_codecs(): + options = dict(port="pose", name="pose", payload_type="geometry_msgs/msg/PoseStamped") + with pytest.raises(ValueError, match="requires its resolved schema"): + RustStreamSpec(**options, codec="cdr") + with pytest.raises(ValueError, match="other codecs must not carry one"): + RustStreamSpec(**options, codec="lcm", cdr_schema=recording_schema(NativePoseStamped)) diff --git a/dimos/experimental/memory/test_rust_recorder_e2e.py b/dimos/experimental/memory/test_rust_recorder_e2e.py index d41ebe3921e..50a24483d36 100644 --- a/dimos/experimental/memory/test_rust_recorder_e2e.py +++ b/dimos/experimental/memory/test_rust_recorder_e2e.py @@ -14,6 +14,7 @@ from __future__ import annotations +from contextlib import ExitStack import importlib.util import os from pathlib import Path @@ -25,13 +26,14 @@ from typing import cast import uuid +from dimos_cdrgen.registry import encode as encode_cdr import numpy as np import pytest from dimos.constants import DIMOS_PROJECT_ROOT from dimos.core.global_config import global_config from dimos.core.stream import In -from dimos.core.transport import LCMTransport, ZenohTransport +from dimos.core.transport import LCMTransport, PubSubTransport, ZenohTransport from dimos.experimental.memory import rust_cli_recorder from dimos.experimental.memory.rust_cli_recorder import RustRecordingSession from dimos.experimental.memory.rust_recorder import ( @@ -42,15 +44,25 @@ ) from dimos.memory.codecs.lcm import LcmCodec from dimos.memory.codecs.lz4 import Lz4Codec +from dimos.memory.store.cdr_mcap import CdrMcapStore from dimos.memory.store.mcap import McapStore from dimos.memory.store.sqlite import SqliteStore from dimos.memory.type.observation import Observation +from dimos.messages.builtin_interfaces.msg import Time as NativeTime +from dimos.messages.geometry_msgs.msg import ( + Point as NativePoint, + Pose as NativePose, + PoseStamped as NativePoseStamped, + Quaternion as NativeQuaternion, +) +from dimos.messages.std_msgs.msg import Header as NativeHeader 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.sensor_msgs.Imu import Imu from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.protocol.pubsub.impl.zenohpubsub import Topic as ZenohTopic +from dimos.protocol.pubsub.impl.zenohpubsub import Topic as ZenohTopic, ZenohPubSubBase +from dimos.protocol.rpc.zenohrpc import ZenohRPC from dimos.protocol.service.zenohservice import ZenohConfig, ZenohSessionPool pytestmark = pytest.mark.self_hosted @@ -420,3 +432,140 @@ def test_tf_records_over_zenoh_and_replays_through_python( "base_link", "camera", ] + + +class CdrInteropRustRecorder(RustRecorder): + pose: In[NativePoseStamped] + + +def _stop_owned_process(process): + if process.poll() is None: + process.terminate() + try: + process.wait(timeout=10) + except subprocess.TimeoutExpired: + process.kill() + process.wait(timeout=10) + + +@pytest.mark.parametrize( + "store_kind", + [ + "sqlite", + pytest.param( + "mcap", marks=pytest.mark.skipif(not _MCAP_AVAILABLE, reason="mcap not installed") + ), + ], +) +def test_cdr_native_artifact_replays_raw_payload_with_reception_time( + tmp_path, + rust_recorder_executable, + store_kind, + monkeypatch, + mocker, +): + artifact = tmp_path / ("native.db" if store_kind == "sqlite" else "native.mcap") + store = ( + RustSqliteStoreConfig(path=str(artifact)) + if store_kind == "sqlite" + else RustMcapStoreConfig(path=str(artifact)) + ) + # RPC is unrelated to this storage acceptance; the actual raw pub/sub and + # native process below remain real and bound only to loopback. + mocker.patch.object(ZenohRPC, "start") + mocker.patch.object(ZenohRPC, "serve_module_rpc") + monkeypatch.setattr(global_config, "transport", "zenoh") + endpoint = f"tcp/127.0.0.1:{_free_port()}" + channel = "example/pose/" + uuid.uuid4().hex + with ExitStack() as resources: + recorder = CdrInteropRustRecorder( + source_dir=None, + build_command=None, + executable=str(rust_recorder_executable), + store=store, + record_tf=False, + encoding_threads=2, + session=ZenohConfig( + mode="peer", + connect=[], + listen=[endpoint], + multicast=False, + gossip=False, + connect_timeout=0, + ), + ) + resources.callback(recorder.stop) + recorder.pose.transport = mocker.Mock(spec=PubSubTransport, channel=channel) + specs = recorder._stream_specs() + assert specs[0].codec == "cdr" + recorder._prepare_store(specs) + recorder.config.streams = specs + pool = ZenohSessionPool() + resources.callback(pool.close_all) + publisher = ZenohPubSubBase( + session_pool=pool, + mode="client", + connect=[endpoint], + multicast=False, + gossip=False, + scouting=False, + connect_timeout=5, + ) + resources.callback(publisher.stop) + process = resources.enter_context( + subprocess.Popen( + [str(rust_recorder_executable)], + cwd=_RUST_PACKAGE, + env={ + **os.environ, + "DIMOS_TRANSPORT": "zenoh", + "RUST_LOG": "info,dimos_memory_recorder=debug", + }, + stdin=subprocess.PIPE, + stdout=subprocess.DEVNULL, + stderr=subprocess.PIPE, + ) + ) + resources.callback(_stop_owned_process, process) + assert process.stdin is not None + process.stdin.write(recorder._stdin_blob({"pose": channel})) + process.stdin.close() + _wait_for_log(process, "memory recorder ready") + publisher.start() + expected = NativePoseStamped( + NativeHeader(NativeTime(1_700_000_000, 123_456_789), "map"), + NativePose(NativePoint(1.5, -2.0, 3.0), NativeQuaternion(0.0, 0.0, 0.0, 1.0)), + ) + payload = encode_cdr(expected) + received_after = time.time() + publisher.publish(ZenohTopic(channel), payload) + _wait_for_log(process, "memory recorder batch written") + process.terminate() + assert process.wait(timeout=10) == 0 + memory = ( + SqliteStore(path=str(artifact)) + if store_kind == "sqlite" + else CdrMcapStore(path=str(artifact)) + ) + with memory: + observations = list(memory.stream("pose")) + assert len(observations) == 1 + actual = observations[0] + assert received_after <= actual.ts <= time.time() + assert type(actual.data) is NativePoseStamped + assert type(actual.data.header) is NativeHeader + assert actual.data == expected + assert encode_cdr(actual.data) == payload + if store_kind == "mcap": + # MCAP is optional; this parametrization is skipped when it is absent. + reader_module = importlib.import_module("mcap.reader") + with artifact.open("rb") as source: + records = list(reader_module.make_reader(source).iter_messages()) + assert len(records) == 1 + actual_schema, actual_channel, message = records[0] + assert message.publish_time == message.log_time + assert message.data == payload + assert actual_channel.message_encoding == "cdr" + assert actual_schema.encoding == "ros2msg" + assert actual_schema.name == "geometry_msgs/msg/PoseStamped" + assert actual_schema.data.decode() == specs[0].cdr_schema["data"] diff --git a/dimos/hardware/sensors/camera/realsense/rust/Cargo.toml b/dimos/hardware/sensors/camera/realsense/rust/Cargo.toml index 5bd9e8c3c7e..75aff44563c 100644 --- a/dimos/hardware/sensors/camera/realsense/rust/Cargo.toml +++ b/dimos/hardware/sensors/camera/realsense/rust/Cargo.toml @@ -33,7 +33,7 @@ infrared_left_camera_info = "sensor_msgs.CameraInfo" infrared_right_camera_info = "sensor_msgs.CameraInfo" [dependencies] -dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } realsense-rust = "1.3" realsense-sys = "2.56" diff --git a/dimos/hardware/sensors/camera/v4l2/rust/Cargo.toml b/dimos/hardware/sensors/camera/v4l2/rust/Cargo.toml index 554b02f3566..4981ac593d8 100644 --- a/dimos/hardware/sensors/camera/v4l2/rust/Cargo.toml +++ b/dimos/hardware/sensors/camera/v4l2/rust/Cargo.toml @@ -21,7 +21,7 @@ python = "dimos.hardware.sensors.camera.v4l2.module:V4L2Camera" jpeg_out = "sensor_msgs.CompressedImage" [dependencies] -dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal"] } serde = { version = "1", features = ["derive"] } diff --git a/dimos/hardware/sensors/lidar/livox/rust/Cargo.toml b/dimos/hardware/sensors/lidar/livox/rust/Cargo.toml index 2b042fdd770..3a787626f00 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 = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } serde = { version = "1", features = ["derive"] } serde_json = "1" diff --git a/dimos/hardware/sensors/lidar/pointlio/rust/Cargo.toml b/dimos/hardware/sensors/lidar/pointlio/rust/Cargo.toml index a6cb8b21048..e0837fc7160 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 = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } serde = { version = "1", features = ["derive"] } serde_json = "1" diff --git a/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.toml b/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.toml index 220a2de1080..ae0e2bf2183 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 = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } tokio = { version = "1", features = ["rt-multi-thread", "macros", "time", "net", "sync"] } serde = { version = "1", features = ["derive"] } socket2 = "0.6" diff --git a/dimos/mapping/dim_slam/rust/Cargo.toml b/dimos/mapping/dim_slam/rust/Cargo.toml index 8df62977ea2..72e51072660 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 = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } tokio = { version = "1", features = ["rt-multi-thread", "macros"] } tracing = "0.1" diff --git a/dimos/mapping/ray_tracing/rust/Cargo.toml b/dimos/mapping/ray_tracing/rust/Cargo.toml index e96b69ebefa..b872c506948 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 = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal"] } serde = { version = "1", features = ["derive"] } diff --git a/dimos/memory/codecs/base.py b/dimos/memory/codecs/base.py index a33667e36f5..5fd6342b9c8 100644 --- a/dimos/memory/codecs/base.py +++ b/dimos/memory/codecs/base.py @@ -17,6 +17,8 @@ import importlib from typing import Any, Protocol, TypeVar, runtime_checkable +from dimos.memory.codecs.cdr import CdrCodec + T = TypeVar("T") @@ -38,6 +40,8 @@ def codec_for(payload_type: type[Any] | None = None, sample: Any = None) -> Code from dimos.memory.codecs.pickle import PickleCodec if payload_type is not None: + if hasattr(payload_type, "__msgtype__"): + return CdrCodec(payload_type) from dimos.msgs.sensor_msgs.Image import Image, ImageFormat if issubclass(payload_type, Image): @@ -115,6 +119,8 @@ def _make_one(name: str, payload_module: str, inner: Codec[Any] | None = None) - from dimos.memory.codecs.jpeg import JpegCodec return JpegCodec() + if name == "cdr": + return CdrCodec(resolve_payload_type(payload_module)) if name == "lcm": from dimos.memory.codecs.lcm import LcmCodec diff --git a/dimos/memory/codecs/cdr.py b/dimos/memory/codecs/cdr.py new file mode 100644 index 00000000000..25771667631 --- /dev/null +++ b/dimos/memory/codecs/cdr.py @@ -0,0 +1,42 @@ +# 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. + +"""CDR storage for installed generated values; independent of legacy codecs.""" + +from __future__ import annotations + +from typing import Generic, TypeVar + +from dimos.protocol.pubsub.codec import describe_message + +T = TypeVar("T") + + +class CdrCodec(Generic[T]): + def __init__(self, msg_type: type[T]) -> None: + descriptor = describe_message(msg_type) + if descriptor.encoding != "cdr": + raise TypeError(f"{msg_type!r} is not an installed generated CDR message type") + self._msg_type = msg_type + self._codec = descriptor.codec + + @property + def payload_type(self) -> type[T]: + return self._msg_type + + def encode(self, value: T) -> bytes: + return self._codec.encode(value) + + def decode(self, data: bytes) -> T: + return self._codec.decode(data) diff --git a/dimos/memory/store/cdr_mcap.py b/dimos/memory/store/cdr_mcap.py new file mode 100644 index 00000000000..c1d1225e84d --- /dev/null +++ b/dimos/memory/store/cdr_mcap.py @@ -0,0 +1,255 @@ +# 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. + +"""Read-only CDR MCAP storage using embedded schemas and installed generated types.""" + +from __future__ import annotations + +from collections.abc import Iterator +from dataclasses import replace +from functools import partial +from typing import Any, Protocol, runtime_checkable + +from dimos_cdrgen.registry import message_types, schema as cdr_schema + +from dimos.memory.backend import Backend +from dimos.memory.codecs.base import codec_for +from dimos.memory.codecs.cdr import CdrCodec +from dimos.memory.notifier.subject import SubjectNotifier +from dimos.memory.observationstore.base import ObservationStore, ObservationStoreConfig +from dimos.memory.store.base import Store, StoreConfig +from dimos.memory.type.filter import StreamQuery +from dimos.memory.type.observation import Observation + + +@runtime_checkable +class StreamCodec(Protocol): + """What the store needs to turn a channel's stored bytes into a payload.""" + + @property + def payload_type(self) -> type: ... + + def decode(self, data: bytes) -> Any: ... + + +class _BytesCodec: + """Identity codec: hands back a codecless channel's stored bytes as ``Stream[bytes]``.""" + + payload_type = bytes + + def decode(self, data: bytes) -> bytes: + return data + + +_BYTES_CODEC = _BytesCodec() + + +def _slug(topic: str) -> str: + return topic.removeprefix("rt/").lstrip("/").replace("/", "_") + + +class CdrMcapObservationStoreConfig(ObservationStoreConfig): + name: str = "" + + +class CdrMcapObservationStore(ObservationStore[Any]): + """Read-only metadata/query over one mcap channel. Payloads load lazily.""" + + config: CdrMcapObservationStoreConfig + + def __init__( + self, + *, + name: str, + path: str, + topic: str, + codec: StreamCodec, + count: int, + observation_uses_publish_time: bool, + ) -> None: + super().__init__(name=name) + self._path = path + self._topic = topic + self._codec = codec + self._count = count + # Immutable channel metadata: this store never writes and each iterator + # owns its own file reader, so timestamp selection has no async state. + self._observation_uses_publish_time = observation_uses_publish_time + + @property + def name(self) -> str: + return self.config.name + + def _iter(self, reverse: bool = False) -> Iterator[Observation[Any]]: + from mcap.reader import make_reader # optional mcap dependency + + decode, dtype, n = self._codec.decode, self._codec.payload_type, self._count + with open(self._path, "rb") as f: + msgs = make_reader(f).iter_messages(topics=[self._topic], reverse=reverse) + for i, (_schema, _channel, message) in enumerate(msgs): + observation_time = ( + message.publish_time + if self._observation_uses_publish_time + else message.log_time + ) + yield Observation( + id=(n - 1 - i) if reverse else i, + ts=observation_time / 1e9, + data_type=dtype, + _loader=partial(decode, message.data), + ) + + def query(self, q: StreamQuery) -> Iterator[Observation[Any]]: + # MCAP is natively log-time ordered, so id ordering never needs a sort. + # Native DimOS recordings expose publish_time as observation ts; source + # time can differ from log/reception order and must use the generic sort. + if q.order_field == "id" or ( + q.order_field == "ts" and not self._observation_uses_publish_time + ): + it = self._iter(reverse=q.order_desc) + q = replace(q, order_field=None, order_desc=False) + return q.apply(it) + return q.apply(self._iter()) + + def count(self, q: StreamQuery) -> int: + if not q.filters and q.search_text is None and q.search_vec is None: + n = self._count + if q.offset_val: + n = max(0, n - q.offset_val) + if q.limit_val is not None: + n = min(n, q.limit_val) + return n + return sum(1 for _ in self.query(q)) + + def fetch_by_ids(self, ids: list[int]) -> list[Observation[Any]]: + want = set(ids) + return [o for o in self._iter() if o.id in want] + + def insert(self, obs: Observation[Any]) -> int: + raise NotImplementedError("CdrMcapStore is read-only") + + +class CdrMcapStoreConfig(StoreConfig): + path: str = "" + + +class CdrMcapStore(Store): + """A memory store backed by an mcap file (read-only). + + Only CDR channels are accepted. Unknown schemas are exposed as raw bytes. Names default to + the slugified topic (see :func:`_slug`); ``streams`` (friendly name -> topic) + overrides the name for specific topics. + """ + + config: CdrMcapStoreConfig + + def __init__( + self, + *, + streams: dict[str, str] | None = None, + **kwargs: Any, + ) -> None: + from mcap.reader import make_reader # optional mcap dependency + + super().__init__(**kwargs) + self._codecs: dict[str, StreamCodec] = {} + name_of = {topic: name for name, topic in (streams or {}).items()} # topic -> override + with open(self.config.path, "rb") as f: + summary = make_reader(f).get_summary() + self._stream_topic: dict[str, str] = {} # stream name -> topic + self._available: dict[str, int] = {} # stream name -> message count + self._observation_uses_publish_time: dict[str, bool] = {} + # Channels with no registered codec are still exposed, as Stream[bytes] via + # _BYTES_CODEC — reachable but undecoded. _raw maps their stream name to the + # source schema so summary() can flag them [raw bytes: ]. + self._raw: dict[str, str | None] = {} # raw stream name -> source schema + channel_signatures: dict[str, tuple[Any, ...]] = {} + if summary is not None and summary.statistics is not None: + for cid, ch in summary.channels.items(): + if ch.message_encoding != "cdr": + raise ValueError(f"CdrMcapStore requires CDR channels: {ch.topic!r}") + declared = summary.schemas.get(ch.schema_id) + signature = ( + ch.message_encoding, + (declared.name, declared.encoding, declared.data) if declared else None, + ch.metadata.get("dimos.observation_time"), + ) + if ch.topic in channel_signatures and channel_signatures[ch.topic] != signature: + raise ValueError( + f"Conflicting schemas or timing policies for topic {ch.topic!r}" + ) + channel_signatures[ch.topic] = signature + count = summary.statistics.channel_message_counts.get(cid, 0) + name = name_of.get(ch.topic) or _slug(ch.topic) + taken = self._stream_topic.get(name) + if taken is not None and taken != ch.topic: + raise ValueError( + f"stream {name!r} would name both {taken!r} and {ch.topic!r};" + " pass streams= to name them apart" + ) + if ch.topic not in self._codecs: + # Unknown schemas remain available as raw bytes. + self._codecs[ch.topic] = _BYTES_CODEC + self._raw[name] = declared.name if declared else None + if declared is not None and declared.encoding == "ros2msg": + kind = message_types().get(declared.name) + if kind is not None: + if declared.data.decode("utf-8") != cdr_schema(declared.name): + raise ValueError(f"CDR schema mismatch: {declared.name}") + self._codecs[ch.topic] = CdrCodec(kind) + self._raw.pop(name, None) + self._stream_topic[name] = ch.topic + self._available[name] = self._available.get(name, 0) + count + self._observation_uses_publish_time[name] = ( + ch.metadata.get("dimos.observation_time") == "publish_time" + ) + + def list_streams(self) -> list[str]: + return sorted(set(self._available) | set(self._streams)) + + def summary(self) -> str: + """Base summary, tagging codecless streams with ``[raw bytes: ]``.""" + lines = [] + for name, stream in self.streams.items(): + line = stream.summary() # "Stream(\"name\"): ..." + if name in self._raw: + head = str(stream) # "Stream(\"name\")" + line = f"{head} [raw bytes: {self._raw[name] or '?'}]{line[len(head) :]}" + lines.append(line) + return "\n".join(lines) + + def _create_backend( + self, name: str, payload_type: type | None = None, **config: Any + ) -> Backend[Any]: + if name not in self._available: + raise KeyError(f"No stream {name!r}. Available: {sorted(self._available)}") + topic = self._stream_topic[name] + codec = self._codecs[topic] + ptype = codec.payload_type + obs = CdrMcapObservationStore( + name=name, + path=self.config.path, + topic=topic, + codec=codec, + count=self._available[name], + observation_uses_publish_time=self._observation_uses_publish_time[name], + ) + return Backend( + metadata_store=obs, + codec=codec_for(ptype), # storage codec, unused (blob_store=None) + data_type=ptype, + blob_store=None, + vector_store=None, + notifier=SubjectNotifier(), + ) diff --git a/dimos/memory/store/test_cdr_recording.py b/dimos/memory/store/test_cdr_recording.py new file mode 100644 index 00000000000..aab309c06b3 --- /dev/null +++ b/dimos/memory/store/test_cdr_recording.py @@ -0,0 +1,140 @@ +# 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. + +"""Read-only CDR stores decode self-describing MCAP recordings.""" + +from contextlib import contextmanager + +from dimos_cdrgen.registry import encode, schema +from mcap.writer import Writer +import pytest + +from dimos.memory.codecs.base import codec_for, codec_from_id, codec_id +from dimos.memory.codecs.cdr import CdrCodec +from dimos.memory.store.cdr_mcap import CdrMcapStore +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.dimos_msgs.msg import LineSegment3D, LineSegments3D +from dimos.messages.geometry_msgs.msg import Point +from dimos.messages.std_msgs.msg import Header + + +def message(sec=17): + return LineSegments3D( + header=Header(stamp=Time(sec=sec, nanosec=250_000_000), frame_id="imu"), + segments=[LineSegment3D(Point(1.0, 2.0, 3.0), Point(4.0, 5.0, 6.0), 0.75)], + ) + + +@contextmanager +def mcap_fixture(path): + with path.open("wb") as output: + writer = Writer(output) + writer.start(profile="ros2") + try: + yield writer + finally: + writer.finish() + + +def test_cdr_codec_restores_generated_values(): + expected = message() + codec = codec_for(LineSegments3D) + assert isinstance(codec, CdrCodec) + assert codec_id(codec) == "cdr" + rebuilt = codec_from_id("cdr", f"{LineSegments3D.__module__}.{LineSegments3D.__qualname__}") + assert rebuilt.decode(encode(expected)) == expected + with pytest.raises(TypeError, match="declared type"): + codec.encode(Header(Time(0, 0), "wrong-type")) + with pytest.raises(TypeError, match="Unsupported"): + CdrCodec(str) + + +@pytest.mark.parametrize("payload", [b"malformed", b"\x00\x07\x00\x00" + bytes(4)]) +def test_cdr_storage_rejects_invalid_encapsulation(payload): + with pytest.raises(ValueError, match="XCDR1 encapsulation"): + CdrCodec(LineSegments3D).decode(payload) + + +def test_mismatched_known_schema_is_rejected_before_decoding(tmp_path): + path = tmp_path / "bad.mcap" + with mcap_fixture(path) as writer: + schema_id = writer.register_schema( + LineSegments3D.__msgtype__, "ros2msg", b"int32 changed\n" + ) + channel = writer.register_channel("imu", "cdr", schema_id) + writer.add_message(channel, log_time=1, publish_time=1, data=encode(message())) + with pytest.raises(ValueError, match="CDR schema mismatch"): + CdrMcapStore(path=str(path)) + + +def test_unknown_cdr_schema_never_uses_a_legacy_topic_decoder(tmp_path): + path = tmp_path / "unknown.mcap" + with mcap_fixture(path) as writer: + schema_id = writer.register_schema("future_msgs/msg/New", "ros2msg", b"uint32 value\n") + channel = writer.register_channel("dimos/imu/sensor_msgs.Imu", "cdr", schema_id) + writer.add_message(channel, log_time=1, publish_time=1, data=b"unknown bytes") + with CdrMcapStore(path=str(path)) as store: + observations = list(store.stream("dimos_imu_sensor_msgs.Imu")) + assert observations[0].data == b"unknown bytes" + assert "raw bytes: future_msgs/msg/New" in store.summary() + + +def test_conflicting_channel_schemas_are_rejected(tmp_path): + path = tmp_path / "conflict.mcap" + with mcap_fixture(path) as writer: + for text in (b"uint32 value\n", b"uint64 value\n"): + schema_id = writer.register_schema("future_msgs/msg/New", "ros2msg", text) + channel = writer.register_channel("same", "cdr", schema_id) + writer.add_message(channel, log_time=1, publish_time=1, data=b"") + with pytest.raises(ValueError, match="Conflicting schemas"): + CdrMcapStore(path=str(path)) + + +def test_cdr_store_rejects_non_cdr_channels(tmp_path): + path = tmp_path / "legacy.mcap" + with mcap_fixture(path) as writer: + channel = writer.register_channel("imu", "lcm", 0) + writer.add_message(channel, log_time=1, publish_time=1, data=b"legacy") + with pytest.raises(ValueError, match="requires CDR channels"): + CdrMcapStore(path=str(path)) + + +def test_cdr_store_remaps_topics_and_queries_source_time(tmp_path): + path = tmp_path / "source-order.mcap" + with mcap_fixture(path) as writer: + schema_id = writer.register_schema( + LineSegments3D.__msgtype__, "ros2msg", schema(LineSegments3D.__msgtype__).encode() + ) + channel = writer.register_channel( + "/sensors/segments", + "cdr", + schema_id, + metadata={"dimos.observation_time": "publish_time"}, + ) + for log_ns, sec in ((1, 20), (2, 10)): + writer.add_message( + channel, + log_time=log_ns, + publish_time=sec * 1_000_000_000, + data=encode(message(sec)), + ) + with CdrMcapStore(path=str(path), streams={"segments": "/sensors/segments"}) as store: + stream = store.stream("segments") + assert store.list_streams() == ["segments"] + assert stream.count() == 2 + assert stream.order_by("ts").first().data == message(10) + assert stream.order_by("ts", desc=True).first().data == message(20) + assert stream.order_by("id", desc=True).first().data == message(10) + with pytest.raises(NotImplementedError, match="read-only"): + stream.append(message(30), ts=30) diff --git a/dimos/message_codegen/build.py b/dimos/message_codegen/build.py new file mode 100644 index 00000000000..f10fb51734f --- /dev/null +++ b/dimos/message_codegen/build.py @@ -0,0 +1,122 @@ +# 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. + +"""DimOS source-build inputs; imported only by packaging and preparation tools.""" + +from hashlib import sha256 +import os +from pathlib import Path, PurePosixPath +import shutil +from tempfile import TemporaryDirectory +from typing import Annotated, Literal + +from dimos_cdrgen.source_inputs import prepare_sources +from pydantic import AfterValidator, BaseModel, ConfigDict, Field + +ROOT = Path(__file__).resolve().parents[2] +PROJECT = ROOT / "packages/dimos-generated" + + +def _relative_path(value: str) -> str: + path = PurePosixPath(value) + if not path.parts or path.is_absolute() or ".." in path.parts: + raise ValueError("Unsafe source input path") + return value + + +RelativePath = Annotated[str, AfterValidator(_relative_path)] +Sha256 = Annotated[str, Field(pattern=r"^[0-9a-f]{64}$")] + + +class SourceFile(BaseModel): + model_config = ConfigDict(strict=True, extra="forbid") + + path: RelativePath + sha256: Sha256 + + +class SourceArchive(BaseModel): + model_config = ConfigDict(strict=True, extra="forbid") + + repository: str | None = None + revision: str | None = None + url: str + sha256: Sha256 + prefix: RelativePath + files: dict[RelativePath, SourceFile] + + +class SourceLock(BaseModel): + model_config = ConfigDict(strict=True, extra="forbid") + + format: Literal[1] + sources: list[SourceArchive] + + +def _matches(directory: Path, files: dict[str, str]) -> bool: + return all( + (directory / name).is_file() + and sha256((directory / name).read_bytes()).hexdigest() == digest + for name, digest in files.items() + ) + + +def prepare_builtin_sources( + cache: Path | None = None, *, offline: bool = False, project: Path = PROJECT +) -> Path: + cache = cache or Path( + os.environ.get("DIMOS_MESSAGE_SOURCE_CACHE", ROOT / "build/builtin-source-cache") + ) + lock = project / "schema-sources.json" + data = SourceLock.model_validate_json(lock.read_text()) + upstream = { + name: entry.sha256 for source in data.sources for name, entry in source.files.items() + } + custom_root = project / "custom/dimos_msgs" + custom = { + "dimos_msgs/msg/" + path.name: sha256(path.read_bytes()).hexdigest() + for path in (custom_root / "msg").glob("*.msg") + } + custom["licenses/dimos_msgs-LICENSE"] = sha256( + (custom_root / "LICENSE").read_bytes() + ).hexdigest() + expected = {**upstream, **custom} + target = project / "interfaces" + actual = {str(path.relative_to(target)) for path in target.rglob("*") if path.is_file()} + if actual == expected.keys() and _matches(target, expected): + return target + # Stage both downloaded and local definitions before replacing previous inputs. + with TemporaryDirectory(prefix=".interfaces-", dir=project) as directory: + stage = Path(directory) / "inputs" + if _matches(target, upstream): + stage.mkdir() + for name in upstream: + output = stage / name + output.parent.mkdir(parents=True, exist_ok=True) + shutil.copyfile(target / name, output) + else: + prepare_sources(lock, stage, cache / "archives", offline=offline) + shutil.copytree(custom_root / "msg", stage / "dimos_msgs/msg") + (stage / "licenses").mkdir(exist_ok=True) + shutil.copyfile(custom_root / "LICENSE", stage / "licenses/dimos_msgs-LICENSE") + backup = Path(directory) / "previous" + if target.exists(): + target.rename(backup) + try: + stage.rename(target) + except OSError: + if backup.exists(): + backup.rename(target) + raise + return target diff --git a/dimos/message_codegen/test_build.py b/dimos/message_codegen/test_build.py new file mode 100644 index 00000000000..c2780f1c718 --- /dev/null +++ b/dimos/message_codegen/test_build.py @@ -0,0 +1,160 @@ +# 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. + +"""Source-build preparation is verified, reusable and preserves previous outputs.""" + +from hashlib import sha256 +from io import BytesIO +import json +from pathlib import Path +import shutil +import tarfile + +from pydantic import ValidationError +import pytest + +from dimos.message_codegen.build import prepare_builtin_sources + + +@pytest.fixture +def source_project(tmp_path): + project = tmp_path / "project" + source = project / "custom/dimos_msgs/msg/Reading.msg" + source.parent.mkdir(parents=True) + source.write_text("float64 value\n") + (source.parents[1] / "LICENSE").write_text("Project license\n") + payload = BytesIO() + contents = b"uint32 sec\n" + with tarfile.open(fileobj=payload, mode="w:gz") as archive: + info = tarfile.TarInfo("upstream/stamp_msgs/msg/Stamp.msg") + info.size = len(contents) + archive.addfile(info, BytesIO(contents)) + data = payload.getvalue() + url = "https://example.test/messages.tar.gz" + lock = { + "format": 1, + "sources": [ + { + "url": url, + "sha256": sha256(data).hexdigest(), + "prefix": "upstream/", + "files": { + "stamp_msgs/msg/Stamp.msg": { + "path": "stamp_msgs/msg/Stamp.msg", + "sha256": sha256(contents).hexdigest(), + } + }, + } + ], + } + (project / "schema-sources.json").write_text(json.dumps(lock)) + cache = tmp_path / "cache" + archives = cache / "archives" + archives.mkdir(parents=True) + (archives / (sha256(url.encode()).hexdigest() + ".tar.gz")).write_bytes(data) + return project, cache, data + + +def test_cold_preparation_downloads_verified_inputs(source_project, mocker): + project, cache, data = source_project + shutil.rmtree(cache) + response = mocker.patch("requests.get").return_value.__enter__.return_value + response.content = data + + target = prepare_builtin_sources(cache, project=project) + + assert (target / "stamp_msgs/msg/Stamp.msg").read_text() == "uint32 sec\n" + assert (target / "dimos_msgs/msg/Reading.msg").read_text() == "float64 value\n" + assert (target / "licenses/dimos_msgs-LICENSE").read_text() == "Project license\n" + + +def test_prepared_inputs_and_local_rename_work_offline_without_archives(source_project): + project, cache, _ = source_project + target = prepare_builtin_sources(cache, offline=True, project=project) + shutil.rmtree(cache) + assert prepare_builtin_sources(cache, offline=True, project=project) == target + source = project / "custom/dimos_msgs/msg/Reading.msg" + source.unlink() + source.with_name("Changed.msg").write_text("float64 value\nstring label\n") + + prepare_builtin_sources(cache, offline=True, project=project) + + assert not (target / "dimos_msgs/msg/Reading.msg").exists() + assert (target / "dimos_msgs/msg/Changed.msg").read_text() == "float64 value\nstring label\n" + assert (target / "stamp_msgs/msg/Stamp.msg").read_text() == "uint32 sec\n" + + +def test_bad_source_hash_preserves_prepared_inputs(source_project): + project, cache, _ = source_project + target = prepare_builtin_sources(cache, offline=True, project=project) + path = project / "schema-sources.json" + lock = json.loads(path.read_text()) + lock["sources"][0]["files"]["stamp_msgs/msg/Stamp.msg"]["sha256"] = "0" * 64 + path.write_text(json.dumps(lock)) + + with pytest.raises(ValueError, match="SHA256 mismatch"): + prepare_builtin_sources(cache, offline=True, project=project) + + assert (target / "stamp_msgs/msg/Stamp.msg").read_text() == "uint32 sec\n" + assert (target / "dimos_msgs/msg/Reading.msg").read_text() == "float64 value\n" + + +def test_offline_cold_inputs_fail_without_creating_output(source_project): + project, cache, _ = source_project + shutil.rmtree(cache) + with pytest.raises(FileNotFoundError, match="not cached"): + prepare_builtin_sources(cache, offline=True, project=project) + assert not (project / "interfaces").exists() + + +@pytest.mark.parametrize("invalid", ["format", "path", "hash", "missing", "unknown"]) +def test_invalid_source_lock_preserves_inputs(source_project, invalid): + project, cache, _ = source_project + target = prepare_builtin_sources(cache, offline=True, project=project) + previous = (target / "stamp_msgs/msg/Stamp.msg").read_bytes() + lock_path = project / "schema-sources.json" + lock = json.loads(lock_path.read_text()) + if invalid == "format": + lock["format"] = 2 + elif invalid == "hash": + lock["sources"][0]["sha256"] = "invalid" + elif invalid == "missing": + del lock["sources"][0]["url"] + elif invalid == "unknown": + lock["sources"][0]["sha265"] = "0" * 64 + else: + files = lock["sources"][0]["files"] + files["../outside.msg"] = files.pop("stamp_msgs/msg/Stamp.msg") + lock_path.write_text(json.dumps(lock)) + with pytest.raises(ValidationError): + prepare_builtin_sources(cache, offline=True, project=project) + assert (target / "stamp_msgs/msg/Stamp.msg").read_bytes() == previous + + +def test_failed_publish_restores_previous_inputs(source_project, monkeypatch): + project, cache, _ = source_project + target = prepare_builtin_sources(cache, offline=True, project=project) + source = project / "custom/dimos_msgs/msg/Reading.msg" + source.write_text("string replacement\n") + rename = Path.rename + + def fail_publish(path, destination): + if path.name == "inputs" and destination == target: + raise OSError("publish failed") + return rename(path, destination) + + monkeypatch.setattr(Path, "rename", fail_publish) + with pytest.raises(OSError, match="publish failed"): + prepare_builtin_sources(cache, offline=True, project=project) + assert (target / "dimos_msgs/msg/Reading.msg").read_text() == "float64 value\n" diff --git a/dimos/message_codegen/test_bundled.py b/dimos/message_codegen/test_bundled.py new file mode 100644 index 00000000000..58245504546 --- /dev/null +++ b/dimos/message_codegen/test_bundled.py @@ -0,0 +1,175 @@ +# 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. + +"""Acceptance for built-in messages owned by the main DimOS distribution. + +Build the root wheel and sdist first, then set DIMOS_BUNDLE_DIST and +DIMOS_BUNDLE_WHEELHOUSE. Python dependencies use only that local wheelhouse. Git-source acceptance +also downloads verified schema archives. Web assets follow the existing DimOS build policy. +""" + +import os +from pathlib import Path +import subprocess +import tarfile +import venv +import zipfile + +import pytest + + +def run(*command, cwd, env=None): + result = subprocess.run(command, cwd=cwd, env=env, capture_output=True, text=True, check=False) + assert result.returncode == 0, result.stdout + result.stderr + return result.stdout + + +@pytest.fixture(scope="module") +def artifacts(): + if "DIMOS_BUNDLE_DIST" not in os.environ: + pytest.skip("Set DIMOS_BUNDLE_DIST and DIMOS_BUNDLE_WHEELHOUSE for artifact acceptance") + dist = Path(os.environ["DIMOS_BUNDLE_DIST"]).resolve() + return ( + next(dist.glob("dimos-*.whl")), + next(dist.glob("dimos-*.tar.gz")), + Path(os.environ["DIMOS_BUNDLE_WHEELHOUSE"]).resolve(), + next(dist.glob("dimos-*.whl")).name.split("-")[1], + ) + + +def test_main_wheel_owns_messages_and_schema_closure(artifacts): + wheel, sdist, _, _ = artifacts + with zipfile.ZipFile(wheel) as archive: + names = set(archive.namelist()) + metadata = archive.read(next(n for n in names if n.endswith("/METADATA"))).decode() + assert "Requires-Dist: dimos-generated" not in metadata + assert "dimos/core/module.py" in names + assert "dimos/web/relay_bridge/_relay_dist/relay/main.ts" in names + if os.environ.get("DIMOS_ALLOW_MISSING_COCKPIT") != "1": + assert "dimos/web/relay_bridge/_relay_dist/sdk/dist/sdk.js" in names + assert "dimos/web/relay_bridge/_relay_dist/cockpit/dist/index.html" in names + for license in ( + Path(__file__).resolve().parents[2] / "packages/dimos-generated/interfaces/licenses" + ).glob("*"): + assert any( + ".dist-info/" in name and name.endswith("/" + license.name) for name in names + ) + assert not any(name.startswith("dimos_generated") for name in names) + assert "dimos/messages/std_msgs/msg/__init__.py" in names + assert "dimos/messages_schemas/schemas/std_msgs/msg/Header.msg" in names + assert "dimos/messages_schemas/schemas/builtin_interfaces/msg/Time.msg" in names + assert "dimos/messages_schemas/package/cpp/CMakeLists.txt" in names + assert "dimos/messages_schemas/package/rust/Cargo.toml" in names + with tarfile.open(sdist) as archive: + names = {name.split("/", 1)[-1] for name in archive.getnames()} + assert "packages/dimos-generated/interfaces/std_msgs/msg/Header.msg" in names + assert "packages/dimos-generated/interfaces/licenses/dimos_msgs-LICENSE" in names + + +@pytest.mark.parametrize("mode", ["wheel", "source", "editable", "git"]) +def test_installed_messages_and_external_header_identity(tmp_path, artifacts, mode): + wheel, sdist, wheelhouse, version = artifacts + environment = tmp_path / "venv" + venv.EnvBuilder(with_pip=True).create(environment) + python = str(environment / "bin/python") + pip = [python, "-m", "pip", "install", "--no-index", "--find-links", str(wheelhouse)] + run(*pip, "dimos-cdrgen==0.1.2", "setuptools>=70", "wheel", "pybind11", cwd=tmp_path) + if mode == "git": + root = Path(__file__).resolve().parents[2] + head = run("git", "rev-parse", "HEAD", cwd=root).strip() + source = f"git+{root.as_uri()}@{head}" + elif mode == "wheel": + source = wheel + else: + with tarfile.open(sdist) as archive: + archive.extractall(tmp_path / "source", filter="data") + source = next((tmp_path / "source").iterdir()) + env = {**os.environ, "DIMOS_OFFLINE": "0" if mode == "git" else "1"} + run( + *pip, + "--no-deps", + *(["-e"] if mode == "editable" else []), + str(source), + cwd=tmp_path, + env=env, + ) + # The wheel demo deliberately rejects editable paths outside site-packages. + if mode != "editable": + example = ( + Path(__file__).resolve().parents[2] / "examples/message-package/demo_installed_wheel.py" + ) + output = run(python, "-I", str(example), cwd=tmp_path) + assert "Messages: 73 types" in output + assert "PASS: dimos_msgs/msg/LineSegments3D" in output + project = tmp_path / "external" + definition = project / "interfaces/demo_msgs/msg/Reading.msg" + definition.parent.mkdir(parents=True) + definition.write_text("std_msgs/Header header\nfloat64 value\n") + (project / "pyproject.toml").write_text( + """[build-system] +requires = ["setuptools>=70", "wheel", "dimos-cdrgen==0.1.2", "dimos==0.0.14"] +build-backend = "setuptools.build_meta" +[project] +name = "external-reading" +version = "0.1.0" +dependencies = ["dimos==0.0.14", "dimos-cdrgen==0.1.2"] +[tool.dimos.messages] +python-module = "my_robot.messages" +[tool.dimos.messages.dependencies] +"dimos.messages" = "0.0.14" +""".replace("0.0.14", version) + ) + (project / "setup.py").write_text( + "from pathlib import Path\nfrom setuptools import setup\n" + "from dimos_cdrgen.source_package import setup_arguments\n" + "setup(**setup_arguments(Path(__file__).resolve().parent))\n" + ) + run(*pip, "--no-deps", "--no-build-isolation", str(project), cwd=tmp_path) + run( + python, + "-I", + "-c", + """from importlib.metadata import distribution, entry_points, PackageNotFoundError +import json +from importlib.util import find_spec +from dimos.messages.std_msgs.msg import Header +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages_schemas.provider import message_types, package_root +from my_robot.messages.demo_msgs.msg import Reading +from my_robot.messages_schemas.provider import package_root as external_root +from dimos_cdrgen.registry import encode, decode +assert distribution("dimos").version == "0.0.14" +assert find_spec("dimos_generated") is None +try: + distribution("dimos-generated") +except PackageNotFoundError: + pass +else: + raise AssertionError("Built-ins must have only the DimOS distribution owner") +assert len(message_types()) == 73 +assert len([ep for ep in entry_points(group="dimos.messages") if ep.name == "dimos"]) == 1 +header = Header(Time(17, 23), "map") +value = Reading(header=header, value=21.5) +assert type(value.header) is Header +assert type(decode(encode(value), Reading).header) is Header +assert decode(encode(value), Reading) == value +metadata = json.loads((package_root() / "message-package.json").read_text()) +assert metadata["version"] == "0.0.14" +assert (external_root() / "schemas/std_msgs/msg/Header.msg").is_file() +assert (external_root() / "schemas/builtin_interfaces/msg/Time.msg").is_file() +assert not (external_root() / "cpp/std_msgs").exists() +assert all("dimos-generated" not in req for req in distribution("external-reading").requires) +""".replace("0.0.14", version), + cwd=tmp_path, + ) diff --git a/dimos/message_codegen/test_integration.py b/dimos/message_codegen/test_integration.py index cbef553c448..e4eb5602b0e 100644 --- a/dimos/message_codegen/test_integration.py +++ b/dimos/message_codegen/test_integration.py @@ -15,13 +15,11 @@ """Verify the DimOS adapter and one installed consumer of published cdrgen.""" import json -import os from pathlib import Path import subprocess import sys from types import SimpleNamespace -from dimos_cdrgen.generate import generate import pytest from dimos.message_codegen import native_build @@ -62,12 +60,12 @@ def test_installed_message_consumer(tmp_path: Path) -> None: source.parent.mkdir(parents=True) source.write_text("int32 value\n") (project / "pyproject.toml").write_text("""[build-system] -requires = ["setuptools>=77", "wheel", "dimos-cdrgen==0.1.0"] +requires = ["setuptools>=77", "wheel", "dimos-cdrgen==0.1.2"] build-backend = "setuptools.build_meta" [project] name = "dimos-cdr-consumer" version = "0.1.0" -dependencies = ["dimos-cdrgen==0.1.0"] +dependencies = ["dimos-cdrgen==0.1.2"] [tool.dimos.messages] source = "interfaces" """) @@ -108,87 +106,3 @@ def test_installed_message_consumer(tmp_path: Path) -> None: """, cwd=tmp_path, ) - - -@pytest.fixture(scope="session") -def native_build_python(tmp_path_factory: pytest.TempPathFactory) -> Path: - # Native generator prerequisites are build tools, isolated from DimOS runtime deps. - # In particular, cdrgen0.1.0's PyYAML pin must not downgrade the runtime lock. - environment = tmp_path_factory.mktemp("cdr-native-toolchain") / "venv" - run("uv", "venv", "--python", sys.executable, str(environment), cwd=environment.parent) - python = environment / ("Scripts/python.exe" if os.name == "nt" else "bin/python") - offline_args = ("--offline",) if os.environ.get("DIMOS_OFFLINE") == "1" else () - run( - "uv", - "pip", - "install", - *offline_args, - "--python", - str(python), - "dimos-cdrgen[native]==0.1.0", - cwd=environment.parent, - ) - return python - - -@pytest.mark.native_e2e -def test_cpp_consumer_uses_dimos_header(tmp_path: Path, native_build_python: Path) -> None: - source = tmp_path / "interfaces/consumer_msgs/msg/Tick.msg" - source.parent.mkdir(parents=True) - source.write_text("int32 value\n") - generated = tmp_path / "generated" - generate( - [source.parents[2]], generated, ["consumer_msgs/msg/Tick"], "dimos_consumer", shared=True - ) - prefix_file = tmp_path / "native-prefix.txt" - run( - str(native_build_python), - "-c", - """import json -import os -from pathlib import Path -import sys -from dimos_cdrgen.native_build import prepare_cpp -sources = {name: Path(path) for name, path in json.loads(os.environ.get("DIMOS_NATIVE_SOURCE_DIRS", "{}")).items()} -prefix = prepare_cpp( - Path(sys.argv[1]), cache=Path(sys.argv[2]), - offline=os.environ.get("DIMOS_OFFLINE") == "1", source_dirs=sources or None, -) -Path(sys.argv[3]).write_text(str(prefix)) -""", - str(generated), - os.environ.get("DIMOS_NATIVE_TEST_CACHE", str(tmp_path / "native-cache")), - str(prefix_file), - cwd=tmp_path, - ) - prefix = Path(prefix_file.read_text()) - toolchain = native_build.write_cmake_toolchain(prefix, tmp_path / "toolchain.cmake") - include = Path(__file__).resolve().parents[2] / "native/cpp/include" - (tmp_path / "CMakeLists.txt").write_text(f'''cmake_minimum_required(VERSION 3.20) -project(dimos_cdr_consumer LANGUAGES CXX) -find_package(dimos_consumer CONFIG REQUIRED) -add_executable(consumer main.cpp) -target_compile_features(consumer PRIVATE cxx_std_17) -target_include_directories(consumer PRIVATE "{include}") -target_link_libraries(consumer PRIVATE dimos_consumer::messages) -''') - (tmp_path / "main.cpp").write_text("""#include -#include -#include -int main() { - const std::vector expected{0, 1, 0, 0, 17, 0, 0, 0}; - auto value = dimos::native::cdr_decode(expected); - return value.value == 17 && dimos::native::cdr_encode(value) == expected ? 0 : 1; -} -""") - run( - "cmake", - "-S", - str(tmp_path), - "-B", - str(tmp_path / "build"), - f"-DCMAKE_TOOLCHAIN_FILE={toolchain}", - cwd=tmp_path, - ) - run("cmake", "--build", str(tmp_path / "build"), "--parallel", "2", cwd=tmp_path) - run(str(tmp_path / "build/consumer"), cwd=tmp_path) diff --git a/dimos/message_codegen/test_project_install.py b/dimos/message_codegen/test_project_install.py new file mode 100644 index 00000000000..cf59f66ef55 --- /dev/null +++ b/dimos/message_codegen/test_project_install.py @@ -0,0 +1,170 @@ +# 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. + +"""Real isolated pip source/wheel/sdist/editable acceptance for message authors.""" + +import os +from pathlib import Path +import shutil +import subprocess +from tempfile import TemporaryDirectory +import venv + +import pytest + +ROOT = Path(__file__).resolve().parents[2] + + +def run(python, *arguments, cwd, env): + result = subprocess.run( + [str(python), *arguments], check=False, cwd=cwd, env=env, capture_output=True, text=True + ) + assert result.returncode == 0, result.stdout + result.stderr + return result.stdout + + +@pytest.fixture(scope="module") +def installed_toolchain(tmp_path_factory): + location = os.environ.get("DIMOS_EXTERNAL_WHEELHOUSE") + if not location: + pytest.skip("Set DIMOS_EXTERNAL_WHEELHOUSE to the complete review wheelhouse") + wheelhouse = Path(location).resolve() + parent = Path(os.environ.get("DIMOS_EXTERNAL_TEST_ROOT", tmp_path_factory.getbasetemp())) + parent.mkdir(parents=True, exist_ok=True) + with TemporaryDirectory(prefix="message-install-", dir=parent) as directory: + folder = Path(directory) + environment = folder / "venv" + venv.EnvBuilder(with_pip=True).create(environment) + python = environment / "bin/python" + temporary = folder / "pip-tmp" + temporary.mkdir() + env = { + **os.environ, + "PIP_NO_INDEX": "1", + "PIP_FIND_LINKS": str(wheelhouse), + "TMPDIR": str(temporary), + } + env.pop("PYTHONPATH", None) + run(python, "-m", "pip", "install", "dimos==0.0.14", cwd=folder, env=env) + forbidden = folder / "no-native-tools" + forbidden.mkdir() + for name in ("cmake", "cargo", "rustc", "c++", "g++", "cc"): + script = forbidden / name + script.write_text("#!/bin/sh\nexit 97\n") + script.chmod(0o755) + env.update( + PATH=str(forbidden) + os.pathsep + str(environment / "bin") + os.pathsep + env["PATH"], + CC="/bin/false", + CXX="/bin/false", + ) + yield python, env, folder + + +@pytest.mark.parametrize("mode", ["source", "wheel", "sdist", "editable"]) +def test_clean_isolated_message_install_and_builtin_identity(installed_toolchain, tmp_path, mode): + python, env, _ = installed_toolchain + project = tmp_path / "project" + shutil.copytree(ROOT / "examples/external-messages", project) + source = project + if mode == "wheel": + run( + python, + "-m", + "pip", + "wheel", + "--no-deps", + "--wheel-dir", + str(tmp_path / "dist"), + ".", + cwd=project, + env=env, + ) + source = next((tmp_path / "dist").glob("acme_telemetry-*.whl")) + elif mode == "sdist": + run( + python, + "-c", + "from setuptools.build_meta import build_sdist; build_sdist('dist')", + cwd=project, + env=env, + ) + source = next((project / "dist").glob("*.tar.gz")) + run( + python, + "-m", + "pip", + "install", + "--force-reinstall", + "--no-deps", + *(["-e"] if mode == "editable" else []), + str(source), + cwd=tmp_path, + env=env, + ) + run( + python, + "-I", + "-c", + """from importlib.metadata import entry_points +from acme_telemetry.telemetry_msgs.msg import DeviceReading +from acme_telemetry_schemas.provider import package_root +from dimos.messages.std_msgs.msg import Header +from dimos.messages.builtin_interfaces.msg import Time +from dimos_cdrgen.registry import encode, decode +value = DeviceReading(Header(Time(17, 23), "map"), 7, 2.5, "sensor") +assert type(value.header) is Header +assert decode(encode(value), DeviceReading) == value +assert type(decode(encode(value), DeviceReading).header) is Header +assert len([ep for ep in entry_points(group='dimos.messages') if ep.name == 'acme_telemetry']) == 1 +assert (package_root() / 'schemas/std_msgs/msg/Header.msg').is_file() +assert (package_root() / 'cpp/CMakeLists.txt').is_file() +assert (package_root() / 'rust/Cargo.toml').is_file() +""", + cwd=tmp_path, + env=env, + ) + output = run(python, "-I", str(project / "demo_reading.py"), cwd=tmp_path, env=env) + assert "PASS: external message registered" in output + run(python, "-m", "pip", "check", cwd=tmp_path, env=env) + if mode == "editable": + definition = project / "interfaces/telemetry_msgs/msg/DeviceReading.msg" + definition.write_text(definition.read_text() + "uint32 status\n") + run(python, "-m", "pip", "install", "--no-deps", "-e", str(project), cwd=tmp_path, env=env) + run( + python, + "-I", + "-c", + """from acme_telemetry.telemetry_msgs.msg import DeviceReading +from dimos.messages.std_msgs.msg import Header +from dimos.messages.builtin_interfaces.msg import Time +value = DeviceReading(Header(Time(0, 0), 'map'), 7, 2.5, 'sensor', 9) +assert value.status == 9 +""", + cwd=tmp_path, + env=env, + ) + definition.rename(definition.with_name("RenamedReading.msg")) + run(python, "-m", "pip", "install", "--no-deps", "-e", str(project), cwd=tmp_path, env=env) + run( + python, + "-I", + "-c", + """from acme_telemetry.telemetry_msgs import msg +assert hasattr(msg, 'RenamedReading') +assert not hasattr(msg, 'DeviceReading') +""", + cwd=tmp_path, + env=env, + ) + assert not (project / "build/message-source/rust/target").exists() diff --git a/dimos/message_utils/__init__.py b/dimos/message_utils/__init__.py new file mode 100644 index 00000000000..e69de29bb2d diff --git a/dimos/message_utils/camera_info.py b/dimos/message_utils/camera_info.py new file mode 100644 index 00000000000..c8040990805 --- /dev/null +++ b/dimos/message_utils/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/message_utils/geometry.py b/dimos/message_utils/geometry.py new file mode 100644 index 00000000000..2ab85421267 --- /dev/null +++ b/dimos/message_utils/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/message_utils/image.py b/dimos/message_utils/image.py new file mode 100644 index 00000000000..16548a5f175 --- /dev/null +++ b/dimos/message_utils/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/message_utils/pointcloud.py b/dimos/message_utils/pointcloud.py new file mode 100644 index 00000000000..fed75827913 --- /dev/null +++ b/dimos/message_utils/pointcloud.py @@ -0,0 +1,387 @@ +# 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 +from dataclasses import replace +import math +from typing import Any + +import numpy as np +from numpy.typing import NDArray +import open3d as o3d # type: ignore[import-untyped] + +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"} + + +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 deepcopy(second) + if not second.width * second.height: + return deepcopy(first) + 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 = replace(message, header=deepcopy(message.header), fields=deepcopy(message.fields)) + 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.float32) + if depth.encoding != "32FC1": + distances *= depth_scale + distances[~np.isfinite(distances) | (distances <= 0)] = 0 + rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth( + o3d.geometry.Image(np.ascontiguousarray(image_to_rgb(color))), + o3d.geometry.Image(np.ascontiguousarray(distances)), + depth_scale=1.0, + depth_trunc=depth_trunc, + convert_rgb_to_intensity=False, + ) + intrinsics = o3d.camera.PinholeCameraIntrinsic( + depth.width, + depth.height, + matrix[0, 0], + matrix[1, 1], + matrix[0, 2], + matrix[1, 2], + ) + cloud = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd, intrinsics) + colors = np.rint(np.asarray(cloud.colors) * 255).astype(np.uint8) + return pointcloud_from_xyz_rgb(np.asarray(cloud.points), colors, 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/message_utils/test_camera_info.py b/dimos/message_utils/test_camera_info.py new file mode 100644 index 00000000000..bcef99230c3 --- /dev/null +++ b/dimos/message_utils/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.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" + + +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/message_utils/test_geometry.py b/dimos/message_utils/test_geometry.py new file mode 100644 index 00000000000..a26d0a07982 --- /dev/null +++ b/dimos/message_utils/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.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, + 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 + + +@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/message_utils/test_image_compression.py b/dimos/message_utils/test_image_compression.py new file mode 100644 index 00000000000..0514d681424 --- /dev/null +++ b/dimos/message_utils/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.message_utils.image import ( + compressed_image_from_image, + image_from_array, + image_from_compressed, + image_to_bgr, + image_to_rgb, + image_view, +) +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() +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/message_utils/test_image_views.py b/dimos/message_utils/test_image_views.py new file mode 100644 index 00000000000..a4d0680bcaf --- /dev/null +++ b/dimos/message_utils/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.message_utils.image import ( + image_brightness, + image_from_array, + image_from_file, + image_sharpness, + image_to_jpeg, + image_to_rgb, + image_view, +) +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: + 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/message_utils/test_pointcloud.py b/dimos/message_utils/test_pointcloud.py new file mode 100644 index 00000000000..48411b23d56 --- /dev/null +++ b/dimos/message_utils/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.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, + pointcloud_from_xyz_rgb, + pointcloud_rgb, + pointcloud_stamps, + pointcloud_view, + pointcloud_xyz, + select_points, + 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"]) +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/message_utils/test_pointcloud_colors.py b/dimos/message_utils/test_pointcloud_colors.py new file mode 100644 index 00000000000..4043a764772 --- /dev/null +++ b/dimos/message_utils/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.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 + + +@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/message_utils/test_time.py b/dimos/message_utils/test_time.py new file mode 100644 index 00000000000..9a5f2e8da5a --- /dev/null +++ b/dimos/message_utils/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.message_utils.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.message_utils.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/message_utils/time.py b/dimos/message_utils/time.py new file mode 100644 index 00000000000..3d9197cb58a --- /dev/null +++ b/dimos/message_utils/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/navigation/global_planner/mls_planner/rust/Cargo.toml b/dimos/navigation/global_planner/mls_planner/rust/Cargo.toml index 9f47c6ad8fa..bb04033aac2 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 = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal", "time"] } serde = { version = "1", features = ["derive"] } diff --git a/dimos/navigation/local_planner/rust/Cargo.toml b/dimos/navigation/local_planner/rust/Cargo.toml index 6d62af49fd7..5e747841873 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 = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", optional = true, version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen", optional = true } tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal", "time"], optional = true } tracing = { version = "0.1", optional = true } diff --git a/dimos/navigation/trajectory_follower/fancy/rust/Cargo.toml b/dimos/navigation/trajectory_follower/fancy/rust/Cargo.toml index ad24d3e149e..9baf7d0d17b 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 = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", optional = true, version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen", optional = true } tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal", "time"], optional = true } tracing = { version = "0.1", optional = true } diff --git a/dimos/navigation/trajectory_follower/fancy/rust/flake.lock b/dimos/navigation/trajectory_follower/fancy/rust/flake.lock index 9a4b4df94e3..eeb07ccc345 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": 1792745145, + "narHash": "sha256-cJLM/uV6rBI3BAo0B8Tn/F225s48HdNIu7cKaLLC7PE=", "owner": "dimensionalOS", "repo": "dimos", - "rev": "b173fa6dcd3d7046429e6405b3826f8b96952cf8", + "rev": "e5c07673474aabf2256f53fd660511514610b91e", "type": "github" }, "original": { diff --git a/dimos/perception/depth2depth_cloud/rust/Cargo.toml b/dimos/perception/depth2depth_cloud/rust/Cargo.toml index 08b0649468f..5f32e3bb96d 100644 --- a/dimos/perception/depth2depth_cloud/rust/Cargo.toml +++ b/dimos/perception/depth2depth_cloud/rust/Cargo.toml @@ -30,7 +30,7 @@ tf = "tf2_msgs.TFMessage" depth_cloud = "sensor_msgs.PointCloud2" [dependencies] -dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443" } +dimos-module = { git = "https://github.com/dimensionalOS/dimos", rev = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal"] } serde = { version = "1", features = ["derive"] } diff --git a/dimos/protocol/pubsub/codec.py b/dimos/protocol/pubsub/codec.py new file mode 100644 index 00000000000..811b9450571 --- /dev/null +++ b/dimos/protocol/pubsub/codec.py @@ -0,0 +1,91 @@ +# 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. + +"""Message codecs selected once from the declared message type.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Any, Generic, Literal, Protocol, TypeVar, cast + +from dimos_cdrgen.registry import decode, encode, message_types + +from dimos.msgs.protocol import DimosMsg + +T = TypeVar("T") + + +class MessageCodec(Protocol[T]): + def encode(self, message: T) -> bytes: ... + + def decode(self, payload: bytes) -> T: ... + + +@dataclass(frozen=True) +class LCMCodec(Generic[T]): + message_type: type[T] + + def encode(self, message: T) -> bytes: + if not isinstance(message, self.message_type): + raise TypeError("Message does not match the declared type") + return cast("DimosMsg", message).lcm_encode() + + def decode(self, payload: bytes) -> T: + return cast("T", cast("type[DimosMsg]", self.message_type).lcm_decode(payload)) + + +@dataclass(frozen=True) +class CDRCodec(Generic[T]): + message_type: type[T] + + def encode(self, message: T) -> bytes: + if type(message) is not self.message_type: + raise TypeError("Message does not match the declared type") + return encode(message) + + def decode(self, payload: bytes) -> T: + if len(payload) < 4 or payload[:2] not in (b"\x00\x00", b"\x00\x01"): + raise ValueError("Expected standalone XCDR1 encapsulation") + return decode(payload, self.message_type) + + +@dataclass(frozen=True) +class MessageDescriptor(Generic[T]): + message_type: type[T] + encoding: Literal["lcm", "cdr"] + name: str + codec: MessageCodec[T] + + +def describe_message(message_type: type[T]) -> MessageDescriptor[T]: + legacy = all( + callable(getattr(message_type, method, None)) for method in ("lcm_encode", "lcm_decode") + ) + if legacy and not hasattr(message_type, "__msgtype__"): + name = getattr(message_type, "msg_name", None) + if not isinstance(name, str): + raise TypeError("Legacy message type must declare msg_name") + return MessageDescriptor(message_type, "lcm", name, LCMCodec(message_type)) + if not hasattr(message_type, "__msgtype__"): + raise TypeError(f"Unsupported message type: {message_type!r}") + names = [name for name, cls in message_types().items() if cls is message_type] + if (names and legacy) or len(names) > 1: + raise TypeError("Ambiguous message encoding") + if names: + return MessageDescriptor(message_type, "cdr", names[0], CDRCodec(message_type)) + if legacy and isinstance(getattr(message_type, "msg_name", None), str): + return MessageDescriptor( + message_type, "lcm", cast("Any", message_type).msg_name, LCMCodec(message_type) + ) + raise TypeError(f"Unsupported message type: {message_type!r}") diff --git a/dimos/protocol/pubsub/encoders.py b/dimos/protocol/pubsub/encoders.py index ff5c78a2f52..bfd2f217eff 100644 --- a/dimos/protocol/pubsub/encoders.py +++ b/dimos/protocol/pubsub/encoders.py @@ -18,9 +18,9 @@ from abc import ABC, abstractmethod import pickle -from typing import TYPE_CHECKING, Generic, Protocol, TypeVar, cast +from typing import TYPE_CHECKING, Any, Generic, Protocol, TypeVar, cast -from dimos.msgs.protocol import DimosMsg +from dimos.protocol.pubsub.codec import MessageDescriptor if TYPE_CHECKING: from collections.abc import Callable @@ -33,8 +33,6 @@ class DecodingError(Exception): """Raised by decode() to skip a message without calling the callback.""" - pass - class PubSubEncoderMixin(Generic[TopicT, MsgT, EncodingT], ABC): """Mixin that encodes messages before publishing and decodes them after receiving. @@ -62,6 +60,9 @@ def encode(self, msg: MsgT, topic: TopicT) -> EncodingT: ... @abstractmethod def decode(self, msg: EncodingT, topic: TopicT) -> MsgT: ... + def decode_received(self, payload: EncodingT, declared: TopicT, received: TopicT) -> MsgT: + return self.decode(payload, received) + def publish(self, topic: TopicT, message: MsgT) -> None: """Encode the message and publish it.""" encoded_message = self.encode(message, topic) @@ -74,9 +75,11 @@ def subscribe( ) -> Callable[[], None]: """Subscribe with automatic decoding.""" + declared_topic = topic + def wrapper_cb(encoded_data: EncodingT, topic: TopicT) -> None: try: - decoded_message = self.decode(encoded_data, topic) + decoded_message = self.decode_received(encoded_data, declared_topic, topic) except DecodingError: return callback(decoded_message, topic) @@ -95,32 +98,64 @@ def decode(self, msg: bytes, _: TopicT) -> MsgT: class LCMTopicProto(Protocol): - """Protocol for topics usable with LCM encoders.""" + """Compatibility interface for legacy encoders such as JPEG.""" + + topic: str + lcm_type: type[Any] | None - topic: str # At decode time, always concrete string - lcm_type: type[DimosMsg] | None +class MessageTopic(Protocol): + @property + def message(self) -> MessageDescriptor[Any] | None: ... -class LCMEncoderMixin(PubSubEncoderMixin[LCMTopicProto, DimosMsg, bytes]): - """Encoder mixin for DimosMsg using LCM binary encoding.""" + +class MessageEncoderMixin(PubSubEncoderMixin[MessageTopic, Any, bytes]): + """Apply the topic's bound codec without changing transport behavior.""" def subscribe( - self, topic: LCMTopicProto, callback: Callable[[DimosMsg, LCMTopicProto], None] + self, topic: MessageTopic, callback: Callable[[Any, MessageTopic], None] ) -> Callable[[], None]: - # Import the type's heavy decode dependencies now, on the subscriber's - # thread. Left to the first decode, the import would run on the LCM - # handler thread and block it for seconds, dropping messages meanwhile. - warmup = getattr(topic.lcm_type, "lcm_warmup", None) - if warmup is not None: - warmup() + descriptor = topic.message + if descriptor is not None and descriptor.encoding == "lcm": + warmup = getattr(descriptor.message_type, "lcm_warmup", None) + if warmup is not None: + warmup() + return super().subscribe(topic, callback) - def encode(self, msg: DimosMsg | bytes, _: LCMTopicProto) -> bytes: - if isinstance(msg, bytes): + def decode_received( + self, payload: bytes, declared: MessageTopic, received: MessageTopic + ) -> Any: + descriptor = declared.message + actual = received.message + if descriptor is not None and descriptor.encoding == "cdr": + if ( + actual is None + or actual.encoding != "cdr" + or actual.message_type is not descriptor.message_type + ): + raise DecodingError("Message metadata does not match the declared type") + return self.decode(payload, declared) + if descriptor is not None and actual is not None and actual.encoding != descriptor.encoding: + raise DecodingError("Message encoding does not match the declared type") + return self.decode(payload, received) + + def encode(self, msg: Any, topic: MessageTopic) -> bytes: + descriptor = topic.message + if isinstance(msg, bytes) and (descriptor is None or descriptor.encoding == "lcm"): return msg - return msg.lcm_encode() + if descriptor is None: + raise TypeError("Typed publication requires a declared message type") + return descriptor.codec.encode(msg) + + def decode(self, msg: bytes, topic: MessageTopic) -> Any: + if topic.message is None: + raise DecodingError("Typed subscription requires a declared message type") + try: + return topic.message.codec.decode(msg) + except ValueError as error: + raise DecodingError(str(error)) from error + - def decode(self, msg: bytes, topic: LCMTopicProto) -> DimosMsg: - if topic.lcm_type is None: - raise DecodingError(f"Cannot decode: topic {topic.topic!r} has no lcm_type") - return topic.lcm_type.lcm_decode(msg) +# Keep existing imports working while callers migrate to the generic name. +LCMEncoderMixin = MessageEncoderMixin diff --git a/dimos/protocol/pubsub/impl/lcmpubsub.py b/dimos/protocol/pubsub/impl/lcmpubsub.py index f47280f35cb..fdeaca2fc28 100644 --- a/dimos/protocol/pubsub/impl/lcmpubsub.py +++ b/dimos/protocol/pubsub/impl/lcmpubsub.py @@ -15,14 +15,15 @@ from __future__ import annotations from collections.abc import Callable -from dataclasses import dataclass +from dataclasses import dataclass, field import re import threading from typing import Any from dimos.msgs.protocol import DimosMsg +from dimos.protocol.pubsub.codec import MessageDescriptor, describe_message from dimos.protocol.pubsub.encoders import ( - LCMEncoderMixin, + MessageEncoderMixin, PickleEncoderMixin, ) from dimos.protocol.pubsub.patterns import Glob @@ -36,7 +37,21 @@ @dataclass class Topic: topic: str | re.Pattern[str] | Glob - lcm_type: type[DimosMsg] | None = None + lcm_type: type[Any] | None = None + message_type: type[Any] | None = field(default=None, kw_only=True) + message: MessageDescriptor[Any] | None = field(init=False, default=None, repr=False) + + def __post_init__(self) -> None: + if ( + self.lcm_type is not None + and self.message_type is not None + and self.lcm_type is not self.message_type + ): + raise TypeError("Conflicting declared message types") + declared = self.message_type if self.message_type is not None else self.lcm_type + self.message = describe_message(declared) if declared is not None else None + self.message_type = declared + self.lcm_type = declared @property def is_pattern(self) -> bool: @@ -51,9 +66,10 @@ def pattern(self) -> str: return self.topic def __str__(self) -> str: - if self.lcm_type is None: + if self.message is None or self.message.encoding == "cdr": return self.pattern - return f"{self.pattern}#{self.lcm_type.msg_name}" + assert self.message is not None + return f"{self.pattern}#{self.message.name}" @staticmethod def from_channel_str(channel: str, default_lcm_type: type[DimosMsg] | None = None) -> Topic: @@ -64,6 +80,9 @@ def from_channel_str(channel: str, default_lcm_type: type[DimosMsg] | None = Non """ from dimos.msgs.helpers import resolve_msg_type + if default_lcm_type is not None and describe_message(default_lcm_type).encoding == "cdr": + return Topic(channel, message_type=default_lcm_type) + if "#" not in channel: return Topic(topic=channel, lcm_type=default_lcm_type) @@ -149,7 +168,7 @@ def unsubscribe() -> None: class LCM( # type: ignore[misc] - LCMEncoderMixin, + MessageEncoderMixin, LCMPubSubBase, ): ... diff --git a/dimos/protocol/pubsub/impl/test_lcmpubsub.py b/dimos/protocol/pubsub/impl/test_lcmpubsub.py index 4ab7030b107..9290065dcf6 100644 --- a/dimos/protocol/pubsub/impl/test_lcmpubsub.py +++ b/dimos/protocol/pubsub/impl/test_lcmpubsub.py @@ -18,6 +18,8 @@ import pytest +from dimos.core.transport import LCMTransport +from dimos.messages.std_msgs.msg import Int32 from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -184,3 +186,28 @@ def test_lcm_geometry_msgs_autopickle_pubsub(test_message: Any, pickle_lcm: Pick assert isinstance(received_topic, Topic) assert received_topic == topic + + +def test_generated_message_transport_and_unsubscribe(): + transport = LCMTransport("codec-check", Int32, url="memq://") + received = [] + delivered = threading.Event() + + def receive(value): + received.append(value) + delivered.set() + + try: + unsubscribe = transport.subscribe(receive) + LCMPubSubBase.publish(transport.lcm, transport.topic, b"broken") + transport.broadcast(None, Int32(42)) + assert delivered.wait(3) + assert received == [Int32(42)] + unsubscribe() + delivered.clear() + transport.subscribe(lambda value: delivered.set()) + transport.broadcast(None, Int32(43)) + assert delivered.wait(3) + assert received == [Int32(42)] + finally: + transport.stop() diff --git a/dimos/protocol/pubsub/impl/test_zenohpubsub.py b/dimos/protocol/pubsub/impl/test_zenohpubsub.py index d1c6d03f1e5..cc43ff71d35 100644 --- a/dimos/protocol/pubsub/impl/test_zenohpubsub.py +++ b/dimos/protocol/pubsub/impl/test_zenohpubsub.py @@ -27,6 +27,8 @@ import zenoh +from dimos.core.transport import ZenohTransport +from dimos.messages.std_msgs.msg import Int32 from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.sensor_msgs.Image import Image from dimos.protocol.pubsub.impl.lcmpubsub import Topic as LCMTopic @@ -493,3 +495,40 @@ def test_round_trip_untyped(self) -> None: reconstructed = _key_expr_to_topic(key) assert reconstructed.topic == original.topic assert reconstructed.lcm_type is None + + +def test_generated_message_transport_qos_and_unsubscribe(): + pool = ZenohSessionPool() + qos = ZenohQoS(reliability="reliable", congestion_control="block") + transport = ZenohTransport( + Topic("codec-check", message_type=Int32, qos=qos), + session_pool=pool, + connect=[], + listen=[LOOPBACK_LISTEN], + scouting=False, + multicast=False, + gossip=False, + ) + received = [] + delivered = threading.Event() + + def receive(value): + received.append(value) + delivered.set() + + try: + unsubscribe = transport.subscribe(receive) + assert transport.publish_qos == qos.to_wire() + ZenohPubSubBase.publish(transport.zenoh, transport.topic, b"broken") + transport.broadcast(None, Int32(42)) + assert delivered.wait(3) + assert received == [Int32(42)] + unsubscribe() + delivered.clear() + transport.subscribe(lambda value: delivered.set()) + transport.broadcast(None, Int32(43)) + assert delivered.wait(3) + assert received == [Int32(42)] + finally: + transport.stop() + pool.close_all() diff --git a/dimos/protocol/pubsub/impl/webrtc/test_transport.py b/dimos/protocol/pubsub/impl/webrtc/test_transport.py index d2a81dd5103..5f1529f0295 100644 --- a/dimos/protocol/pubsub/impl/webrtc/test_transport.py +++ b/dimos/protocol/pubsub/impl/webrtc/test_transport.py @@ -28,7 +28,9 @@ from dimos.core.coordination.blueprints import Blueprint from dimos.core.coordination.module_coordinator import _materialize_transports from dimos.core.transport import WebRTCTransport +from dimos.messages.std_msgs.msg import Int32 from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped +from dimos.protocol.pubsub.codec import describe_message from dimos.protocol.pubsub.impl.webrtc.providers import spec from dimos.protocol.pubsub.impl.webrtc.providers.broker import BrokerConfig from dimos.protocol.pubsub.impl.webrtc.providers.spec import ( @@ -512,3 +514,24 @@ def test_subscribe_all_fires_once_per_message() -> None: ps.subscribe("u", lambda data, t: None) ps.publish("u", b"y") assert seen == [(b"x", "t"), (b"y", "u")] + + +def test_cdr_transport_raw_payload_errors_and_unsubscribe(): + config = MockConfig(name="cdr-raw") + transport = MockTransport("messages", Int32, config=config) + received = [] + wire = [] + try: + config.provider().subscribe("messages", lambda data, topic: wire.append(data)) + unsubscribe = transport.subscribe(received.append) + config.provider().publish("messages", b"\x00\x01\x00\x00") + assert received == [] + transport.broadcast(None, Int32(7)) + assert received == [Int32(7)] + assert wire[-1] == describe_message(Int32).codec.encode(Int32(7)) + unsubscribe() + transport.broadcast(None, Int32(8)) + assert received == [Int32(7)] + finally: + transport.stop() + config.provider().stop() diff --git a/dimos/protocol/pubsub/impl/zenohpubsub.py b/dimos/protocol/pubsub/impl/zenohpubsub.py index 802fc57a7a3..0ce32074b6a 100644 --- a/dimos/protocol/pubsub/impl/zenohpubsub.py +++ b/dimos/protocol/pubsub/impl/zenohpubsub.py @@ -18,12 +18,13 @@ from dataclasses import dataclass from functools import lru_cache import threading -from typing import Any, Literal +from typing import Any, Literal, cast import zenoh from dimos.msgs.helpers import resolve_msg_type -from dimos.protocol.pubsub.encoders import LCMEncoderMixin, PickleEncoderMixin +from dimos.protocol.pubsub.codec import describe_message +from dimos.protocol.pubsub.encoders import MessageEncoderMixin, PickleEncoderMixin from dimos.protocol.pubsub.impl.lcmpubsub import Topic as LCMTopic from dimos.protocol.pubsub.spec import AllPubSub from dimos.protocol.service.zenohservice import ZenohService @@ -91,8 +92,8 @@ def key_expr(self) -> str: Topic("dimos/cmd_vel", Twist) -> "dimos/cmd_vel/geometry_msgs.Twist" Topic("dimos/data") -> "dimos/data" """ - if self.lcm_type is not None: - return f"{self.pattern}/{self.lcm_type.msg_name}" + if self.message is not None and self.message.encoding == "lcm": + return f"{self.pattern}/{self.message.name}" return self.pattern @@ -118,6 +119,8 @@ def _key_expr_to_topic(key_expr: str, default_lcm_type: type | None = None) -> T "dimos/data" -> Topic("dimos/data", default_lcm_type) "dimos/data/unknown.Foo" -> Topic("dimos/data/unknown.Foo", default_lcm_type) """ + if default_lcm_type is not None and describe_message(default_lcm_type).encoding == "cdr": + return Topic(key_expr, message_type=default_lcm_type) # Try to resolve the last segment as a message type parts = key_expr.rsplit("/", 1) if len(parts) == 2: @@ -195,7 +198,7 @@ def publish(self, topic: Topic, message: bytes) -> None: publisher = self._get_publisher(key_expr, qos) publisher.put(message) except Exception: - logger.error(f"Error publishing to {key_expr}", exc_info=True) + logger.exception(f"Error publishing to {key_expr}") def subscribe( self, topic: Topic, callback: Callable[[bytes, Topic], None] @@ -207,7 +210,7 @@ def on_sample(sample: zenoh.Sample) -> None: try: data = sample.payload.to_bytes() except Exception: - logger.error(f"Error reading payload from {key_expr}", exc_info=True) + logger.exception(f"Error reading payload from {key_expr}") return # Concrete subscriptions only ever receive their own key, so the # subscribed topic can be passed through without re-parsing. @@ -215,7 +218,7 @@ def on_sample(sample: zenoh.Sample) -> None: if sample_key == key_expr: recv_topic = topic else: - recv_topic = _key_expr_to_topic(sample_key, topic.lcm_type) + recv_topic = _key_expr_to_topic(sample_key, cast("Hashable", topic.lcm_type)) callback(data, recv_topic) sub = self.session.declare_subscriber(key_expr, on_sample) @@ -279,7 +282,7 @@ def drain() -> None: try: callback(msg, topic) except Exception: - logger.error("Error in subscribe_all callback", exc_info=True) + logger.exception("Error in subscribe_all callback") thread = threading.Thread(target=drain, name="zenoh-subscribe-all", daemon=True) @@ -329,18 +332,14 @@ def stop(self) -> None: class Zenoh( # type: ignore[misc] - LCMEncoderMixin, + MessageEncoderMixin, ZenohPubSubBase, ): """Zenoh pub/sub with LCM encoding for typed DimosMsg.""" - ... - class PickleZenoh( PickleEncoderMixin, # type: ignore[type-arg] ZenohPubSubBase, ): """Zenoh pub/sub with pickle encoding for arbitrary Python objects.""" - - ... diff --git a/dimos/protocol/pubsub/test_codec.py b/dimos/protocol/pubsub/test_codec.py new file mode 100644 index 00000000000..712c789b589 --- /dev/null +++ b/dimos/protocol/pubsub/test_codec.py @@ -0,0 +1,72 @@ +# 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. + +"""Codec contracts and channel metadata, without network access.""" + +import pickle + +import pytest + +from dimos.messages.geometry_msgs.msg import Vector3 as CDRVector3 +from dimos.messages.std_msgs.msg import Int32, String +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.protocol.pubsub.codec import describe_message +from dimos.protocol.pubsub.impl.lcmpubsub import Topic +from dimos.protocol.pubsub.impl.zenohpubsub import Topic as ZenohTopic, _key_expr_to_topic + + +@pytest.mark.parametrize( + "payload", [b"", b"bad!", b"\x00\x01\x00\x00", b"\x00\x03\x00\x00" + b"\x00" * 4] +) +def test_cdr_rejects_invalid_payload_without_fallback(payload): + with pytest.raises(ValueError): + describe_message(Int32).codec.decode(payload) + + +@pytest.mark.parametrize( + "declared,message", + [(Int32, String("wrong")), (Vector3, CDRVector3(1, 2, 3)), (CDRVector3, Vector3(1, 2, 3))], +) +def test_codec_rejects_wrong_declared_type(declared, message): + with pytest.raises(TypeError, match="declared type"): + describe_message(declared).codec.encode(message) + + +def test_unsupported_and_ambiguous_types_fail_at_binding(monkeypatch): + with pytest.raises(TypeError, match="Unsupported"): + Topic("test", message_type=dict) + monkeypatch.setattr(Int32, "lcm_encode", lambda self: b"", raising=False) + monkeypatch.setattr(Int32, "lcm_decode", lambda data: None, raising=False) + with pytest.raises(TypeError, match="Ambiguous"): + Topic("test", message_type=Int32) + + +@pytest.mark.parametrize("topic_type", [Topic, ZenohTopic]) +def test_generic_and_legacy_constructor_names_are_compatible(topic_type): + topic = topic_type("test", message_type=Int32) + assert topic == topic_type("test", Int32) + assert pickle.loads(pickle.dumps(topic)) == topic + with pytest.raises(TypeError, match="Conflicting"): + topic_type("test", Vector3, message_type=Int32) + + +def test_legacy_channel_names_and_plain_cdr_topics(): + assert str(Topic("test", Vector3)) == "test#geometry_msgs.Vector3" + assert ZenohTopic("test", Vector3).key_expr == "test/geometry_msgs.Vector3" + cdr = Topic("test", message_type=Int32) + assert str(cdr) == "test" + assert Topic.from_channel_str(str(cdr), Int32) == cdr + zenoh = ZenohTopic("test", message_type=Int32) + assert zenoh.key_expr == "test" + assert _key_expr_to_topic(zenoh.key_expr, Int32) == zenoh diff --git a/dimos/protocol/pubsub/test_encoder.py b/dimos/protocol/pubsub/test_encoder.py index dec1e429726..31ca775a5c8 100644 --- a/dimos/protocol/pubsub/test_encoder.py +++ b/dimos/protocol/pubsub/test_encoder.py @@ -1,5 +1,3 @@ -#!/usr/bin/env python3 - # Copyright 2025-2026 Dimensional Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -14,9 +12,17 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dataclasses import dataclass import json from typing import Any +from dimos_cdrgen.registry import encode as encode_cdr +import pytest + +from dimos.messages.std_msgs.msg import Int32, String +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.protocol.pubsub.codec import MessageDescriptor, describe_message +from dimos.protocol.pubsub.encoders import MessageEncoderMixin from dimos.protocol.pubsub.impl.memory import Memory, MemoryWithJSONEncoder @@ -169,3 +175,58 @@ def callback(message: Any, topic: str) -> None: # Verify it's actually JSON decoded_raw = json.loads(raw_message.decode("utf-8")) assert decoded_raw == original_message + + +@dataclass(frozen=True) +class _TypedTopic: + name: str + message: MessageDescriptor + + +class _EncodedMemory(MessageEncoderMixin, Memory): + pass + + +@pytest.mark.parametrize("message", [Vector3(1, 2, 3), Int32(42), String("hello")]) +def test_message_encoder_publish_subscribe_and_unsubscribe(message): + pubsub = _EncodedMemory() + topic = _TypedTopic("messages", describe_message(type(message))) + received = [] + wire = [] + remove_raw = Memory.subscribe(pubsub, topic, lambda data, topic: wire.append(data)) + unsubscribe = pubsub.subscribe(topic, lambda value, topic: received.append(value)) + try: + pubsub.publish(topic, message) + expected = message.lcm_encode() if isinstance(message, Vector3) else encode_cdr(message) + assert wire == [expected] + assert received == [message] + assert type(received[0]) is type(message) + unsubscribe() + pubsub.publish(topic, message) + assert received == [message] + assert wire == [expected, expected] + finally: + unsubscribe() + remove_raw() + + +def test_message_encoder_keeps_legacy_and_cdr_subscriptions_separate(): + pubsub = _EncodedMemory() + legacy = _TypedTopic("legacy", describe_message(Vector3)) + cdr = _TypedTopic("cdr", describe_message(Int32)) + received_legacy = [] + received_cdr = [] + stop_legacy = pubsub.subscribe(legacy, lambda value, topic: received_legacy.append(value)) + stop_cdr = pubsub.subscribe(cdr, lambda value, topic: received_cdr.append(value)) + try: + pubsub.publish(legacy, Vector3(1, 2, 3)) + Memory.publish(pubsub, cdr, b"broken") + assert received_cdr == [] + with pytest.raises(TypeError, match="declared type"): + pubsub.publish(cdr, Vector3(1, 2, 3)) + pubsub.publish(cdr, Int32(7)) + assert received_legacy == [Vector3(1, 2, 3)] + assert received_cdr == [Int32(7)] + finally: + stop_legacy() + stop_cdr() diff --git a/dimos/robot/unitree/go2/dds/rust/Cargo.toml b/dimos/robot/unitree/go2/dds/rust/Cargo.toml index 1a264d70fdf..96957ff404a 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 = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", tag = "v0.1.6" } serde = { version = "1", features = ["derive"] } serde-big-array = "0.5" diff --git a/dimos/skills/visual_navigation_skills.py b/dimos/skills/visual_navigation_skills.py index c3a15503d86..5b118a77f0a 100644 --- a/dimos/skills/visual_navigation_skills.py +++ b/dimos/skills/visual_navigation_skills.py @@ -25,7 +25,7 @@ from pydantic import Field -from dimos.perception.visual_servoing import ( # type: ignore[import-untyped] +from dimos.perception.visual_servoing import ( # type: ignore[import-not-found] VisualServoing, ) from dimos.skills.skills import AbstractRobotSkill diff --git a/dimos/stream/audio/node_simulated.py b/dimos/stream/audio/node_simulated.py index f6dbe616084..e40fcb85d0e 100644 --- a/dimos/stream/audio/node_simulated.py +++ b/dimos/stream/audio/node_simulated.py @@ -19,7 +19,7 @@ from reactivex import Observable, create, disposable from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT -from dimos.stream.audio.abstract import ( # type: ignore[import-untyped] +from dimos.stream.audio.abstract import ( # type: ignore[import-not-found] AbstractAudioEmitter, AudioEvent, ) diff --git a/dimos/visualization/rerun/bridge.py b/dimos/visualization/rerun/bridge.py index 543e3488b2f..819cafa663d 100644 --- a/dimos/visualization/rerun/bridge.py +++ b/dimos/visualization/rerun/bridge.py @@ -17,6 +17,7 @@ from __future__ import annotations from collections.abc import Callable +from contextlib import ExitStack from dataclasses import field import functools from operator import attrgetter @@ -52,6 +53,16 @@ from dimos.core.core import rpc from dimos.core.global_config import global_config from dimos.core.module import Module, ModuleConfig +from dimos.core.transport import PubSubTransport +from dimos.messages.sensor_msgs.msg import ( + CameraInfo as CdrCameraInfo, + CompressedImage as CdrCompressedImage, + Image as CdrImage, +) +from dimos.messages.tf2_msgs.msg import TFMessage as CdrTFMessage +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.tf2_msgs.TFMessage import TfFrameTree, TFMessage @@ -62,6 +73,7 @@ from dimos.protocol.service.lcmservice import autoconf from dimos.utils.generic import get_local_ips from dimos.utils.logging_config import setup_logger +from dimos.visualization.rerun.cdr import camera_pinhole, generated_archetypes, message_frame_id from dimos.visualization.rerun.constants import ( RERUN_ENABLE_WEB, RERUN_GRPC_PORT, @@ -278,7 +290,12 @@ class Config(ModuleConfig): honored. """ - pubsubs: list[SubscribeAllCapable[Any, Any]] = field(default_factory=lambda: [LCM()]) + # Construct the selected backend only when the bridge starts. Creating a + # blueprint or importing a camera module must not open multicast sockets. + pubsubs: list[SubscribeAllCapable[Any, Any]] | None = None + # Explicit subscriptions for the proposal's generated CDR channels. Each + # transport belongs to this bridge; legacy discovery/defaults stay intact. + cdr_transports: dict[str, PubSubTransport[Any]] = field(default_factory=dict) visual_override: dict[Glob | str, VisualOverride] = field(default_factory=dict) static: dict[str, Callable[[Any], Any]] = field(default_factory=dict) @@ -342,7 +359,7 @@ def __init__(self, **kwargs: Any) -> None: self._last_log = {} self._override_cache: dict[str, Callable[[Any], RerunData | None]] = {} self._frame_attached: dict[str, str] = {} - self._camera_infos: dict[str, CameraInfo] = {} + self._camera_infos: dict[str, CameraInfo | CdrCameraInfo] = {} self._image_entities: set[str] = set() self._min_intervals: dict[str, float] = {} self._tf_lock = threading.Lock() @@ -409,7 +426,7 @@ def final_convert(msg: Any) -> RerunData | None: return msg if isinstance(msg, RerunConvertible): return msg.to_rerun() - return None + return generated_archetypes(msg, entity_path) # compose all converters def composed(msg: Any) -> RerunData | None: @@ -456,13 +473,43 @@ def _on_message(self, msg: Any, topic: Any) -> None: return self._last_log[entity_path] = now + if self._tf_tree is not None and isinstance(msg, CdrTFMessage): + # The existing frame-tree layout helper consumes local numerical values. + transforms = [ + Transform( + translation=Vector3( + t.transform.translation.x, + t.transform.translation.y, + t.transform.translation.z, + ), + rotation=Quaternion( + t.transform.rotation.x, + t.transform.rotation.y, + t.transform.rotation.z, + t.transform.rotation.w, + ), + frame_id=t.header.frame_id, + child_frame_id=t.child_frame_id, + ts=t.header.stamp.sec + t.header.stamp.nanosec / 1_000_000_000, + ) + for t in msg.transforms + ] + with self._tf_lock: + for entry in generated_archetypes(msg, entity_path) or []: + rr.log(entry[0], entry[1]) + self._tf_tree.update(transforms) + return + if self._tf_tree is not None and isinstance(msg, TFMessage): with self._tf_lock: for path, archetype in msg.to_rerun(self._tf_tree): rr.log(path, archetype) return - if isinstance(msg, CameraInfo) and entity_path not in self.config.visual_override: + if ( + isinstance(msg, (CameraInfo, CdrCameraInfo)) + and entity_path not in self.config.visual_override + ): self._log_camera_info(entity_path, msg) return @@ -471,16 +518,26 @@ def _on_message(self, msg: Any, topic: Any) -> None: if not rerun_data: return + is_image = isinstance(msg, (Image, CdrImage, CdrCompressedImage)) + if is_image: + self._image_entities.add(entity_path) + # TFMessage for example returns list of (entity_path, archetype) tuples if is_rerun_multi(rerun_data): for entry in rerun_data: rr.log(entry[0], entry[1], static=isinstance(entry, RerunEntry) and entry.static) # Pin the base entity to the message frame once, so child entities # follow it the way a single archetype does below. - frame_id = getattr(msg, "frame_id", None) + frame_id = message_frame_id(msg) if frame_id and self._frame_attached.get(entity_path) != frame_id: - rr.log(entity_path, rr.Transform3D(parent_frame=f"tf#/{frame_id}")) + if not any( + entry[0] == entity_path and isinstance(entry[1], rr.Transform3D) + for entry in rerun_data + ): + rr.log(entity_path, rr.Transform3D(parent_frame=f"tf#/{frame_id}")) self._frame_attached[entity_path] = frame_id + if is_image and frame_id in self._camera_infos: + rr.log(entity_path, self._camera_pinhole(self._camera_infos[frame_id])) else: rr.log(entity_path, cast("Archetype", rerun_data)) if isinstance(msg, Image): @@ -488,14 +545,18 @@ def _on_message(self, msg: Any, topic: Any) -> None: # if source msg carries a frame_id, attach the entity to that TF frame # should skip if archetype is a Transform3D if not isinstance(rerun_data, rr.Transform3D): - frame_id = getattr(msg, "frame_id", None) + frame_id = message_frame_id(msg) if frame_id and self._frame_attached.get(entity_path) != frame_id: rr.log(entity_path, rr.Transform3D(parent_frame=f"tf#/{frame_id}")) self._frame_attached[entity_path] = frame_id - if isinstance(msg, Image) and frame_id in self._camera_infos: - rr.log(entity_path, self._camera_infos[frame_id].to_rerun_pinhole()) + if is_image and frame_id in self._camera_infos: + rr.log(entity_path, self._camera_pinhole(self._camera_infos[frame_id])) + + @staticmethod + def _camera_pinhole(info: CameraInfo | CdrCameraInfo) -> Archetype: + return camera_pinhole(info) if isinstance(info, CdrCameraInfo) else info.to_rerun_pinhole() - def _log_camera_info(self, entity_path: str, info: CameraInfo) -> None: + def _log_camera_info(self, entity_path: str, info: CameraInfo | CdrCameraInfo) -> None: """A CameraInfo is the pinhole of every image in its optical frame. Rerun draws the frustum only when the Pinhole sits on the image entity, @@ -504,13 +565,14 @@ def _log_camera_info(self, entity_path: str, info: CameraInfo) -> None: """ import rerun as rr - if not info.frame_id: - rr.log(entity_path, info.to_rerun_pinhole()) + frame = message_frame_id(info) + if not frame: + rr.log(entity_path, self._camera_pinhole(info)) return - self._camera_infos[info.frame_id] = info + self._camera_infos[frame] = info for image_path, frame_id in self._frame_attached.items(): - if frame_id == info.frame_id and image_path in self._image_entities: - rr.log(image_path, info.to_rerun_pinhole()) + if frame_id == frame and image_path in self._image_entities: + rr.log(image_path, self._camera_pinhole(info)) @rpc def start(self) -> None: @@ -596,6 +658,32 @@ def start(self) -> None: self.register_disposable(Disposable(pubsub.stop)) # type: ignore[union-attr] self._log_static() + for topic, transport in self.config.cdr_transports.items(): + self._subscribe_cdr(topic, transport) + + def _subscribe_cdr(self, topic: str, transport: PubSubTransport[Any]) -> None: + cleanup = ExitStack() + cleanup.callback(transport.stop) + try: + unsubscribe = transport.subscribe(functools.partial(self._on_cdr_message, topic=topic)) + cleanup.callback(unsubscribe) + except BaseException: + cleanup.close() + raise + self.register_disposable(Disposable(cleanup.close)) + + def _on_cdr_message(self, msg: Any, *, topic: str) -> None: + import rerun as rr + + stamp = getattr(getattr(msg, "header", None), "stamp", None) + if stamp is not None: + nanos = int(stamp.sec) * 1_000_000_000 + int(stamp.nanosec) + rr.set_time("source", timestamp=np.datetime64(nanos, "ns")) + try: + self._on_message(msg, "/" + topic.strip("/")) + finally: + if stamp is not None: + rr.disable_timeline("source") def _subscribe(self, pubsub: SubscribeAllCapable[Any, Any]) -> Callable[[], None]: """Subscribe to the named topics, or to everything when none are named. diff --git a/dimos/visualization/rerun/cdr.py b/dimos/visualization/rerun/cdr.py new file mode 100644 index 00000000000..920de372993 --- /dev/null +++ b/dimos/visualization/rerun/cdr.py @@ -0,0 +1,254 @@ +# 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. + +"""Viewer adapters live outside the generated plain value classes.""" + +from __future__ import annotations + +from dataclasses import asdict +import json +from typing import TYPE_CHECKING, Any + +import numpy as np + +from dimos.message_utils.image import ( + image_from_compressed, + image_to_rgb, + image_view, +) +from dimos.message_utils.pointcloud import pointcloud_rgb, pointcloud_xyz +from dimos.messages.dimos_msgs.msg import EntityMarkers +from dimos.messages.geometry_msgs.msg import PointStamped, PoseStamped +from dimos.messages.nav_msgs.msg import Odometry, Path +from dimos.messages.sensor_msgs.msg import CameraInfo, CompressedImage, Image, PointCloud2 +from dimos.messages.tf2_msgs.msg import TFMessage + +if TYPE_CHECKING: + import rerun as rr + + from dimos.visualization.rerun.bridge import RerunMulti + + +def message_frame_id(message: Any) -> str | None: + header = getattr(message, "header", None) + frame = ( + getattr(header, "frame_id", None) + if header is not None + else getattr(message, "frame_id", None) + ) + return frame if isinstance(frame, str) else None + + +def camera_pinhole(info: CameraInfo, optical_frame: str | None = None) -> rr.Pinhole: + import rerun as rr + + frame = info.header.frame_id if optical_frame is None else optical_frame + return rr.Pinhole( + focal_length=[info.k[0], info.k[4]], + principal_point=[info.k[2], info.k[5]], + width=info.width, + height=info.height, + image_plane_distance=1.0, + parent_frame=f"tf#/{frame}" if frame else None, + ) + + +def image_archetype(message: Image | CompressedImage) -> rr.Image | rr.DepthImage | rr.EncodedImage: + import rerun as rr + + if isinstance(message, CompressedImage): + name = message.format.lower() + if "jxl" in name or ("png" in name and name.startswith("16uc1;")): + return image_archetype(image_from_compressed(message)) + if "jpeg" in name or "jpg" in name: + return rr.EncodedImage(contents=bytes(message.data), media_type="image/jpeg") + if "png" in name: + return rr.EncodedImage(contents=bytes(message.data), media_type="image/png") + raise ValueError(f"unsupported compressed image format {message.format!r}") + if message.encoding in ("16UC1", "32FC1", "mono16"): + pixels = image_view(message) + pixels = np.ascontiguousarray(pixels, dtype=pixels.dtype.newbyteorder("=")) + if message.encoding != "mono16": + return rr.DepthImage(pixels, meter=1000.0 if message.encoding == "16UC1" else 1.0) + return rr.Image( + bytes=pixels.tobytes(), + width=message.width, + height=message.height, + datatype=np.uint16, + color_model="L", + ) + return rr.Image( + bytes=image_to_rgb(message).tobytes(), + width=message.width, + height=message.height, + datatype=np.uint8, + color_model="RGB", + ) + + +def cloud_archetype( + message: PointCloud2, + *, + voxel_size: float = 0.05, + mode: str = "points", + bottom_cutoff: float | None = None, + height_colors: bool = False, + color: tuple[int, int, int] | None = None, +) -> rr.Points3D | rr.Boxes3D: + """Render plain cloud values, keeping color/geometry filters aligned. + + Existing bridge defaults use screen-space dots. Offline map tools explicitly + select world-space spheres/boxes and the existing height-based turbo colors. + """ + from matplotlib import colormaps + import rerun as rr + + points, colors = pointcloud_xyz(message), pointcloud_rgb(message) + keep = np.isfinite(points).all(axis=1) + if bottom_cutoff is not None: + keep &= points[:, 2] >= bottom_cutoff + points = points[keep] + if colors is not None: + colors = colors[keep] + elif height_colors and len(points): + z = points[:, 2] + levels = ((z - z.min()) / (z.max() - z.min() + 1e-8) * 255).astype(np.uint8) + colors = (colormaps["turbo"](levels)[:, :3] * 255).astype(np.uint8) + if color is not None: + colors = np.asarray(color, dtype=np.uint8) + if mode == "boxes": + return rr.Boxes3D(centers=points, half_sizes=[voxel_size / 2] * 3, colors=colors) + return rr.Points3D( + points, + colors=colors, + radii=rr.components.Radius.ui_points(2.0) if mode == "points" else voxel_size / 2, + ) + + +def entity_points(message: EntityMarkers) -> rr.Points3D: + """Render generated world entities with the existing labels, colors and radii.""" + import rerun as rr + + colors = { + "person": (255, 100, 100, 255), + "object": (100, 255, 100, 255), + "location": (100, 100, 255, 255), + } + return rr.Points3D( + positions=[[m.position.x, m.position.y, m.position.z] for m in message.markers], + labels=[f"{m.entity_id}: {m.label[:40]}" for m in message.markers], + colors=[colors.get(m.entity_type, (200, 200, 200, 255)) for m in message.markers], + radii=[0.15] * len(message.markers), + ) + + +def path_archetype( + message: Path, + *, + color: tuple[int, int, int] = (0, 255, 128), + z_offset: float = 0.5, + radii: float = 0.05, +) -> rr.LineStrips3D: + import rerun as rr + + if not message.poses: + return rr.LineStrips3D([]) + points = [ + [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z + z_offset] + for pose in message.poses + ] + return rr.LineStrips3D([points], colors=[color], radii=radii) + + +def generated_archetypes(message: Any, entity: str) -> RerunMulti | None: + """Render stamped geometry and its exact fields; other domains remain explicit.""" + import rerun as rr + + if type(message) in (Image, CompressedImage, PointCloud2): + archetype = ( + cloud_archetype(message) if type(message) is PointCloud2 else image_archetype(message) + ) + entries: RerunMulti = [(entity, archetype)] + frame = message_frame_id(message) + if frame: + entries.append((entity, rr.Transform3D(parent_frame=f"tf#/{frame}"))) + return entries + if type(message) is CameraInfo: + entries = [(entity, camera_pinhole(message))] + elif type(message) is TFMessage: + entries = [] + for value in message.transforms: + t, q = value.transform.translation, value.transform.rotation + entries.append( + ( + "tf#/" + rr.escape_entity_path_part(value.child_frame_id), + rr.Transform3D( + translation=[t.x, t.y, t.z], + quaternion=rr.Quaternion(xyzw=[q.x, q.y, q.z, q.w]), + parent_frame="tf#/" + value.header.frame_id, + child_frame="tf#/" + value.child_frame_id, + ), + ) + ) + elif type(message) is Path: + entries = [(entity, path_archetype(message))] + if message.header.frame_id: + entries.append((entity, rr.Transform3D(parent_frame=f"tf#/{message.header.frame_id}"))) + elif type(message) is EntityMarkers: + entries = [(entity, entity_points(message))] + if message.header.frame_id: + entries.append((entity, rr.Transform3D(parent_frame=f"tf#/{message.header.frame_id}"))) + elif type(message) in (PoseStamped, Odometry): + pose = message.pose.pose if type(message) is Odometry else message.pose + point, rotation = pose.position, pose.orientation + entries = [ + ( + entity, + rr.Transform3D( + translation=[point.x, point.y, point.z], + quaternion=rr.Quaternion(xyzw=[rotation.x, rotation.y, rotation.z, rotation.w]), + parent_frame=f"tf#/{message.header.frame_id}" + if message.header.frame_id + else None, + child_frame=f"tf#/{message.child_frame_id}" + if type(message) is Odometry and message.child_frame_id + else None, + ), + ), + (entity, rr.TransformAxes3D(0.35)), + ] + elif type(message) is PointStamped: + point = message.point + entries = [(entity, rr.Points3D([[point.x, point.y, point.z]]))] + if message.header.frame_id: + entries.append((entity, rr.Transform3D(parent_frame=f"tf#/{message.header.frame_id}"))) + else: + return None + fields = asdict( + message, + dict_factory=lambda items: {key: value for key, value in items if key != "__msgtype__"}, + ) + entries.append( + (entity + "/message", rr.TextDocument(json.dumps(fields, indent=2, default=_json_value))) + ) + return entries + + +def _json_value(value: Any) -> Any: + """Represent generated numerical arrays without changing their source values.""" + if isinstance(value, np.ndarray): + return value.tolist() + if isinstance(value, np.generic): + return value.item() + raise TypeError(f"Cannot render {type(value).__name__} as JSON") diff --git a/dimos/visualization/rerun/test_cdr.py b/dimos/visualization/rerun/test_cdr.py new file mode 100644 index 00000000000..2f087fb714c --- /dev/null +++ b/dimos/visualization/rerun/test_cdr.py @@ -0,0 +1,339 @@ +# 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. + +"""Generated values stay plain while the viewer owns rendering and subscriptions.""" + +import json +from unittest.mock import Mock + +from dimos_cdrgen.registry import decode, encode +import numpy as np +import pytest +import rerun as rr + +from dimos.core.global_config import GlobalConfig +from dimos.core.transport import ZenohTransport +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.dimos_msgs.msg import EntityMarker, EntityMarkers +from dimos.messages.geometry_msgs.msg import ( + Point, + PointStamped, + PoseWithCovariance, + Quaternion, + Transform, + TransformStamped, + Twist, + TwistWithCovariance, + Vector3, +) +from dimos.messages.nav_msgs.msg import Odometry +from dimos.messages.sensor_msgs.msg import ( + CameraInfo, + Image, + PointCloud2, + PointField, + RegionOfInterest, +) +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.protocol.rpc.zenohrpc import ZenohRPC +from dimos.visualization.rerun.bridge import Config, RerunBridgeModule, _resolve_pubsubs +from dimos.visualization.rerun.cdr import ( + cloud_archetype, + generated_archetypes, + image_archetype, +) +from dimos.visualization.vis_module import vis_module +from dimos.web.relay_bridge.tests.cdr_fixtures import build_messages + + +def image_from_array(pixels, *, encoding, header=None): + return Image( + header=header or Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + width=pixels.shape[1], + height=pixels.shape[0], + encoding=encoding, + is_bigendian=int(pixels.dtype.byteorder == ">"), + step=pixels.strides[0], + data=np.frombuffer(pixels.tobytes(), dtype=np.uint8), + ) + + +def camera_info_from_intrinsics(fx, fy, cx, cy, width, height, *, header): + return CameraInfo( + distortion_model="plumb_bob", + d=np.zeros(5, dtype=np.float64), + r=np.eye(3, dtype=np.float64).ravel(), + 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), + header=header, + width=width, + height=height, + k=np.asarray([fx, 0, cx, 0, fy, cy, 0, 0, 1], dtype=np.float64), + ) + + +def pointcloud_from_xyz_rgb(points, colors, *, header): + data = np.empty(len(points), dtype=[("x", " None: + message = build_messages()[0][1] + entries = generated_archetypes(message, "world/pose") + assert entries is not None + assert [path for path, _ in entries] == ["world/pose", "world/pose", "world/pose/message"] + transform, axes, text = [archetype for _, archetype in entries] + np.testing.assert_allclose( + transform.translation.as_arrow_array().to_pylist(), [[1.5, -2.5, 0.25]] + ) + np.testing.assert_allclose( + transform.quaternion.as_arrow_array().to_pylist(), [[0, 0, 0.6, 0.8]] + ) + assert transform.parent_frame.as_arrow_array().to_pylist() == ["tf#/map"] + assert isinstance(axes, rr.TransformAxes3D) + fields = json.loads(text.text.as_arrow_array().to_pylist()[0]) + assert fields["header"] == { + "stamp": {"sec": 1757400000, "nanosec": 500000123}, + "frame_id": "map", + } + assert "__msgtype__" not in fields + assert not hasattr(message, "to_rerun") + + +def test_point_frame_and_unknown_values() -> None: + pose = build_messages()[0][1] + msg = PointStamped(header=pose.header, point=pose.pose.position) + entries = generated_archetypes(msg, "world/point") + assert entries is not None + assert isinstance(entries[0][1], rr.Points3D) + assert entries[1][1].parent_frame.as_arrow_array().to_pylist() == ["tf#/map"] + assert generated_archetypes({"x": 1}, "world/no") is None + + +@pytest.mark.parametrize("nanoseconds", [0, 1757400000500000123]) +def test_source_time_is_scoped_and_reset_after_render_failure(bridge, mocker, nanoseconds) -> None: + msg = build_messages()[0][1] + msg.header.stamp = Time(sec=nanoseconds // 1_000_000_000, nanosec=nanoseconds % 1_000_000_000) + render = mocker.patch.object(bridge, "_on_message", side_effect=ValueError("bad renderer")) + set_time = mocker.patch.object(rr, "set_time") + clear_time = mocker.patch.object(rr, "disable_timeline") + with pytest.raises(ValueError, match="bad renderer"): + bridge._on_cdr_message(msg, topic="/pose") + render.assert_called_once_with(msg, "/pose") + set_time.assert_called_once_with("source", timestamp=np.datetime64(nanoseconds, "ns")) + clear_time.assert_called_once_with("source") + + +def test_subscription_cleanup_order_and_failed_start(bridge) -> None: + operations: list[str] = [] + transport = Mock(spec=ZenohTransport) + transport.subscribe.return_value = lambda: operations.append("unsubscribe") + transport.stop.side_effect = lambda: operations.append("stop") + bridge._subscribe_cdr("pose", transport) + bridge.stop() + assert operations == ["unsubscribe", "stop"] + failed = Mock(spec=ZenohTransport) + failed.subscribe.side_effect = RuntimeError("native start failed") + with pytest.raises(RuntimeError, match="native start failed"): + bridge._subscribe_cdr("pose", failed) + failed.stop.assert_called_once_with() + + +def test_bridge_default_renderer_handles_generated_pose(bridge, mocker) -> None: + log = mocker.patch.object(rr, "log") + bridge._on_message(build_messages()[0][1], "/pose") + assert [call.args[0] for call in log.call_args_list] == [ + "world/pose", + "world/pose", + "world/pose/message", + ] + assert isinstance(log.call_args_list[0].args[1], rr.Transform3D) + + +@pytest.mark.parametrize("shape", [(1, 2), (2, 1)]) +def test_single_row_or_column_rgb_keeps_dimensions_and_color_bytes(shape) -> None: + pixels = np.array([[[255, 0, 0], [0, 255, 0]]], dtype=np.uint8).reshape(*shape, 3) + image = decode(encode(image_from_array(pixels, encoding="rgb8")), Image) + archetype = image_archetype(image) + assert archetype.format.as_arrow_array().to_pylist()[0]["height"] == shape[0] + assert archetype.format.as_arrow_array().to_pylist()[0]["width"] == shape[1] + assert archetype.buffer.as_arrow_array().to_pylist() == [[255, 0, 0, 0, 255, 0]] + + +def test_big_endian_depth_renderer_preserves_values_and_units() -> None: + pixels = np.array([[1, 256, 65535]], dtype=">u2") + archetype = image_archetype(image_from_array(pixels, encoding="16UC1")) + assert isinstance(archetype, rr.DepthImage) + assert archetype.meter.as_arrow_array().to_pylist() == [1000.0] + raw = bytes(archetype.buffer.as_arrow_array().to_pylist()[0]) + np.testing.assert_array_equal(np.frombuffer(raw, dtype=np.uint16), pixels.ravel()) + + +@pytest.mark.parametrize("camera_first", [False, True]) +def test_calibration_attaches_in_both_arrival_orders(bridge, mocker, camera_first) -> None: + header = build_messages()[0][1].header + image = image_from_array(np.zeros((1, 2, 3), dtype=np.uint8), encoding="rgb8", header=header) + calibration = camera_info_from_intrinsics(10, 20, 1, 0.5, 2, 1, header=header) + log = mocker.patch.object(rr, "log") + samples = [(calibration, "/calibration"), (image, "/image")] + for message, topic in samples if camera_first else reversed(samples): + bridge._on_message(message, topic) + pinholes = [call for call in log.call_args_list if isinstance(call.args[1], rr.Pinhole)] + assert len(pinholes) == 1 + assert pinholes[0].args[0] == "world/image" + assert pinholes[0].args[1].parent_frame.as_arrow_array().to_pylist() == ["tf#/map"] + + +def test_cloud_renderer_filters_nonfinite_positions_with_matching_colors() -> None: + header = build_messages()[0][1].header + cloud = pointcloud_from_xyz_rgb( + np.array([[1, 2, 3], [float("nan"), 0, 0], [4, 5, 6]]), + np.array([[255, 0, 0], [0, 255, 0], [0, 0, 255]], dtype=np.uint8), + header=header, + ) + archetype = cloud_archetype(cloud) + assert archetype.positions.as_arrow_array().to_pylist() == [[1, 2, 3], [4, 5, 6]] + assert archetype.colors.as_arrow_array().to_pylist() == [0xFF0000FF, 0x0000FFFF] + + +@pytest.mark.parametrize("backend", ["zenoh", "lcm"]) +def test_blueprint_does_not_construct_pubsubs_and_resolution_keeps_backend(mocker, backend) -> None: + lcm = mocker.patch("dimos.visualization.rerun.bridge.LCM") + zenoh = mocker.patch("dimos.visualization.rerun.bridge.Zenoh") + config = Config(g=GlobalConfig(transport=backend)) + assert config.pubsubs is None + vis_module("rerun") + lcm.assert_not_called() + zenoh.assert_not_called() + resolved = _resolve_pubsubs(config) + expected = zenoh if backend == "zenoh" else lcm + expected.assert_called_once_with() + assert resolved == [expected.return_value] + + +def test_generated_entity_marker_adapter_keeps_labels_colors_frame_and_exact_fields(): + header = Header(frame_id="world", stamp=Time(sec=1700000000, nanosec=123456789)) + message = EntityMarkers( + header=header, + markers=[ + EntityMarker( + entity_id="E1", + label="person", + entity_type="person", + position=Point(x=1, y=2, z=0.3), + ) + ], + ) + message = decode(encode(message), EntityMarkers) + entries = generated_archetypes(message, "memory/entities") + assert entries is not None + points, frame, fields = [value for _, value in entries] + np.testing.assert_allclose(points.positions.as_arrow_array().to_pylist(), [[1, 2, 0.3]]) + assert points.labels.as_arrow_array().to_pylist() == ["E1: person"] + assert points.colors.as_arrow_array().to_pylist() == [0xFF6464FF] + assert frame.parent_frame.as_arrow_array().to_pylist() == ["tf#/world"] + assert ( + json.loads(fields.text.as_arrow_array().to_pylist()[0])["header"]["stamp"]["nanosec"] + == 123456789 + ) + + +@pytest.mark.parametrize("mode", ["points", "boxes", "spheres"]) +def test_fixed_display_color_overrides_sensor_rgb_after_invalid_points_filter(mode): + cloud = pointcloud_from_xyz_rgb( + np.array([[1, 2, 3], [np.nan, 0, 0]], dtype=np.float32), + np.array([[255, 0, 0], [0, 255, 0]], dtype=np.uint8), + header=Header(stamp=Time(sec=0, nanosec=0), frame_id="world"), + ) + before = encode(cloud) + value = cloud_archetype(cloud, mode=mode, color=(76, 220, 41)) + positions = value.centers if mode == "boxes" else value.positions + assert positions.as_arrow_array().to_pylist() == [[1, 2, 3]] + assert value.colors.as_arrow_array().to_pylist() == [0x4CDC29FF] + assert encode(cloud) == before + + +def test_generated_odometry_exposes_pose_axes_and_full_covariance_fields(): + pose = build_messages()[0][1] + value = Odometry( + pose.header, + "base", + PoseWithCovariance(pose.pose, np.zeros(36)), + TwistWithCovariance(Twist(Vector3(0.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), np.zeros(36)), + ) + value.pose.covariance[0] = 0.25 + entries = generated_archetypes(value, "world/odometry") + assert entries is not None + assert isinstance(entries[0][1], rr.Transform3D) + assert isinstance(entries[1][1], rr.TransformAxes3D) + assert entries[0][1].translation.as_arrow_array().to_pylist() == [[1.5, -2.5, 0.25]] + fields = json.loads(entries[-1][1].text.as_arrow_array().to_pylist()[0]) + assert fields["pose"]["covariance"][0] == 0.25 + assert fields["child_frame_id"] == "base" + assert entries[0][1].child_frame.as_arrow_array().to_pylist() == ["tf#/base"] + assert fields["header"]["stamp"]["nanosec"] == 500000123 + assert value.pose.covariance[0] == 0.25 + + +def test_generated_tf_preserves_frame_tree_layout_and_transform_values(bridge, mocker): + bridge.config.tf_axes = 0.35 + bridge._tf_tree = bridge._new_tf_tree() + log = mocker.patch.object(rr, "log") + value = TFMessage( + [ + TransformStamped( + Header(Time(0, 1), "world"), + "camera", + Transform(Vector3(1.0, 2.0, 3.0), Quaternion(0.0, 0.0, 0.0, 1.0)), + ) + ] + ) + bridge._on_message(value, "tf") + assert bridge._tf_tree.placements() == { + "world": "world/tf/world", + "camera": "world/tf/world/camera", + } + transform = next(call.args[1] for call in log.call_args_list if call.args[0] == "tf#/camera") + assert transform.translation.as_arrow_array().to_pylist() == [[1.0, 2.0, 3.0]] + assert transform.child_frame.as_arrow_array().to_pylist() == ["tf#/camera"] + assert value.transforms[0].header.stamp == Time(0, 1) diff --git a/dimos/visualization/rerun/test_cdr_e2e.py b/dimos/visualization/rerun/test_cdr_e2e.py new file mode 100644 index 00000000000..bafeef18d47 --- /dev/null +++ b/dimos/visualization/rerun/test_cdr_e2e.py @@ -0,0 +1,102 @@ +# 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. + +"""Run the reviewer example and inspect its live bridge and native MCAP importer.""" + +from pathlib import Path +import subprocess +import sys + +import pytest + + +def test_live_bridge_and_native_mcap_import_cover_scene( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + # The pinned Rerun CLI disables analytics when CI is present. This changes + # only the child environment, not the user's persistent viewer settings. + monkeypatch.setenv("CI", "1") + monkeypatch.delenv("FORCE_RERUN_ANALYTICS", raising=False) + example = Path(__file__).resolve().parents[3] / "examples/message-viewer/demo_cdr_rerun.py" + output = tmp_path / "pose.rrd" + command = [sys.executable, "-I", str(example), str(output)] + subprocess.run(command, check=True, capture_output=True, text=True, timeout=30) + contents = output.read_bytes() + assert contents.startswith(b"RRF") + rerun = Path(sys.executable).parent / "rerun" + result = subprocess.run( + [str(rerun), "rrd", "print", "-vvv", str(output)], + check=True, + capture_output=True, + text=True, + timeout=30, + ) + for marker in ( + "TransformAxes3D:axis_length", + "[[1.5, -2.5, 0.25]]", + "[[0.0, 0.0, 0.6, 0.8]]", + "tf#/map", + "tf#/camera", + "2023-11-14T22:13:20.123456789", + '"nanosec": 123456789', + "Exact message values", + "Points3D:positions", + "Points3D:colors", + "Boxes3D:centers", + "Image:buffer", + "Pinhole:image_from_camera", + "DepthImage:meter", + "EncodedImage:blob", + "world/path", + "world/odometry", + "world/entities", + "demo person", + "world/legacy_pose", + "world/late_image", + "world/cloud_boxes", + "world/cloud_height", + "Generated RGB pixels", + ): + assert marker in result.stdout, marker + recording = output.with_suffix(".mcap") + assert recording.read_bytes().startswith(b"\x89MCAP") + native = output.with_name(output.stem + "-native.rrd") + subprocess.run( + [str(rerun), "mcap", "convert", str(recording), "-o", str(native)], + check=True, + capture_output=True, + text=True, + timeout=30, + ) + imported = subprocess.run( + [str(rerun), "rrd", "print", "-vvv", str(native)], + check=True, + capture_output=True, + text=True, + timeout=30, + ) + for marker in ( + "/pose", + "/tf", + "Image:buffer", + "Points3D:positions", + "Pinhole:image_from_camera", + "InstancePoses3D", + "2023-11-14T22:13:20.123456789", + ): + assert marker in imported.stdout, marker + repeated = subprocess.run(command, capture_output=True, text=True, timeout=30) + assert repeated.returncode != 0 + assert "FileExistsError" in repeated.stderr + assert output.read_bytes() == contents diff --git a/dimos/visualization/rerun/test_cdr_entities.py b/dimos/visualization/rerun/test_cdr_entities.py new file mode 100644 index 00000000000..9cfd9c3676d --- /dev/null +++ b/dimos/visualization/rerun/test_cdr_entities.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. + +"""Tests for refactored temporal memory components.""" + +from dimos_cdrgen.registry import decode as cdr_decode, encode as cdr_encode +import rerun as rr + +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.dimos_msgs.msg import EntityMarker, EntityMarkers +from dimos.messages.geometry_msgs.msg import Point +from dimos.messages.std_msgs.msg import Header +from dimos.visualization.rerun.cdr import entity_points + + +def test_markers_to_rerun() -> None: + markers = EntityMarkers( + markers=[ + EntityMarker( + entity_id="E1", + label="person walking", + entity_type="person", + position=Point(x=1.0, y=2.0, z=0.3), + ), + EntityMarker( + entity_id="E2", + label="wooden table", + entity_type="object", + position=Point(x=3.0, y=4.0, z=0.3), + ), + ], + header=Header(stamp=Time(sec=0, nanosec=0), frame_id=""), + ) + archetype = entity_points(cdr_decode(cdr_encode(markers), EntityMarkers)) + # Should return rr.Points3D + + assert isinstance(archetype, rr.Points3D) diff --git a/dimos/visualization/rerun/test_cdr_image_values.py b/dimos/visualization/rerun/test_cdr_image_values.py new file mode 100644 index 00000000000..3e56d4bac7c --- /dev/null +++ b/dimos/visualization/rerun/test_cdr_image_values.py @@ -0,0 +1,57 @@ +# 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 imagecodecs +import numpy as np +import pytest +import rerun as rr + +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.visualization.rerun.cdr import image_archetype + + +def test_png_color_is_rendered_as_encoded_image(): + pixels = np.array([[[255, 0, 0], [0, 255, 0]]], dtype=np.uint8) + message = CompressedImage( + header=Header(stamp=Time(sec=0, nanosec=0), frame_id="camera"), + format="png", + data=np.frombuffer(imagecodecs.png_encode(pixels), dtype=np.uint8), + ) + archetype = image_archetype(message) + assert isinstance(archetype, rr.EncodedImage) + assert bytes(archetype.blob.as_arrow_array().to_pylist()[0]) == bytes(message.data) + + +@pytest.mark.parametrize("codec", ["png", "jxl"]) +def test_compressed_uint16_depth_keeps_values_and_units(codec): + pixels = np.arange(80, dtype=np.uint16).reshape(10, 8) + encoded = ( + imagecodecs.png_encode(pixels) + if codec == "png" + else imagecodecs.jpegxl_encode(pixels, lossless=True) + ) + message = CompressedImage( + header=Header(frame_id="depth", stamp=Time(sec=0, nanosec=0)), + format=f"16UC1; {codec}", + data=np.frombuffer(encoded, dtype=np.uint8), + ) + archetype = image_archetype(message) + assert isinstance(archetype, rr.DepthImage) + assert archetype.meter.as_arrow_array().to_pylist() == [1000.0] + np.testing.assert_array_equal( + np.frombuffer(bytes(archetype.buffer.as_arrow_array().to_pylist()[0]), dtype=np.uint16), + pixels.ravel(), + ) diff --git a/dimos/web/cdr_codec.py b/dimos/web/cdr_codec.py new file mode 100644 index 00000000000..459565fe887 --- /dev/null +++ b/dimos/web/cdr_codec.py @@ -0,0 +1,63 @@ +# 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. + +"""Generated CDR payloads and self-contained ROS2 schemas for web channels.""" + +from collections.abc import Mapping +import re +from typing import Any + +from dimos_cdrgen.registry import encode, message_types, schema + +CDR_V1_SUFFIX = ".cdr.v1" +_TYPE_NAME = re.compile(r"^[A-Za-z][A-Za-z0-9_]*/msg/[A-Za-z][A-Za-z0-9_]*$") + + +def cdr_type_name(message_type: type[Any]) -> str | None: + name = getattr(message_type, "__msgtype__", None) + if ( + isinstance(name, str) + and _TYPE_NAME.fullmatch(name) + and message_types().get(name) is message_type + ): + return name + return None + + +def export_schema(message_type: type[Any]) -> dict[str, str]: + name = cdr_type_name(message_type) + if name is None: + raise ValueError(f"{message_type.__qualname__} has no generated CDR schema") + return {"type": name, "definition": schema(name)} + + +def check_cdr_params(params: Mapping[str, Any]) -> None: + schema = params.get("cdr") + if not ( + isinstance(schema, Mapping) + and isinstance(schema.get("type"), str) + and _TYPE_NAME.fullmatch(schema["type"]) + and isinstance(schema.get("definition"), str) + ): + raise ValueError( + "*.cdr.v1 channels need params['cdr'] = {type, definition}; cockpit() fills it in" + ) + + +def encode_cdr_v1(msg: Any, params: Mapping[str, Any]) -> bytes: + if cdr_type_name(type(msg)) != params["cdr"]["type"]: + raise ValueError( + f"message type does not match declared channel type {params['cdr']['type']}" + ) + return encode(msg) diff --git a/dimos/web/cockpit.py b/dimos/web/cockpit.py index 22dbe908c44..9049f922b80 100644 --- a/dimos/web/cockpit.py +++ b/dimos/web/cockpit.py @@ -51,7 +51,8 @@ from dimos.msgs.geometry_msgs.PointStamped import PointStamped from dimos.msgs.nav_msgs.Path import Path from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.web.codecs import is_generic_lcm_encoding +from dimos.web.cdr_codec import export_schema as export_cdr_schema +from dimos.web.codecs import is_generic_cdr_encoding, is_generic_lcm_encoding from dimos.web.lcm_codec import default_encoding, export_schema, schema_class_for from dimos.web.relay_bridge.manifest import ( MANIFEST_VERSION, @@ -168,7 +169,8 @@ class Channel: JSON-shaped types and dataclasses (rx) / JSON scalars, lists and dicts (tx decode), or `.lcm.v1` for rx DimOS messages with a dimos_lcm schema (the frame is `lcm_encode()`, the schema rides - params["lcm"]). None picks the default: `.lcm.v1` for rx + params["lcm"]), or explicit `/msg/.cdr.v1` for installed + generated values (complete ROS2 schemas ride params["cdr"]). None picks the default: `.lcm.v1` for rx DimOS messages, "json.v1" otherwise. A dir="tx" channel is a generic browser publish input: it must declare @@ -278,6 +280,10 @@ def _request_of(channel: Channel) -> ChannelRequest: # Deep plain copy: the manifest and specs must not alias the (frozen) # authoring record's nested values. params = _thaw_params(channel.params or {}) + if is_generic_cdr_encoding(channel.encoding): + if "cdr" in params: + raise ValueError("params key 'cdr' is reserved for the CDR schema") + params["cdr"] = export_cdr_schema(channel.message_type) if is_generic_lcm_encoding(channel.encoding): if "lcm" in params: raise ValueError("params key 'lcm' is reserved for the LCM schema") diff --git a/dimos/web/codecs.py b/dimos/web/codecs.py index e0129908846..fee05c93c96 100644 --- a/dimos/web/codecs.py +++ b/dimos/web/codecs.py @@ -35,10 +35,11 @@ def decode_goal(value: dict[str, Any], context: PublishContext) -> PoseStamped: Encoders take the message (plus, optionally, the channel's params mapping) and return `bytes`, an `EncodedPayload` when the frame needs header meta, or None -to skip the sample. Two codecs apply without registration: json.v1 for +to skip the sample. Codecs apply without registration: json.v1 for JSON-shaped message types, and `.lcm.v1` (dimos/web/lcm_codec.py) for DimOS messages with a dimos_lcm schema (the frame is `msg.lcm_encode()`, -the schema rides the channel's params["lcm"]). Anything else potentially +the schema rides the channel's params["lcm"]), and explicit `/msg/.cdr.v1` +for installed generated CDR values with complete params["cdr"] schemas. Anything else potentially large (images, arrays, bytes) needs an explicit @web_encoder. """ @@ -52,6 +53,13 @@ def decode_goal(value: dict[str, Any], context: PublishContext) -> PoseStamped: import types from typing import Any, TypeVar, Union, get_args, get_origin, get_type_hints +from dimos.web.cdr_codec import ( + CDR_V1_SUFFIX, + cdr_type_name, + check_cdr_params, + encode_cdr_v1, + export_schema as export_cdr_schema, +) from dimos.web.lcm_codec import ( LCM_V1_SUFFIX, check_lcm_params, @@ -421,6 +429,11 @@ def resolve_decoder(encoding: str, message_type: type[Any]) -> DecoderDef: ) +def is_generic_cdr_encoding(encoding: str) -> bool: + """True for a generic CDR encoder unless a registered encoder owns the id.""" + return encoding.endswith(CDR_V1_SUFFIX) and encoding not in _encoders + + def is_generic_lcm_encoding(encoding: str) -> bool: """True for a *.lcm.v1 id the generic LCM codec serves (a registered @web_encoder with that id keeps its own params and encoder).""" @@ -446,6 +459,16 @@ def resolve_encoder(encoding: str, message_type: type[Any]) -> EncoderDef: "and qualified name" ) return definition + if encoding.endswith(CDR_V1_SUFFIX): + if encoding != f"{cdr_type_name(message_type)}{CDR_V1_SUFFIX}": + raise ValueError( + f"encoding {encoding!r} does not match the installed CDR type " + f"{message_type.__qualname__}" + ) + export_cdr_schema(message_type) + return EncoderDef( + encoding, message_type, encode_cdr_v1, takes_params=True, check_params=check_cdr_params + ) if encoding.endswith(LCM_V1_SUFFIX): if encoding != f"{lcm_type_name(message_type)}{LCM_V1_SUFFIX}": raise ValueError( diff --git a/dimos/web/relay_bridge/module_test_support.py b/dimos/web/relay_bridge/module_test_support.py index 7bb849106c3..51fdf38cbfc 100644 --- a/dimos/web/relay_bridge/module_test_support.py +++ b/dimos/web/relay_bridge/module_test_support.py @@ -27,9 +27,11 @@ import threading import time from typing import Any +from unittest.mock import Mock import pytest +from dimos.protocol.rpc.zenohrpc import ZenohRPC from dimos.web.relay_bridge import relay_bridge_module from dimos.web.relay_bridge.protocol import PROTOCOL_VERSION, DataFrame, IceServer, Msg, RtcTrack from dimos.web.relay_bridge.relay_bridge_module import RelayBridgeModule @@ -261,6 +263,10 @@ async def fake_fetch(base_url: str, **kwargs: Any) -> RelayInfo: monkeypatch.setattr(relay_bridge_module, "fetch_relay_info", fake_fetch) monkeypatch.setattr(RelayClient, "connect", fake_connect) + # Module construction starts RPC before the relay itself. Keep this + # no-network harness off native sockets as well as off WebTransport. + monkeypatch.setattr(ZenohRPC, "start", Mock()) + monkeypatch.setattr(ZenohRPC, "serve_module_rpc", Mock()) def make_bridge( diff --git a/dimos/web/relay_bridge/test_relay_bridge_authoring.py b/dimos/web/relay_bridge/test_relay_bridge_authoring.py index 8b59103dc61..2be80cec126 100644 --- a/dimos/web/relay_bridge/test_relay_bridge_authoring.py +++ b/dimos/web/relay_bridge/test_relay_bridge_authoring.py @@ -27,6 +27,7 @@ import struct from typing import Any +from dimos_cdrgen.registry import encode as encode_cdr from langchain_core.messages import AIMessage import numpy as np import pytest @@ -36,6 +37,7 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.resource_monitor.stats import ProcessStats, WorkerStats from dimos.core.stream import In, Out +from dimos.messages.geometry_msgs.msg import PoseStamped as GeneratedPoseStamped from dimos.msgs.geometry_msgs.PointStamped import PointStamped from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -68,6 +70,7 @@ Subs, ) from dimos.web.relay_bridge.relay_bridge_module import RelayBridgeModule, with_relay_bridge +from dimos.web.relay_bridge.tests.cdr_fixtures import build_messages # Composition helpers live at module level: under PEP 563 (`from __future__ @@ -212,6 +215,41 @@ def test_authored_specs_drive_generated_port(monkeypatch) -> None: stop_module(module) +def test_generated_cdr_is_encoded_only_for_subscribed_viewers(monkeypatch) -> None: + blueprint = cockpit( + channels=[ + Channel( + "generated_pose", + GeneratedPoseStamped, + encoding="geometry_msgs/msg/PoseStamped.cdr.v1", + max_hz=1000.0, + ) + ] + ) + module, clients = start_authored(monkeypatch, blueprint, wire=("generated_pose",)) + try: + transport = transport_of(module, "generated_pose") + msg = build_messages()[0][1] + transport.publish(msg) + flush_loop(module) + assert clients[0].frames == [] + push(module, clients[0], Subs(chs=["generated_pose"], n=1)) + assert wait_until(lambda: transport.subscribers) + transport.publish(msg) + assert wait_until(lambda: clients[0].frames) + channel, payload, delivery, meta = clients[0].frames[0] + assert (channel, payload, delivery, meta) == ( + "generated_pose", + encode_cdr(msg), + "reliable", + None, + ) + push(module, clients[0], Subs(chs=[], n=2)) + assert wait_until(lambda: not transport.subscribers) + finally: + stop_module(module) + + def test_two_jpeg_channels_use_independent_quality(monkeypatch) -> None: # The second camera the old single-int _jpeg_quality could not express. blueprint = cockpit( diff --git a/dimos/web/relay_bridge/tests/cdr_fixtures.py b/dimos/web/relay_bridge/tests/cdr_fixtures.py new file mode 100644 index 00000000000..b0719e5bcc7 --- /dev/null +++ b/dimos/web/relay_bridge/tests/cdr_fixtures.py @@ -0,0 +1,139 @@ +# 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. + +"""Regenerate browser fixtures with: python -m dimos.web.relay_bridge.tests.cdr_fixtures.""" + +import base64 +import json +from typing import Any + +from dimos_cdrgen.definitions import Definitions +from dimos_cdrgen.registry import encode as cdr_encode +import numpy as np + +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.dimos_msgs.msg import LineSegment3D, LineSegments3D +from dimos.messages.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Vector3 +from dimos.messages.sensor_msgs.msg import Image, Imu, JointState +from dimos.messages.std_msgs.msg import Empty, Header, Int64, String +from dimos.web.cdr_codec import CDR_V1_SUFFIX, export_schema +from dimos.web.relay_bridge.locate import find_web_dir + + +def build_messages() -> list[tuple[str, Any]]: + header = Header(stamp=Time(sec=1757400000, nanosec=500000123), frame_id="map") + return [ + ( + "pose_stamped", + PoseStamped( + header=header, + pose=Pose( + position=Point(x=1.5, y=-2.5, z=0.25), + orientation=Quaternion(z=0.6, w=0.8, x=0.0, y=0.0), + ), + ), + ), + ( + "custom_segments", + LineSegments3D( + header=header, + segments=[ + LineSegment3D( + start=Point(x=1, y=0.0, z=0.0), end=Point(y=2, x=0.0, z=0.0), weight=4 + ) + ], + ), + ), + ( + "image", + Image( + header=header, + width=2, + height=1, + encoding="rgb8", + step=6, + data=np.array([0, 1, 2, 253, 254, 255], dtype=np.uint8), + is_bigendian=0, + ), + ), + ( + "imu", + Imu( + header=header, + orientation_covariance=np.asarray(range(9), dtype=np.float64), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + angular_velocity=Vector3(x=0.0, y=0.0, z=0.0), + angular_velocity_covariance=np.zeros(9, dtype=np.float64), + linear_acceleration=Vector3(x=0.0, y=0.0, z=0.0), + linear_acceleration_covariance=np.zeros(9, dtype=np.float64), + ), + ), + ( + "joints", + JointState( + header=header, + name=["joint_a", "joint_b"], + position=np.array([1.5, -2.5], dtype=np.float64), + velocity=np.array([], dtype=np.float64), + effort=np.array([], dtype=np.float64), + ), + ), + ("int64", Int64(data=-9007199254740993)), + ("empty", Empty()), + ("string", String(data="robot 🦾 café")), + ] + + +def build_vectors() -> list[dict[str, Any]]: + definitions = {message.name: message for message in Definitions([], installed=True).resolve()} + + def walk(msg: Any) -> dict[str, Any]: + result: dict[str, Any] = {} + for field in definitions[msg.__msgtype__].fields: + value = getattr(msg, field.name) + convert = ( + walk + if field.type.nested + else ( + str + if field.type.name in ("int64", "uint64") + else lambda x: x.item() if isinstance(x, np.generic) else x + ) + ) + result[field.name] = ( + [convert(item) for item in value] if field.type.is_array else convert(value) + ) + return result + + return [ + { + "name": name, + "encoding": msg.__msgtype__ + CDR_V1_SUFFIX, + "schema": export_schema(type(msg)), + "value": walk(msg), + "payload_b64": base64.b64encode(cdr_encode(msg)).decode(), + "big_endian_b64": base64.b64encode(cdr_encode(msg, little_endian=False)).decode(), + } + for name, msg in build_messages() + ] + + +def main() -> None: + path = find_web_dir() / "sdk" / "tests" / "fixtures" / "cdr_frames.json" + path.write_text(json.dumps({"vectors": build_vectors()}, indent=2, ensure_ascii=False) + "\n") + print(path) + + +if __name__ == "__main__": + main() diff --git a/dimos/web/test_cdr_codec.py b/dimos/web/test_cdr_codec.py new file mode 100644 index 00000000000..9297de60208 --- /dev/null +++ b/dimos/web/test_cdr_codec.py @@ -0,0 +1,100 @@ +# 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. + +"""Web CDR schemas and bytes come from the installed owner, not another generator.""" + +import base64 +import json +import pickle +import subprocess +import sys +from typing import Any + +from dimos_cdrgen.registry import decode, encode +import pytest + +from dimos.messages.geometry_msgs.msg import Point, PoseStamped +from dimos.web.cdr_codec import check_cdr_params, encode_cdr_v1, export_schema +from dimos.web.cockpit import Channel, cockpit +from dimos.web.codecs import resolve_encoder +from dimos.web.relay_bridge.locate import find_web_dir +from dimos.web.relay_bridge.tests.cdr_fixtures import build_messages, build_vectors + + +def test_cdr_channel_embeds_schema_and_preserves_owned_encoder() -> None: + channel = Channel( + "pose", PoseStamped, encoding="geometry_msgs/msg/PoseStamped.cdr.v1", params={"note": "x"} + ) + blueprint = cockpit(channels=[channel]) + atom = blueprint.blueprints[0] + wire = atom.kwargs["manifest"]["channels"][0] + assert wire["params"] == {"note": "x", "cdr": export_schema(PoseStamped)} + assert "MSG: std_msgs/Header" in wire["params"]["cdr"]["definition"] + assert "MSG: builtin_interfaces/Time" in wire["params"]["cdr"]["definition"] + assert dict(channel.params) == {"note": "x"} + spec = pickle.loads(pickle.dumps(blueprint)).blueprints[0].kwargs["channels"][0] + msg = build_messages()[0][1] + assert spec.encoder is encode_cdr_v1 + assert spec.encoder(msg, spec.params) == encode(msg) + assert next(stream for stream in atom.streams if stream.name == "pose").type is PoseStamped + + +def test_cdr_channel_rejects_foreign_type_and_reserved_schema() -> None: + with pytest.raises(ValueError, match="does not match"): + resolve_encoder("geometry_msgs/msg/Point.cdr.v1", PoseStamped) + with pytest.raises(ValueError, match="reserved"): + cockpit( + channels=[ + Channel( + "pose", + PoseStamped, + encoding="geometry_msgs/msg/PoseStamped.cdr.v1", + params={"cdr": {}}, + ) + ] + ) + with pytest.raises(ValueError, match="declared channel type"): + encode_cdr_v1(Point(x=0, y=0, z=0), {"cdr": export_schema(PoseStamped)}) + with pytest.raises(ValueError, match="no generated CDR schema"): + export_schema(dict) + + +@pytest.mark.parametrize("params", [{}, {"cdr": {}}, {"cdr": {"type": "p.T", "definition": ""}}]) +def test_missing_schema_is_rejected(params: dict[str, Any]) -> None: + with pytest.raises(ValueError, match="params"): + check_cdr_params(params) + + +def test_browser_vectors_are_actual_generated_messages_in_both_byte_orders() -> None: + vectors = json.loads((find_web_dir() / "sdk/tests/fixtures/cdr_frames.json").read_text())[ + "vectors" + ] + assert vectors == build_vectors() + for (_, msg), vector in zip(build_messages(), vectors, strict=True): + for key, little in [("payload_b64", True), ("big_endian_b64", False)]: + payload = base64.b64decode(vector[key]) + restored = decode(payload, type(msg)) + assert type(restored) is type(msg) + assert encode(restored, little_endian=little) == payload + + +def test_codec_import_has_no_generated_or_numpy_side_effects() -> None: + subprocess.run( + [ + sys.executable, + "-c", + "import sys; import dimos.web.cdr_codec; assert 'numpy' not in sys.modules; assert 'dimos.messages' not in sys.modules", + ], + check=True, + ) diff --git a/docs/development/message-source-packages.md b/docs/development/message-source-packages.md index 6d20d3f04ae..9fd42043ee1 100644 --- a/docs/development/message-source-packages.md +++ b/docs/development/message-source-packages.md @@ -1,58 +1,95 @@ # CDR generation in dimOS -> **WIP: CDR migration.** This page documents the generator integration in -> [the first stack layer (#4247)](https://github.com/dimensionalOS/dimos/pull/4247), -> not a complete CDR message/runtime workflow on `main`. The generator dependency, -> dimOS adapters and verification commands below are implemented in this layer. -> Built-in message packaging depends on -> [the second layer (#4366)](https://github.com/dimensionalOS/dimos/pull/4366); -> the runtime encoding switch and CDR recording/viewer flows belong to later layers. -> Installing this layer alone does not switch the existing dimOS runtime to CDR. - -dimOS uses the published `dimos-cdrgen==0.1.0` package directly. Its parser, -generators, ownership checks, codecs and native build recipes live in the -[independent library](https://github.com/TomCC7/dimos-cdrgen). -See its [Python, C++ and Rust guides](https://tomcc7.github.io/dimos-cdrgen/) -for message package configuration and supported schema features. - -Install `dimos-cdrgen==0.1.0` in both the build and runtime dependencies of -message packages. Import its APIs directly: - -```python -from dimos_cdrgen.registry import decode, encode -from dimos_cdrgen.source_package import setup_arguments +**WIP:** Built-in messages work in standalone examples. CDR transport and application +support are incomplete. Existing code continues to use `dimos.msgs`. + +`setup.py` reads the definitions in `packages/dimos-generated` and calls +`dimos-cdrgen==0.1.2`. The wheel contains `dimos.messages`, schemas, and native +source code. The examples use these installed files. + +The [cdrgen guides](https://tomcc7.github.io/dimos-cdrgen/) describe its APIs and +supported schemas. `dimos.message_codegen.native_build.write_cmake_toolchain` +adds paths for the dimOS SDK. + +## Run the built-in examples + +Run all commands below from the repository root. Install `uv` and a C++ compiler +first; the compiler is needed by dimOS's existing A* extension even for a source +installation used only by Python. The examples use Python 3.12. + +### Install and run Python + +```sh skip +uv venv .venv-msg-review --python 3.12 +DIMOS_ALLOW_MISSING_COCKPIT=1 uv pip install --python .venv-msg-review/bin/python . +.venv-msg-review/bin/python -I examples/message-package/demo_installed_wheel.py +``` + +`DIMOS_ALLOW_MISSING_COCKPIT=1` is the existing option to install without prepared +web UI bundles. Omit it when building a complete distribution with those assets. +The example imports the installed package, prints the generated Python and +schema/native-source directories, and checks image and custom-message CDR roundtrips. +Expected output includes: + +```text +Generated Python messages: /lib/python3.12/site-packages/dimos/messages +Packaged schemas and native sources: /lib/python3.12/site-packages/dimos/messages_schemas/package +Messages: 73 types +PASS: sensor_msgs/msg/Image pixels, encoding and source stamp +PASS: dimos_msgs/msg/LineSegments3D geometry, weight and nested type identity +PASS: packaged definitions and C++/Rust build inputs; no compiled message libraries +``` + +The generated directories are installation outputs, not files to edit. Message +inputs live under `packages/dimos-generated` in the checkout. + +### Run C++ and Rust + +Install CMake and Rust/Cargo, then reuse the same environment: + +```sh skip +uv pip install --python .venv-msg-review/bin/python 'dimos-cdrgen[native]==0.1.2' +.venv-msg-review/bin/python -I examples/message-package/demo_native.py +``` + +This one command compiles the ordinary CMake and Cargo consumers in +`examples/message-package/native`. Python writes an installed built-in Header, +C++ appends `/cpp` to its frame ID, Rust appends `/rust`, and Python verifies +`map/cpp/rust` with the timestamp unchanged. Expected final output: + +```text +PASS: built-in Header Python -> C++ -> Rust -> Python ``` -There is no dimOS compatibility distribution. Existing consumers of -`dimos_message_build` or generic `dimos.message_codegen` APIs must change their -imports and dependency declarations to `dimos_cdrgen` and `dimos-cdrgen`. +The script also prints the installed native-source directory. Temporary example +files are cleaned up. Native support builds use `~/.cache/dimos/messages`; +`DIMOS_MESSAGE_NATIVE_CACHE` overrides that location. Set `CARGO_TARGET_DIR` to +retain Rust build artifacts. Cold builds download pinned dependencies; +`DIMOS_OFFLINE=1` requires populated native and Cargo caches. -dimOS retains two adapters: +### What installation does -- `dimos.message_codegen.native_build.write_cmake_toolchain` adds SDK discovery - for installed `dimos/_native/cpp` and source-checkout `native/cpp` layouts. - Import other build functions directly from `dimos_cdrgen.native_build`. -- `dimos/native/cdr_codec.hpp` exposes the library codec under `dimos::native` - for dimOS C++ consumers. +Definitions → setuptools calls cdrgen → Python/schema/native-source +artifacts in the dimOS wheel → installed consumers. Source installation prepares +verified definitions and generates artifacts. Wheel installation only installs +those artifacts. The sdist includes definitions needed to rebuild the wheel. -This first stack layer does not switch runtime transports or supply built-in -message catalogs. Those remain separate PRs. +Installation does not compile or test the C++/Rust message consumers. Running +`demo_native.py` performs that verification explicitly. These examples use files, +with no robot connection, transport or SDK pub/sub. No dedicated native-message +CI is required to install or run them. ## Verify the integration Use a Python environment with the published generator and test tools: -```sh -uv pip install 'dimos-cdrgen[native]==0.1.0' numpy build pytest pytest-asyncio pytest-env -python -m pytest --noconftest -o addopts='' \ - -o 'markers=native_e2e: compiled native consumer acceptance' \ - -q dimos/message_codegen/test_integration.py +```sh skip +uv pip install 'dimos-cdrgen==0.1.2' numpy build pytest pytest-asyncio pytest-env +python -m pytest --noconftest -o addopts='' -q dimos/message_codegen/test_integration.py ``` -The tests check SDK discovery, an installed message wheel built from a source -distribution, Python type identity and the dimOS C++ header against literal CDR -bytes. Native verification needs CMake and a C++17 compiler. It prepares pinned -native dependencies in a temporary cache; use `DIMOS_NATIVE_TEST_CACHE` to reuse -a writable cache, or `DIMOS_NATIVE_SOURCE_DIRS` with `DIMOS_OFFLINE=1` for -pre-fetched source directories. Generic generator regression tests stay in the -library repository. +These checks cover SDK path discovery and an installed Python message consumer. +Native message generation/compilation is not run by CI. To manually verify the +installed built-in messages across Python, C++ and Rust, run the +[built-in example above](#run-c-and-rust). +Generic generator regression tests belong to the cdrgen repository. diff --git a/docs/development/messages-external-project.md b/docs/development/messages-external-project.md new file mode 100644 index 00000000000..7452454738a --- /dev/null +++ b/docs/development/messages-external-project.md @@ -0,0 +1,50 @@ +# External message projects + +An external project owns its `.msg` definitions and registers its generated +message provider through the `dimos.messages` entry-point group. + +The [standalone example](/examples/external-messages/README.md) declares +dimOS as both a build and runtime dependency and `dimos.messages` as its message +dependency. Its small `setup.py` passes cdrgen's `setup_arguments` to setuptools. +Configuration, generation, provider registration and packaging belong to cdrgen; +dimOS does not implement another backend or generator. + +## Install and run + +Run from the repository root. First complete the +[built-in Python installation](/docs/development/message-source-packages.md#install-and-run-python) +to create `.venv-msg-review` with this checkout's dimOS package. No CMake or Cargo +is needed for this external registration example. + +```sh skip +uv pip install --python .venv-msg-review/bin/python 'setuptools>=70' wheel +uv pip install --python .venv-msg-review/bin/python --no-build-isolation ./examples/external-messages +.venv-msg-review/bin/python -I examples/external-messages/demo_reading.py +``` + +`--no-build-isolation` uses the matching dimOS already installed in this environment. +Ordinary isolated source, wheel, sdist and editable installs are also supported +when pip has access to that matching review wheel. An older release with the same +version cannot supply these built-ins. + +Expected output: + +```text +Installed external message sources: /lib/python3.12/site-packages/acme_telemetry_schemas/package +PASS: external message registered; CDR roundtrip reuses built-in Header +``` + +The example verifies a single registered provider, a custom-message CDR roundtrip, +and exact identity of the Header class supplied by dimOS. + +## Edit a definition + +Edit `examples/external-messages/interfaces/telemetry_msgs/msg/DeviceReading.msg`, +then rerun the installation command and restart Python processes. Generated +artifacts are outputs; edit the `.msg` source instead. Editable installations +also need reinstalling after definitions change. Tests cover field changes, +renames and deletion of obsolete generated types. + +The native built-in example is documented in +[Run C++ and Rust](/docs/development/message-source-packages.md#run-c-and-rust). +Transport and SDK integration are separate features. diff --git a/docs/usage/blueprints.md b/docs/usage/blueprints.md index 5420afc204b..dbaacccf50a 100644 --- a/docs/usage/blueprints.md +++ b/docs/usage/blueprints.md @@ -153,7 +153,7 @@ from dimos.core.coordination.blueprints import Blueprint, autoconnect from dimos.core.core import rpc from dimos.core.module import Module from dimos.core.stream import Out, In -from dimos.msgs.sensor_msgs import Image +from dimos.msgs.sensor_msgs.Image import Image class ModuleA(Module): image: Out[Image] @@ -214,7 +214,7 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.core import rpc from dimos.core.module import Module from dimos.core.stream import Out, In -from dimos.msgs.sensor_msgs import Image +from dimos.msgs.sensor_msgs.Image import Image class ConnectionModule(Module): color_image: Out[Image] # Outputs on 'color_image' diff --git a/docs/usage/visualization.md b/docs/usage/visualization.md index 4b3fe350685..9a98cc605b3 100644 --- a/docs/usage/visualization.md +++ b/docs/usage/visualization.md @@ -147,3 +147,57 @@ When a `RerunBridgeModule` is already part of your blueprint, you typically don' Rerun on `dev` is **module-driven**: modules decide what to log, and `Blueprint.build()` sets up the shared viewer + default layout. To use a custom websocket port, set `--rerunwebsocketserver.port`; the bridge in the same blueprint takes the viewer URL from that server. `--rerunbridgemodule.websocket-url` points a bridge at a websocket server elsewhere. Set `host` on each module to override its bind address; both default to the shared `listen_host`. + +## Generated CDR viewer demo (WIP) + +The CDR viewer is under development. This example runs the real Rerun bridge +with local Zenoh transports. It needs no robot or ROS installation. + +From the repository root: + +```sh skip +DIMOS_ALLOW_MISSING_COCKPIT=1 uv sync --locked --extra visualization --extra web +uv run --extra visualization --extra web python -I examples/message-viewer/demo_cdr_rerun.py cdr-viewer.rrd +uv run --extra visualization rerun cdr-viewer.rrd +uv run --extra visualization rerun cdr-viewer.mcap +``` + +Use a new output filename for each run. The example creates the live RRD and +`cdr-viewer.mcap`. Both destinations must be new. The build flag permits +installation without prebuilt browser assets; message generation still runs. + +The live recording uses `RerunBridgeModule.start()`, its typed subscriptions and +rendering callbacks. A receipt signal waits for each message before shutdown. +The MCAP contains the source scene encoded as CDR with complete ROS2 schemas. +It is example input for playback, not a capture by the native recorder. + +Open the MCAP directly with Rerun's native importer. To save its imported data: + +```sh skip +uv run --extra visualization rerun mcap convert cdr-viewer.mcap -o cdr-viewer-native.rrd +``` + +Rerun 0.32 converts supported standard ROS2 types into visualizations. Other +messages, including this scene's Path and custom EntityMarkers, are available +as reflected fields; they do not automatically acquire the live bridge's path +or labeled-entity display. No dimOS Python renderer is used for this import. +Existing `dimos mem rerun` behavior for legacy stores, including `--seconds` +and `--root`, is unchanged. Use the native Rerun command for CDR MCAP files. + +| PR capability | Demo scene | What to check | +| --- | --- | --- | +| TF and source metadata | `tf`, `pose` | Map/camera frames, pose axes at `[1.5, -2.5, 0.25]`, `nanosec: 123456789` in the text tab | +| Geometry adapters | `point`, `odometry`, `path`, `entities` | Point, three-waypoint path, odometry axes, labeled person and object | +| Camera calibration | `image`, `late_image` | Pinhole on each image, whether calibration arrives first or last | +| Sensor buffers | `image`, `depth`, `compressed`, `compressed_depth` | Four RGB pixels, big-endian uint16 depth, PNG image and JXL depth in the sensor tab | +| Cloud rendering | `cloud`, `cloud_boxes`, `cloud_height` | RGB points, boxes and height-colored spheres | +| Legacy rendering | `legacy_pose` | Existing pose rendering alongside generated messages | +| Native MCAP import | `cdr-viewer.mcap` | Standard images, points, calibration and TF; reflected fields for types without a native visualization | + +The example anchors its `world` TF frame to the viewer root. Business producers +must supply their own TF tree. It does not demonstrate robot graph migration or +automatic CDR type discovery. The existing integration test runs this example, +checks the live RRD, then converts the MCAP with the official CLI and checks its imported components. + +For the live browser SDK and manifest path, run the +[Web bridge demo](/docs/web/bridge.md#generated-cdr-messages-wip). diff --git a/docs/web/bridge.md b/docs/web/bridge.md index 651b513e5bf..41b68e161d2 100644 --- a/docs/web/bridge.md +++ b/docs/web/bridge.md @@ -40,6 +40,83 @@ The relay flags are `dimos run` options. Most of the rest are fields of the brid ## Channels +### Generated CDR messages (WIP) + +An installed generated type can be sent as CDR with its complete ROS2 definition +in the manifest. Declare the encoding explicitly in this proposal: + +```python +from dimos.messages.geometry_msgs.msg import PoseStamped +from dimos.web.cockpit import Channel, cockpit + +viewer = cockpit(channels=[Channel( + "measured_pose", PoseStamped, + encoding="geometry_msgs/msg/PoseStamped.cdr.v1", +)]) +``` + +Compose `viewer` with the module producing `measured_pose: Out[PoseStamped]`. +Generated ports use the shared CDR codec on the selected LCM or Zenoh transport. The manifest embeds PoseStamped, Header and Time definitions from +the installed message owner; the browser needs neither ROS nor another generated +message package. Registered custom web encoders continue to take precedence. +Existing LCM/JSON channels and default transport selection are unchanged here. + +The SDK registry resolves the channel schema once and decodes its CDR frames: + +```ts +import { createDecoderRegistry } from "@dimos/sdk"; + +const decode = createDecoderRegistry().resolve(channel); +const value = decode?.(payload, frameHeader).value; +``` + +`channel` is the adopted manifest record, `payload` the frame's `Uint8Array` and +`frameHeader` its SDK header. Invalid schemas are unsupported; malformed CDR +frames fail decoding. The raw cockpit table previews small messages. Messages +with variable-length arrays require a visible panel subscription so a preview +does not pull a large image/cloud buffer. A decoder alone does not add a typed +camera, map or control panel. + +To run a real generated-message source, cockpit bridge and local relay, from +the repository root in terminal 1: + +```sh skip +DIMOS_ALLOW_MISSING_COCKPIT=1 uv sync --locked --extra visualization --extra web +uv run --extra web python -I examples/message-viewer/demo_cdr_browser.py +``` + +In terminal 2: + +```sh skip +cd web/sdk +deno task fixture --host 127.0.0.1 +``` + +Open `http://127.0.0.1:5174/demo_cdr.html?live=1`. Wait for `connected`, +`Received 8/8 channels`, increasing frame counts and zero decode errors. +Select each channel. Check its fields and expand its embedded ROS2 definition: + +- `pose_stamped`: position `[1.5, -2.5, 0.25]` and `nanosec: 500000123`. +- `custom_segments`: custom nested message fields. +- `image`: two near-black/near-white pixels, with bytes `[0, 1, 2, 253, 254, 255]`. +- `imu`, `joints`: fixed and variable arrays. +- `int64`: `-9007199254740993n`, with no JavaScript number rounding. +- `empty`, `string`: an empty message and Unicode text. + +This path uses actual generated Python messages, typed Zenoh transport, +`cockpit()` manifest creation, relay framing, SDK subscriptions and CDR decoding. +The page explicitly subscribes to all demo channels, including arrays. It does +not add domain-specific cockpit panels or browser-to-robot CDR publishing. +Stop terminal 1 with Ctrl-C. Use `--port` to select a different relay port and +open the exact URL printed by the program. + +Without `?live=1`, the same page uses fixed packets to inspect little- and +big-endian decoding. Generate `web/sdk/tests/fixtures/cdr_frames.json` with +`python -m dimos.web.relay_bridge.tests.cdr_fixtures`. SDK tests check both byte +orders; the fixture does not validate a live connection. + +### Existing channels + A channel is one stream crossing the wire in one direction with one encoding. `rx` channels flow from the robot to the browser, `tx` channels from the browser to the robot. The bridge has four built-in ports: | Channel | Type | Encoding | Delivery | diff --git a/docs/web/development.md b/docs/web/development.md index f09ef74aea7..e33c5c79e29 100644 --- a/docs/web/development.md +++ b/docs/web/development.md @@ -56,7 +56,7 @@ After changing cockpit or SDK dependencies, run `deno install` in `web/` and com Without vite, `--local-relay` serves the built `cockpit/dist` at `/` and `sdk/dist/sdk.js` at `/sdk.js`. In a checkout, `ensure_web_dist` in [`relay_process.py`](/dimos/web/relay_bridge/relay_process.py#L76) builds both first when either is missing or older than the sources. It stamps the shared, SDK and cockpit sources, the workspace config and the lockfile, builds under a cross-process lock into temporary directories, and swaps both products in together. -Release wheels ship both bundles prebuilt inside `dimos/web/relay_bridge/_relay_dist` (built by the release workflow, see [`setup.py`](/setup.py#L116)), so a pip-installed dimos never builds or downloads npm packages. +Release wheels ship both bundles prebuilt inside `dimos/web/relay_bridge/_relay_dist` (built by the release workflow, see [root packaging configuration](/setup.py#L116)), so a pip-installed dimos never builds or downloads npm packages. Two overrides: `--no-web-build` skips the staleness check and the build, and `DIMOS_WEB_DIR` points the bridge at another web tree. diff --git a/examples/cdr-recording/README.md b/examples/cdr-recording/README.md new file mode 100644 index 00000000000..ede61c00def --- /dev/null +++ b/examples/cdr-recording/README.md @@ -0,0 +1,87 @@ +# Record a standard ROS 2 message + +**WIP:** this example tests the CDR recording path. Business blueprints have not +been migrated. No robot, ROS installation, or API key is required. + +The example publishes `geometry_msgs/msg/PoseStamped` at 10 Hz over Zenoh. Its +pose moves around a one-metre circle in the `map` frame. The normal DimOS CLI +starts the Rust recorder, which stores the original CDR bytes in MCAP. Python +does not write the recording. + +## Install from this PR + +Use Linux, Python 3.12, [uv](https://docs.astral.sh/uv/getting-started/installation/), +and a current stable [Rust toolchain](https://rustup.rs/). The native build needs +a C/C++ compiler, CMake, pkg-config, and NASM. On Ubuntu: + +```bash +sudo apt-get install build-essential cmake pkg-config nasm libturbojpeg0 libgl1 libglib2.0-0 git-lfs +git lfs install +git clone --branch cc/feat/cdr-unit-recording https://github.com/dimensionalOS/dimos.git dimos-cdr-review +cd dimos-cdr-review +``` + +If you already checked out this PR, start at its repository root instead. All +commands below run from that root; no earlier PR checkout is needed. + +```bash +uv venv .venv-cdr-recording --python 3.12 +DIMOS_ALLOW_MISSING_COCKPIT=1 uv pip install --python .venv-cdr-recording/bin/python -e . -e examples/cdr-recording +source .venv-cdr-recording/bin/activate +cargo build --locked --manifest-path dimos/experimental/memory/rust/Cargo.toml --target-dir build/cdr-recorder -j2 +``` + +The install generates the built-in messages. `DIMOS_ALLOW_MISSING_COCKPIT=1` +allows this headless example to install without a built web frontend. The small +example package registers a blueprint with the existing `dimos.blueprints` entry +point mechanism. + +## Record + +```bash +dimos --transport zenoh --viewer none --n-workers 1 \ + --record mcap --record-engine rust --record-topics pose \ + --record-executable "$PWD/build/cdr-recorder/debug/dimos-memory-recorder" \ + run dimos-cdr-recording-example.pose +``` + +If DimOS offers to raise the shared-memory limit, answer **N**. This small +example works without that system change. + +Wait for `Experimental Rust recorder ready`. Let the example run for about +15 seconds, then press **Ctrl+C once**. Wait for shutdown to finish. The +`Recording saved` log prints the full `artifact_path` to `memory.mcap`. + +The recorder binary comes from this checkout. `--record-executable` selects it +without invoking the default Nix build. Zenoh uses the normal local discovery +settings; this example does not connect to robot hardware. + +## Inspect in Foxglove + +1. Open the printed `memory.mcap` file in Foxglove. +2. Add a **Raw Messages** panel and select the `pose` topic. +3. Confirm the schema is `geometry_msgs/msg/PoseStamped`, `header.frame_id` is + `map`, and `pose.position.x` and `pose.position.y` change during playback. +4. Add a **3D** panel. Set the display frame to `map` and enable `pose`. +5. Play the recording. The pose arrow moves around a circle. No TF topic is + required because the pose and display frame are the same. + +The MCAP contains a `ros2msg` schema with the PoseStamped definition and all its +nested definitions. Its channel uses `cdr` encoding. Foxglove reads that schema +and decodes the recorded bytes; it does not need the DimOS Python package. + +Recording times use reception time. The original `header.stamp` remains in the +payload. See [Foxglove's supported pose types](https://docs.foxglove.dev/docs/visualization/panels/3d#pose). + +## Read with Python + +Replace the path below with the printed `artifact_path`: + +```python +from dimos.memory.store.cdr_mcap import CdrMcapStore + +with CdrMcapStore(path="/path/from/log/memory.mcap") as store: + print(store.list_streams()) # ['pose'] + observation = next(iter(store.stream("pose"))) + print(observation.ts, observation.data) +``` diff --git a/examples/cdr-recording/cdr_recording_example.py b/examples/cdr-recording/cdr_recording_example.py new file mode 100644 index 00000000000..2f8eac9303b --- /dev/null +++ b/examples/cdr-recording/cdr_recording_example.py @@ -0,0 +1,53 @@ +# 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. + +"""Publish a standard ROS 2 pose for the CLI Rust recorder.""" + +import math +import time + +from reactivex import interval + +from dimos.core.core import rpc +from dimos.core.module import Module +from dimos.core.stream import Out +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos.messages.std_msgs.msg import Header + + +class MovingPose(Module): + """Publish a pose moving around a one-metre circle at 10 Hz.""" + + pose: Out[PoseStamped] + + @rpc + def start(self) -> None: + super().start() + self.register_disposable(interval(0.1).subscribe(self._publish)) + + def _publish(self, tick: int) -> None: + angle = tick * 0.05 + sec, nanosec = divmod(time.time_ns(), 1_000_000_000) + self.pose.publish( + PoseStamped( + header=Header(stamp=Time(sec=sec, nanosec=nanosec), frame_id="map"), + pose=Pose( + position=Point(x=math.cos(angle), y=math.sin(angle), z=0.0), + orientation=Quaternion( + x=0.0, y=0.0, z=math.sin(angle / 2), w=math.cos(angle / 2) + ), + ), + ) + ) diff --git a/examples/cdr-recording/pyproject.toml b/examples/cdr-recording/pyproject.toml new file mode 100644 index 00000000000..9c5367471b7 --- /dev/null +++ b/examples/cdr-recording/pyproject.toml @@ -0,0 +1,15 @@ +[build-system] +requires = ["setuptools>=70"] +build-backend = "setuptools.build_meta" + +[project] +name = "dimos-cdr-recording-example" +version = "0.1.0" +requires-python = ">=3.12" +dependencies = ["dimos", "mcap>=1.2"] + +[project.entry-points."dimos.blueprints"] +pose = "cdr_recording_example:MovingPose" + +[tool.setuptools] +py-modules = ["cdr_recording_example"] diff --git a/examples/external-messages/.gitignore b/examples/external-messages/.gitignore new file mode 100644 index 00000000000..e2cae228c89 --- /dev/null +++ b/examples/external-messages/.gitignore @@ -0,0 +1,2 @@ +/build/ +/dist/ diff --git a/examples/external-messages/README.md b/examples/external-messages/README.md new file mode 100644 index 00000000000..b8286f0217a --- /dev/null +++ b/examples/external-messages/README.md @@ -0,0 +1,8 @@ +# External message registration example + +`DeviceReading.msg` defines an external message that reuses the installed DimOS +Header. `demo_reading.py` checks registration, a CDR roundtrip and type identity. + +See the [installation and run instructions](../../docs/development/messages-external-project.md#install-and-run) +for complete commands using the built-in example environment, expected output +and how to reinstall after changing a definition. diff --git a/examples/external-messages/demo_reading.py b/examples/external-messages/demo_reading.py new file mode 100644 index 00000000000..61b94c53829 --- /dev/null +++ b/examples/external-messages/demo_reading.py @@ -0,0 +1,40 @@ +# 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. + +"""Register an external message and reuse the installed built-in Header.""" + +from importlib.metadata import entry_points + +from acme_telemetry.telemetry_msgs.msg import DeviceReading +from acme_telemetry_schemas.provider import package_root +from dimos_cdrgen.registry import decode, encode + +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.std_msgs.msg import Header + + +def main() -> None: + assert ( + len([ep for ep in entry_points(group="dimos.messages") if ep.name == "acme_telemetry"]) == 1 + ) + value = DeviceReading(Header(Time(17, 23), "map"), 7, 2.5, "sensor") + restored = decode(encode(value), DeviceReading) + assert restored == value + assert type(restored.header) is Header + print(f"Installed external message sources: {package_root()}") + print("PASS: external message registered; CDR roundtrip reuses built-in Header") + + +if __name__ == "__main__": + main() diff --git a/examples/external-messages/interfaces/telemetry_msgs/msg/DeviceReading.msg b/examples/external-messages/interfaces/telemetry_msgs/msg/DeviceReading.msg new file mode 100644 index 00000000000..1a307878127 --- /dev/null +++ b/examples/external-messages/interfaces/telemetry_msgs/msg/DeviceReading.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +uint32 sequence +float64 value +string label diff --git a/examples/external-messages/pyproject.toml b/examples/external-messages/pyproject.toml new file mode 100644 index 00000000000..20979c33330 --- /dev/null +++ b/examples/external-messages/pyproject.toml @@ -0,0 +1,13 @@ +[build-system] +requires = ["setuptools>=70", "wheel", "dimos-cdrgen==0.1.2", "dimos==0.0.14"] +build-backend = "setuptools.build_meta" + +[project] +name = "acme-telemetry" +version = "0.1.0" +description = "An external message reusing the installed DimOS Header" +requires-python = ">=3.12" +dependencies = ["dimos==0.0.14", "dimos-cdrgen==0.1.2"] + +[tool.dimos.messages.dependencies] +"dimos.messages" = "0.0.14" diff --git a/examples/external-messages/setup.py b/examples/external-messages/setup.py new file mode 100644 index 00000000000..6034502d921 --- /dev/null +++ b/examples/external-messages/setup.py @@ -0,0 +1,20 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from pathlib import Path + +from dimos_cdrgen.source_package import setup_arguments +from setuptools import setup + +setup(**setup_arguments(Path(__file__).resolve().parent)) diff --git a/examples/message-package/README.md b/examples/message-package/README.md new file mode 100644 index 00000000000..5228169e39a --- /dev/null +++ b/examples/message-package/README.md @@ -0,0 +1,11 @@ +# Built-in message examples + +**WIP:** CDR transport and application support are incomplete. + +- `demo_installed_wheel.py` checks installed Python messages and CDR roundtrips. +- `demo_native.py` sends a built-in Header through Python, C++, Rust, and back to Python. +- `native/` contains the CMake and Cargo source files for `demo_native.py`. + +Follow the [example tutorial](/docs/development/message-source-packages.md#run-the-built-in-examples) +for install commands, expected output, and generated-file locations. +The examples do not need an external message project or a robot. diff --git a/examples/message-package/demo_installed_wheel.py b/examples/message-package/demo_installed_wheel.py new file mode 100644 index 00000000000..0ad1208af0e --- /dev/null +++ b/examples/message-package/demo_installed_wheel.py @@ -0,0 +1,92 @@ +# 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. + +"""Verify an installed message wheel from a clean environment outside the checkout.""" + +from importlib.metadata import distribution, entry_points +from pathlib import Path +import sys + +from dimos_cdrgen.registry import decode, encode +import numpy as np + +import dimos.messages +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.dimos_msgs.msg import LineSegment3D, LineSegments3D +from dimos.messages.geometry_msgs.msg import Point +from dimos.messages.sensor_msgs.msg import Image +from dimos.messages.std_msgs.msg import Header +from dimos.messages_schemas.provider import message_types, package_root + + +def main() -> None: + assert sys.prefix != sys.base_prefix, "Run in the clean verification environment" + assert dimos.messages.__file__ is not None + installed = Path(dimos.messages.__file__).resolve() + assert installed.is_relative_to(Path(sys.prefix).resolve()), installed + owner = distribution("dimos") + assert "dimos/messages/std_msgs/msg/__init__.py" in {str(p) for p in owner.files} + assert len([ep for ep in entry_points(group="dimos.messages") if ep.name == "dimos"]) == 1 + print(f"Installed package: {installed}") + print(f"Generated Python messages: {installed.parent}") + print(f"Packaged schemas and native sources: {package_root()}") + print(f"Messages: {len(message_types())} types") + + image = Image( + header=Header(stamp=Time(sec=17, nanosec=23), frame_id="camera"), + height=2, + width=3, + encoding="rgb8", + is_bigendian=0, + step=9, + data=np.arange(18, dtype=np.uint8), + ) + restored_image = decode(encode(image), Image) + assert type(restored_image) is Image and type(restored_image.header) is Header + assert restored_image.header == Header(Time(17, 23), "camera") + assert (restored_image.height, restored_image.width, restored_image.step) == (2, 3, 9) + assert restored_image.encoding == "rgb8" and restored_image.is_bigendian == 0 + np.testing.assert_array_equal(restored_image.data, np.arange(18, dtype=np.uint8)) + print("PASS: sensor_msgs/msg/Image pixels, encoding and source stamp") + + segments = LineSegments3D( + header=Header(stamp=Time(sec=17, nanosec=23), frame_id="map"), + segments=[LineSegment3D(start=Point(1.0, 2.0, 3.0), end=Point(4.0, 5.0, 6.0), weight=0.75)], + ) + restored_segments = decode(encode(segments), LineSegments3D) + assert restored_segments == segments + assert type(restored_segments.header) is Header + assert type(restored_segments.segments[0].start) is Point + print("PASS: dimos_msgs/msg/LineSegments3D geometry, weight and nested type identity") + + resources = package_root() + for relative in ( + "schemas/sensor_msgs/msg/Image.msg", + "schemas/dimos_msgs/msg/LineSegments3D.msg", + "cpp/CMakeLists.txt", + "cpp/sensor_msgs/msg/Image.msg", + "rust/Cargo.toml", + "rust/build.rs", + "rust/interfaces/dimos_msgs/msg/LineSegments3D.msg", + ): + assert (resources / relative).is_file(), relative + assert not any( + path.suffix in {".so", ".a", ".dylib", ".dll", ".rlib", ".pyd"} + for path in resources.rglob("*") + ) + print("PASS: packaged definitions and C++/Rust build inputs; no compiled message libraries") + + +if __name__ == "__main__": + main() diff --git a/examples/message-package/demo_native.py b/examples/message-package/demo_native.py new file mode 100644 index 00000000000..9933af7c140 --- /dev/null +++ b/examples/message-package/demo_native.py @@ -0,0 +1,74 @@ +# 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. + +"""Exchange an installed built-in Header through Python, C++ and Rust.""" + +import json +import os +from pathlib import Path +import shutil +import subprocess +from tempfile import TemporaryDirectory + +from dimos_cdrgen.native_build import prepare_cpp, write_cmake_toolchain +from dimos_cdrgen.registry import decode, encode + +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.std_msgs.msg import Header +from dimos.messages_schemas.provider import package_root # type: ignore[import-untyped] + + +def main() -> None: + for tool in ("cmake", "cargo"): + if shutil.which(tool) is None: + raise SystemExit(f"Install {tool} before running this native example") + sources = package_root() + print(f"Installed native message sources: {sources}", flush=True) + offline = os.environ.get("DIMOS_OFFLINE") == "1" + cache = Path( + os.environ.get("DIMOS_MESSAGE_NATIVE_CACHE", Path.home() / ".cache/dimos/messages") + ) + prefix = prepare_cpp(sources, cache=cache, offline=offline) + with TemporaryDirectory(prefix="dimos-builtin-example-") as temporary: + project = Path(temporary) + shutil.copytree(Path(__file__).with_name("native"), project, dirs_exist_ok=True) + toolchain = write_cmake_toolchain(prefix, project / "toolchain.cmake") + (project / "python.cdr").write_bytes(encode(Header(Time(17, 23), "map"))) + commands = [ + ["cmake", "-S", ".", "-B", "build", f"-DCMAKE_TOOLCHAIN_FILE={toolchain}"], + ["cmake", "--build", "build", "--parallel", "2"], + ["build/header_relay", "python.cdr", "cpp.cdr"], + [ + "cargo", + "--config", + "patch.crates-io.dimos-generated-messages.path=" + + json.dumps(str(sources / "rust")), + "run", + *(["--offline"] if offline else []), + "--example", + "relay", + "--", + "cpp.cdr", + "rust.cdr", + ], + ] + for command in commands: + subprocess.run(command, cwd=project, check=True) + value = decode((project / "rust.cdr").read_bytes(), Header) + assert value == Header(Time(17, 23), "map/cpp/rust"), value + print("PASS: built-in Header Python -> C++ -> Rust -> Python") + + +if __name__ == "__main__": + main() diff --git a/examples/message-package/native/CMakeLists.txt b/examples/message-package/native/CMakeLists.txt new file mode 100644 index 00000000000..429ba104129 --- /dev/null +++ b/examples/message-package/native/CMakeLists.txt @@ -0,0 +1,5 @@ +cmake_minimum_required(VERSION 3.20) +project(builtin_message_example LANGUAGES CXX) +find_package(dimos_generated CONFIG REQUIRED) +add_executable(header_relay relay.cpp) +target_link_libraries(header_relay PRIVATE dimos_generated::messages) diff --git a/examples/message-package/native/Cargo.toml b/examples/message-package/native/Cargo.toml new file mode 100644 index 00000000000..0a483c82e75 --- /dev/null +++ b/examples/message-package/native/Cargo.toml @@ -0,0 +1,15 @@ +[package] +name = "builtin-message-example" +version = "0.1.0" +edition = "2024" +license = "Apache-2.0" + +[workspace] + +[dependencies] +dimos-generated-messages = "=0.0.14" +cdr = { version = "=0.2.4", git = "https://github.com/TomCC7/cdr-rs", rev = "38a4368255d408faf374726653b39d4a4e9735cb" } + +[[example]] +name = "relay" +path = "relay.rs" diff --git a/examples/message-package/native/relay.cpp b/examples/message-package/native/relay.cpp new file mode 100644 index 00000000000..e2da087b5b9 --- /dev/null +++ b/examples/message-package/native/relay.cpp @@ -0,0 +1,20 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +#include +#include +#include +#include + +using Header = std_msgs::msg::Header; + +int main(int argc, char** argv) { + if (argc != 3) return 2; + std::ifstream input(argv[1], std::ios::binary); + const std::vector bytes{std::istreambuf_iterator(input), {}}; + auto value = dimos::cdr::cdr_decode
(bytes); + value.frame_id += "/cpp"; + const auto encoded = dimos::cdr::cdr_encode(value); + std::ofstream output(argv[2], std::ios::binary); + output.write(reinterpret_cast(encoded.data()), encoded.size()); + return output ? 0 : 1; +} diff --git a/examples/message-package/native/relay.rs b/examples/message-package/native/relay.rs new file mode 100644 index 00000000000..9dd356c70fd --- /dev/null +++ b/examples/message-package/native/relay.rs @@ -0,0 +1,26 @@ +// 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. + +use dimos_generated_messages::std_msgs::msg::header::Header; +use std::{env, fs}; + +fn main() -> Result<(), Box> { + let args: Vec<_> = env::args().collect(); + let bytes = fs::read(&args[1])?; + let mut value: Header = cdr::deserialize(&bytes)?; + value.frame_id.push_str("/rust"); + let output = cdr::serialize::<_, _, cdr::CdrLe>(&value, cdr::Infinite)?; + fs::write(&args[2], output)?; + Ok(()) +} diff --git a/examples/message-viewer/demo_cdr_browser.py b/examples/message-viewer/demo_cdr_browser.py new file mode 100644 index 00000000000..417f30cc060 --- /dev/null +++ b/examples/message-viewer/demo_cdr_browser.py @@ -0,0 +1,183 @@ +# 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. + +"""Publish generated messages through the real cockpit bridge and local relay. + +Keep publishing for the browser page until Ctrl-C. +""" + +from __future__ import annotations + +import argparse +from contextlib import ExitStack +import threading +from typing import Any + +import numpy as np + +from dimos.core.transport import ZenohTransport +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.dimos_msgs.msg import LineSegment3D, LineSegments3D +from dimos.messages.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Vector3 +from dimos.messages.sensor_msgs.msg import Image, Imu, JointState +from dimos.messages.std_msgs.msg import Empty, Header, Int64, String +from dimos.protocol.pubsub.impl.zenohpubsub import Topic +from dimos.protocol.rpc.zenohrpc import ZenohRPC +from dimos.web.cockpit import Channel, cockpit +from dimos.web.relay_bridge.config import RelayBridgeConfig + + +def build_messages() -> list[tuple[str, Any]]: + header = Header(stamp=Time(sec=1757400000, nanosec=500000123), frame_id="map") + return [ + ( + "pose_stamped", + PoseStamped( + header=header, + pose=Pose( + position=Point(x=1.5, y=-2.5, z=0.25), + orientation=Quaternion(z=0.6, w=0.8, x=0.0, y=0.0), + ), + ), + ), + ( + "custom_segments", + LineSegments3D( + header=header, + segments=[ + LineSegment3D( + start=Point(x=1, y=0.0, z=0.0), end=Point(y=2, x=0.0, z=0.0), weight=4 + ) + ], + ), + ), + ( + "image", + Image( + header=header, + width=2, + height=1, + encoding="rgb8", + step=6, + data=np.array([0, 1, 2, 253, 254, 255], dtype=np.uint8), + is_bigendian=0, + ), + ), + ( + "imu", + Imu( + header=header, + orientation_covariance=np.asarray(range(9), dtype=np.float64), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + angular_velocity=Vector3(x=0.0, y=0.0, z=0.0), + angular_velocity_covariance=np.zeros(9, dtype=np.float64), + linear_acceleration=Vector3(x=0.0, y=0.0, z=0.0), + linear_acceleration_covariance=np.zeros(9, dtype=np.float64), + ), + ), + ( + "joints", + JointState( + header=header, + name=["joint_a", "joint_b"], + position=np.array([1.5, -2.5], dtype=np.float64), + velocity=np.array([], dtype=np.float64), + effort=np.array([], dtype=np.float64), + ), + ), + ("int64", Int64(data=-9007199254740993)), + ("empty", Empty()), + ("string", String(data="robot 🦾 café")), + ] + + +class LoopbackRPC(ZenohRPC): + def __init__(self, **kwargs: Any) -> None: + super().__init__( + mode="peer", + listen=["tcp/127.0.0.1:0"], + scouting=False, + multicast=False, + gossip=False, + **kwargs, + ) + + +def run(port: int) -> None: + messages = dict(build_messages()) + channels = [ + Channel(name, type(value), encoding=value.__msgtype__ + ".cdr.v1", max_hz=5) + for name, value in messages.items() + ] + (atom,) = cockpit(channels=channels).blueprints + config = dict( + mode="peer", listen=["tcp/127.0.0.1:0"], scouting=False, multicast=False, gossip=False + ) + stopped = threading.Event() + with ExitStack() as cleanup: + module = atom.module( + **atom.kwargs, + robot_id="cdr-demo", + robot_name="CDR demo", + rpc_transport=LoopbackRPC, + local_port=port, + open_browser=False, + rtc=False, + ) + cleanup.callback(module.stop) + publishers = {} + for name, value in messages.items(): + receiver = ZenohTransport(Topic(name, type(value)), **config) + cleanup.callback(receiver.stop) + module.inputs[name].transport = receiver + publishers[name] = ZenohTransport(Topic(name, type(value)), **config) + cleanup.callback(publishers[name].stop) + module.start() + + def publish() -> None: + while not stopped.is_set(): + for name, value in messages.items(): + publishers[name].broadcast(None, value) + stopped.wait(0.2) + + thread = threading.Thread(target=publish, name="cdr-demo-publisher", daemon=True) + thread.start() + + def stop_publisher() -> None: + stopped.set() + thread.join(timeout=5) + + cleanup.callback(stop_publisher) + print("From web/sdk run: deno task fixture --host 127.0.0.1", flush=True) + print( + f"Open http://127.0.0.1:5174/demo_cdr.html?live=1&relay={module._url}", + flush=True, + ) + try: + stopped.wait() + except KeyboardInterrupt: + pass + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--port", type=int, default=RelayBridgeConfig.model_fields["local_port"].default + ) + args = parser.parse_args() + run(args.port) + + +if __name__ == "__main__": + main() diff --git a/examples/message-viewer/demo_cdr_rerun.py b/examples/message-viewer/demo_cdr_rerun.py new file mode 100644 index 00000000000..db8c576200d --- /dev/null +++ b/examples/message-viewer/demo_cdr_rerun.py @@ -0,0 +1,400 @@ +# 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. + +"""Demonstrate live Rerun bridge rendering and native CDR MCAP viewing without a robot.""" + +from __future__ import annotations + +import argparse +from contextlib import ExitStack +from copy import deepcopy +from functools import partial +from pathlib import Path +import socket +import threading +from typing import Any + +from dimos_cdrgen.registry import encode, schema +import imagecodecs +from mcap.writer import Writer +import numpy as np +import rerun as rr +import rerun.blueprint as rrb + +from dimos.core.transport import ZenohTransport +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.dimos_msgs.msg import EntityMarker, EntityMarkers +from dimos.messages.geometry_msgs.msg import ( + Point, + PointStamped, + Pose, + PoseStamped, + PoseWithCovariance, + Quaternion, + Transform, + TransformStamped, + Twist, + TwistWithCovariance, + Vector3, +) +from dimos.messages.nav_msgs.msg import Odometry, Path as MessagePath +from dimos.messages.sensor_msgs.msg import ( + CameraInfo, + CompressedImage, + Image, + PointCloud2, + PointField, + RegionOfInterest, +) +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped as LegacyPoseStamped +from dimos.protocol.pubsub.impl.zenohpubsub import Topic +from dimos.protocol.pubsub.patterns import Glob +from dimos.protocol.rpc.zenohrpc import ZenohRPC +from dimos.visualization.rerun.bridge import RerunBridgeModule +from dimos.visualization.rerun.cdr import cloud_archetype + + +def scene_messages() -> dict[str, Any]: + message = PoseStamped( + header=Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="map"), + pose=Pose( + position=Point(x=1.5, y=-2.5, z=0.25), orientation=Quaternion(x=0, y=0, z=0.6, w=0.8) + ), + ) + pixels = np.array([[[255, 0, 0], [0, 255, 0]], [[0, 0, 255], [255, 255, 255]]], dtype=np.uint8) + image = Image( + header=message.header, + width=2, + height=2, + encoding="rgb8", + is_bigendian=0, + step=6, + data=pixels.ravel(), + ) + calibration = CameraInfo( + distortion_model="plumb_bob", + d=np.zeros(5, dtype=np.float64), + r=np.eye(3, dtype=np.float64).ravel(), + 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), + header=message.header, + width=2, + height=2, + k=np.array([2, 0, 0.5, 0, 2, 0.5, 0, 0, 1], dtype=np.float64), + ) + points = np.array( + [ + (-0.25, -0.25, 1, 0xFF0000), + (0.25, -0.25, 1, 0x00FF00), + (-0.25, 0.25, 1, 0x0000FF), + (0.25, 0.25, 1, 0xFFFFFF), + ], + dtype=[("x", " rrb.Blueprint: + return rrb.Blueprint( + rrb.Tabs( + rrb.Spatial3DView( + origin="world", + name="Geometry, TF and colored cloud", + contents=[ + "world/pose/**", + "world/point/**", + "world/odometry/**", + "world/path/**", + "world/entities/**", + "world/cloud/**", + "world/cloud_boxes/**", + "world/cloud_height/**", + "world/legacy_pose/**", + "world/tf/**", + "tf#/**", + ], + ), + rrb.Grid( + rrb.Spatial2DView(origin="world/image", name="Generated RGB pixels"), + rrb.Spatial2DView(origin="world/depth", name="Big endian depth"), + rrb.Spatial2DView(origin="world/compressed", name="PNG pixels"), + rrb.Spatial2DView(origin="world/compressed_depth", name="JXL depth"), + rrb.Spatial2DView(origin="world/late_image", name="Calibration arriving last"), + grid_columns=3, + ), + rrb.TextDocumentView(origin="world/pose/message", name="Exact message values"), + ) + ) + + +class DemoBridge(RerunBridgeModule): + """A real bridge with a receipt signal so the demo can flush before exiting.""" + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self.received = threading.Event() + self.errors: list[Exception] = [] + + def _on_cdr_message(self, msg: Any, *, topic: str) -> None: + try: + super()._on_cdr_message(msg, topic=topic) + except Exception as error: + self.errors.append(error) + finally: + self.received.set() + + +class LoopbackRPC(ZenohRPC): + def __init__(self, **kwargs: Any) -> None: + super().__init__( + mode="peer", + listen=["tcp/127.0.0.1:0"], + scouting=False, + multicast=False, + gossip=False, + **kwargs, + ) + + +def run(output: Path) -> None: + # Reserve destinations before starting sockets. Existing recordings are never overwritten. + mcap = output.with_suffix(".mcap") + with ExitStack() as cleanup: + for destination in (output, mcap): + cleanup.enter_context(destination.open("xb")) + config: dict[str, Any] = dict( + mode="peer", listen=["tcp/127.0.0.1:0"], scouting=False, multicast=False, gossip=False + ) + messages = scene_messages() + # Select a free local Rerun port instead of attaching to another user's viewer. + with socket.socket() as reserve: + reserve.bind(("127.0.0.1", 0)) + port = reserve.getsockname()[1] + bridge = DemoBridge( + pubsubs=[], + cdr_transports={ + topic: ZenohTransport(Topic(topic, type(value)), **config) + for topic, value in messages.items() + }, + rpc_transport=LoopbackRPC, + rerun_open="none", + rerun_web=False, + connect_url=f"rerun+http://127.0.0.1:{port}/proxy", + tf_axes=0.25, + blueprint=viewer_layout, + visual_override={ + Glob("world/cloud_boxes"): partial(cloud_archetype, mode="boxes"), + Glob("world/cloud_height"): partial( + cloud_archetype, mode="spheres", height_colors=True + ), + }, + ) + cleanup.callback(bridge.stop) + bridge.start() + rr.save(output) + cleanup.callback(rr.disconnect) + rr.send_blueprint(viewer_layout()) + rr.log("world", rr.Transform3D(child_frame="tf#/world"), static=True) + with mcap.open("wb") as stream: + writer = Writer(stream) + writer.start(profile="ros2") + try: + for index, (topic, value) in enumerate(messages.items()): + with ExitStack() as sender: + transport = ZenohTransport(Topic(topic, type(value)), **config) + sender.callback(transport.stop) + bridge.received.clear() + transport.broadcast(None, value) + if not bridge.received.wait(timeout=10): + raise TimeoutError(f"Bridge did not receive {topic}") + if bridge.errors: + raise bridge.errors[0] + schema_id = writer.register_schema( + value.__msgtype__, "ros2msg", schema(value.__msgtype__).encode() + ) + channel = writer.register_channel(topic, "cdr", schema_id) + timestamp = 1_700_000_000_123_456_789 + index * 1_000_000 + writer.add_message( + channel, log_time=timestamp, publish_time=timestamp, data=encode(value) + ) + print(f"Bridge rendered {topic}: {value.__msgtype__}") + finally: + writer.finish() + # Existing messages still take the bridge's ordinary rendering path. + bridge._on_message(LegacyPoseStamped(position=(0, -1, 0), frame_id="map"), "/legacy_pose") + recording = rr.get_global_data_recording() + if recording is not None: + recording.flush() + print(f"Live bridge: rerun {output}") + print(f"Native MCAP view: rerun {mcap}") + print(f"Reusable recording: {mcap}") + print("Source timestamp: 1700000000.123456789; pose [1.5, -2.5, 0.25].") + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "output", + type=Path, + help="new live .rrd filename; also creates a CDR .mcap for native Rerun viewing", + ) + args = parser.parse_args() + run(args.output) + + +if __name__ == "__main__": + main() diff --git a/examples/native-modules/rust/Cargo.toml b/examples/native-modules/rust/Cargo.toml index b7d0cf69700..b6fb7fbc647 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 = "6663bcb6e5e18f43a6cb9cf77ccbec613e310443", version = "=0.1.0" } lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } tokio = { version = "1", features = ["rt-multi-thread", "macros", "time"] } serde = { version = "1", features = ["derive"] } diff --git a/native/message-source.nix b/native/message-source.nix new file mode 100644 index 00000000000..d54683ac53e --- /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/29/9f/a4fe6f8ea4dcafd3001c868ebbc751bb0af83aecb1fe6523765c12cdedba/dimos_cdrgen-0.1.2-py3-none-any.whl"; + sha256 = "9857c8cc40de58b03f94cb81e7d27ae3caac98101c96cdaa2965413a332e685f"; + }; + generator = pkgs.runCommand "dimos-cdrgen-0.1.2-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/crate-hashes.json b/native/rust/crate-hashes.json new file mode 100644 index 00000000000..3ee4f926e23 --- /dev/null +++ b/native/rust/crate-hashes.json @@ -0,0 +1,6 @@ +{ + "git+https://github.com/TomCC7/cdr-rs?rev=38a4368255d408faf374726653b39d4a4e9735cb#cdr@0.2.4": "sha256-akg/bLBY3wQ1YRr2bijE5ZqB211Cst5lKjELLrgiG6o=", + "git+https://github.com/TomCC7/transforms?rev=5ae9563de0c02638c04e393aadfad28444a4398e#transforms@2.2.0": "sha256-Kh8aVjj7zQ+Xd1QHOcAhSJbubg7azaWFSMpaI/ZAgM4=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#dimos-lcm@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=", + "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#lcm-msgs@0.1.0": "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8=" +} \ No newline at end of file diff --git a/native/rust/dimos-module-macros/src/lib.rs b/native/rust/dimos-module-macros/src/lib.rs index af89a0680b4..ff19a4af1ad 100644 --- a/native/rust/dimos-module-macros/src/lib.rs +++ b/native/rust/dimos-module-macros/src/lib.rs @@ -454,20 +454,33 @@ fn port_decls(classified: &[ClassifiedField], want_input: bool) -> Vec FieldKind::Tf => want_input, _ => false, }) - .map(|f| PortDecl { - name: f.name.to_string(), - // `Tf` carries no generic payload. The port is TFMessage by - // construction. - // 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(), - _ => f - .msg - .clone() - .or_else(|| port_payload_type(f.ty)) - .unwrap_or_default(), - }, + .flat_map(|f| { + let port = PortDecl { + name: f.name.to_string(), + // `Tf` carries no generic payload. The port is TFMessage by + // construction. + // 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(), + _ => f + .msg + .clone() + .or_else(|| port_payload_type(f.ty)) + .unwrap_or_default(), + }, + }; + if matches!(f.kind, FieldKind::Tf) { + vec![ + port, + PortDecl { + name: "tf_static".into(), + ty: TF_PAYLOAD_TYPE.into(), + }, + ] + } else { + vec![port] + } }) .collect() } @@ -537,7 +550,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,6 +662,12 @@ fn classify_field(field: &Field, name: &Ident) -> syn::Result { if found.is_some() { return Err(syn::Error::new_spanned(attr, ONE_ATTR_ONLY)); } + if !matches!(&attr.meta, syn::Meta::Path(_)) { + return Err(syn::Error::new_spanned( + attr, + "#[tf] uses standard ROS TF messages and takes no codec arguments", + )); + } found = Some(FieldKind::Tf); } } @@ -736,6 +755,36 @@ 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)] { + 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(), 2); + assert_eq!(ports[0].ty, "TFMessage"); + assert_eq!(ports[1].name, "tf_static"); + } + } + + #[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 +839,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..72198377574 100644 --- a/native/rust/dimos-module/Cargo.lock +++ b/native/rust/dimos-module/Cargo.lock @@ -249,6 +249,15 @@ dependencies = [ "shlex", ] +[[package]] +name = "cdr" +version = "0.2.4" +source = "git+https://github.com/TomCC7/cdr-rs?rev=38a4368255d408faf374726653b39d4a4e9735cb#38a4368255d408faf374726653b39d4a4e9735cb" +dependencies = [ + "byteorder", + "serde", +] + [[package]] name = "cfg-if" version = "1.0.4" @@ -579,10 +588,21 @@ dependencies = [ "subtle", ] +[[package]] +name = "dimos-generated-messages" +version = "0.0.14" +dependencies = [ + "cdr", + "heapless", + "ros2msg", + "serde", + "serde-big-array", +] + [[package]] name = "dimos-lcm" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?rev=04d78e8622500244123ba9cefa4c51b4cb454549#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", "socket2 0.5.10", @@ -591,8 +611,10 @@ dependencies = [ [[package]] name = "dimos-module" -version = "0.1.0" +version = "0.2.0" dependencies = [ + "cdr", + "dimos-generated-messages", "dimos-lcm", "dimos-module-macros", "lcm-msgs", @@ -604,6 +626,7 @@ dependencies = [ "tracing", "tracing-subscriber", "tracing-test", + "transforms", "url", "validator", "zenoh", @@ -950,6 +973,15 @@ version = "0.33.7" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e762b9b188634fc508d4ec54b62ea3d352c43fd431d18d05d46f559f217f4725" +[[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" @@ -991,6 +1023,28 @@ name = "hashbrown" version = "0.17.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ed5909b6e89a2db4456e54cd5f673791d7eca6732202bbf2a9cc504fe2f9b84a" +dependencies = [ + "allocator-api2", + "equivalent", + "foldhash 0.2.0", +] + +[[package]] +name = "heapless" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +dependencies = [ + "hash32", + "serde", + "stable_deref_trait", +] + +[[package]] +name = "heck" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" [[package]] name = "hermit-abi" @@ -1385,7 +1439,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", ] @@ -1926,6 +1980,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" @@ -2280,6 +2344,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", +] + [[package]] name = "rustc-hash" version = "2.1.3" @@ -2520,6 +2601,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" @@ -3216,6 +3306,18 @@ dependencies = [ "syn 2.0.119", ] +[[package]] +name = "transforms" +version = "2.2.0" +source = "git+https://github.com/TomCC7/transforms?rev=5ae9563de0c02638c04e393aadfad28444a4398e#5ae9563de0c02638c04e393aadfad28444a4398e" +dependencies = [ + "approx", + "hashbrown 0.17.1", + "libm", + "serde", + "thiserror", +] + [[package]] name = "twox-hash" version = "1.6.3" diff --git a/native/rust/dimos-module/Cargo.toml b/native/rust/dimos-module/Cargo.toml index b79f793a92c..a7922d0b1bd 100644 --- a/native/rust/dimos-module/Cargo.toml +++ b/native/rust/dimos-module/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "dimos-module" -version = "0.1.0" +version = "0.2.0" edition = "2021" description = "Rust native module SDK for dimos NativeModule framework" license = "Apache-2.0" @@ -8,9 +8,12 @@ license = "Apache-2.0" [workspace] [dependencies] -dimos-lcm = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } +dimos-generated-messages = { path = "../../../build/message-source/rust" } +transforms = { version = "=2.2.0", git = "https://github.com/TomCC7/transforms", rev = "5ae9563de0c02638c04e393aadfad28444a4398e" } +cdr = { version = "=0.2.4", git = "https://github.com/TomCC7/cdr-rs", rev = "38a4368255d408faf374726653b39d4a4e9735cb" } +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/README.md b/native/rust/dimos-module/README.md new file mode 100644 index 00000000000..b782f4a865b --- /dev/null +++ b/native/rust/dimos-module/README.md @@ -0,0 +1,34 @@ +# Native module SDK + +`#[tf]` binds standard `tf2_msgs/TFMessage` streams named `tf` and `tf_static`. +Both ports must appear in the native module registry and Python wrapper. +The SDK uses generated `TransformStamped` and `Time` values directly. TF +fields accept no codec parameters; CDR is the default wire format. + +`lookup_transform(target, source, time)` maps source coordinates into the +target frame. ROS zero time selects the latest common time. `lookup().at()` +also takes a ROS `Time`; `within()` waits for missing data. Lookups interpolate +and reject extrapolation. The `transforms` library owns the frame tree, +history, composition and interpolation. Its current registry pins each child's +parent and static/dynamic kind; changing either requires rebuilding the buffer. + +`publish()` sends dynamic transforms and `publish_static()` sends static +transforms. Static edges never expire from a receiving buffer and do not +constrain the common time of a dynamic chain. Invalid batches do not change +the graph or enqueue a packet. A failed publication queue leaves the graph +unchanged. + +The SDK's LCM and Zenoh transports do not provide ROS DDS transient-local +history. A static publisher must remain available and reannounce its complete +mount set for late subscribers. Existing Python static-mount publishers still +reannounce on dynamic `tf` for compatibility with the Python query layer; they +have not been migrated to `tf_static` in this SDK change. + +Older modules pinned to SDK 0.1.0 retain their explicit legacy wire path. SDK +0.2.0 does not fall back to legacy TF decoding. + +Message definitions come from the existing built-in message pipeline in +`packages/dimos-generated/schema-sources.json`. Before building the SDK from +source, install the project to generate `build/message-source/rust`. The SDK +and native consumers depend on that same `dimos-generated-messages` crate; +TF messages have no separate definitions or generation command. diff --git a/native/rust/dimos-module/src/cdr.rs b/native/rust/dimos-module/src/cdr.rs new file mode 100644 index 00000000000..833ac0976bd --- /dev/null +++ b/native/rust/dimos-module/src/cdr.rs @@ -0,0 +1,123 @@ +// 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. + +//! SDK error and message-boundary adapter for the upstream complete CDR codec. +use ::cdr::{CdrBe, CdrLe, Infinite}; +use serde::{de::DeserializeOwned, Serialize}; +use std::io::{self, Cursor}; + +pub fn encode(message: &T) -> io::Result> { + encode_endian(message, true) +} + +pub fn encode_endian(message: &T, little_endian: bool) -> io::Result> { + if little_endian { + ::cdr::serialize::<_, _, CdrLe>(message, Infinite) + } else { + ::cdr::serialize::<_, _, CdrBe>(message, Infinite) + } + .map_err(|error| io::Error::new(io::ErrorKind::InvalidInput, error)) +} + +pub fn decode(bytes: &[u8]) -> io::Result { + // The SDK accepts standalone plain XCDR1, not parameter lists or other DDS representations. + if !matches!(bytes.get(..4), Some([0, 0 | 1, 0, 0])) { + return Err(io::Error::new( + io::ErrorKind::InvalidData, + "Expected plain CDR/XCDR1 encapsulation", + )); + } + let mut reader = Cursor::new(bytes); + let value = ::cdr::deserialize_from(&mut reader, ::cdr::Bounded(bytes.len() as u64)) + .map_err(|error| io::Error::new(io::ErrorKind::InvalidData, error))?; + if reader.position() != bytes.len() as u64 { + 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 serialization_failure_is_an_error() { + struct Unencodable; + impl Serialize for Unencodable { + fn serialize(&self, _: S) -> Result { + Err(serde::ser::Error::custom("invalid message")) + } + } + assert_eq!( + encode(&Unencodable).unwrap_err().kind(), + io::ErrorKind::InvalidInput + ); + } + + #[test] + fn oversized_or_unterminated_strings_fail_at_the_library_boundary() { + for bytes in [ + vec![0, 1, 0, 0, 255, 255, 255, 255], + vec![0, 1, 0, 0, 0, 0, 0, 0], + vec![0, 1, 0, 0, 2, 0, 0, 0, b'a', b'b'], + ] { + assert_eq!( + decode::(&bytes).unwrap_err().kind(), + io::ErrorKind::InvalidData + ); + } + assert_eq!( + decode::(&encode(&"相机/café").unwrap()).unwrap(), + "相机/café" + ); + } + + #[test] + fn reject_wrong_encapsulation_truncation_and_trailing_bytes() { + for bytes in [ + vec![], + vec![1, 1, 0, 0, 42, 0, 0, 0], + vec![0, 1, 1, 0, 42, 0, 0, 0], + 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/host.rs b/native/rust/dimos-module/src/host.rs index cc7982b7e55..fa01c48f320 100644 --- a/native/rust/dimos-module/src/host.rs +++ b/native/rust/dimos-module/src/host.rs @@ -891,7 +891,7 @@ mod tests { fn build(builder: &mut Builder, config: ProbeConfig) -> Self { Self { key: config.key, - ping: builder.output("ping", |b: &Vec| b.clone()), + ping: builder.output("ping", |b: &Vec| Ok(b.clone())), } } diff --git a/native/rust/dimos-module/src/lib.rs b/native/rust/dimos-module/src/lib.rs index 5816445c3f4..c2345519993 100644 --- a/native/rust/dimos-module/src/lib.rs +++ b/native/rust/dimos-module/src/lib.rs @@ -17,6 +17,7 @@ #[cfg(test)] extern crate self as dimos_module; +pub mod cdr; pub mod host; pub mod lcm; pub mod log; @@ -32,7 +33,7 @@ pub use dimos_module_macros::{native_config, Module}; pub use host::{host_main, HostSpec, ModuleEntry}; pub use lcm::LcmTransport; pub use module::{run, Builder, Input, Io, Module, ModuleConfig, NativeConfig, NoConfig, Output}; -pub use tf::{Lookup, Tf, Transform}; +pub use tf::{Lookup, Tf, TransformStamped}; pub use transport::{SharedTransport, Transport}; pub use workers::worker_pool; pub use zenoh::ZenohTransport; diff --git a/native/rust/dimos-module/src/module.rs b/native/rust/dimos-module/src/module.rs index 23fe965ba36..75b5aa44a94 100644 --- a/native/rust/dimos-module/src/module.rs +++ b/native/rust/dimos-module/src/module.rs @@ -123,13 +123,13 @@ impl Input { #[derive(Clone)] pub struct Output { pub topic: String, - encode: fn(&T) -> Vec, + encode: fn(&T) -> io::Result>, sender: mpsc::Sender>, } impl Output { pub async fn publish(&self, msg: &T) -> io::Result<()> { - publish_encoded(&self.sender, (self.encode)(msg)).await + publish_encoded(&self.sender, (self.encode)(msg)?).await } } @@ -140,7 +140,7 @@ impl Output { pub struct Io { pub topic: String, receiver: mpsc::Receiver, - encode: fn(&T) -> Vec, + encode: fn(&T) -> io::Result>, sender: mpsc::Sender>, } @@ -150,7 +150,7 @@ impl Io { } pub async fn publish(&self, msg: &T) -> io::Result<()> { - publish_encoded(&self.sender, (self.encode)(msg)).await + publish_encoded(&self.sender, (self.encode)(msg)?).await } } @@ -386,7 +386,7 @@ impl Builder { Input { topic, receiver } } - pub fn output(&mut self, port: &str, encode: fn(&T) -> Vec) -> Output { + pub fn output(&mut self, port: &str, encode: fn(&T) -> io::Result>) -> Output { let topic = self.topic_for(port); let sender = self.add_publisher(&topic); Output { @@ -401,7 +401,7 @@ impl Builder { &mut self, port: &str, decode: fn(&[u8]) -> io::Result, - encode: fn(&T) -> Vec, + encode: fn(&T) -> io::Result>, ) -> Io { let topic = self.topic_for(port); let receiver = self.add_route(&topic, decode); @@ -414,19 +414,22 @@ impl Builder { } } - /// A handle that answers transform queries and publishes on the `tf` topic. - /// - /// The graph fills in the background as `tf` messages arrive. Repeated calls - /// share one graph. + /// Standard ROS TF streams. Dynamic and static messages share one frame tree. pub fn tf(&mut self) -> crate::tf::Tf { if let Some(tf) = &self.tf { return tf.clone(); } let topic = self.topic_for("tf"); + let static_topic = self.topic_for("tf_static"); let sender = self.add_publisher(&topic); - let (tf, route) = - crate::tf::tf_subscription(topic.clone(), crate::tf::DEFAULT_TF_WINDOW_SECS, sender); + let static_sender = self.add_publisher(&static_topic); + let (tf, route, static_route) = + crate::tf::tf_subscription(sender, static_sender, crate::tf::DEFAULT_TF_WINDOW); self.routes.entry(topic).or_default().push(route); + self.routes + .entry(static_topic) + .or_default() + .push(static_route); self.tf = Some(tf.clone()); tf } @@ -942,7 +945,7 @@ mod tests { #[test] fn output_uses_mapped_topic() { let mut builder = builder_with_topics(&[("cmd_vel", "/robot/cmd_vel")]); - let output = builder.output("cmd_vel", |b: &Vec| b.clone()); + let output = builder.output("cmd_vel", |b: &Vec| Ok(b.clone())); assert_eq!(output.topic, "/robot/cmd_vel"); } @@ -950,7 +953,7 @@ mod tests { fn topics_matching_ports_exactly_pass() { let mut builder = builder_with_topics(&[("cmd", "/robot/cmd"), ("odom", "/robot/odom")]); builder.input("cmd", |b| Ok(b.to_vec())); - builder.output("odom", |b: &Vec| b.clone()); + builder.output("odom", |b: &Vec| Ok(b.clone())); builder.enforce_topics_match_ports().expect("exact match"); } @@ -958,7 +961,7 @@ mod tests { fn a_port_the_coordinator_never_sent_is_rejected() { let mut builder = builder_with_topics(&[("cmd", "/robot/cmd")]); builder.input("cmd", |b| Ok(b.to_vec())); - builder.output("odom", |b: &Vec| b.clone()); + builder.output("odom", |b: &Vec| Ok(b.clone())); let err = builder .enforce_topics_match_ports() .expect_err("odom has no topic"); @@ -977,7 +980,10 @@ mod tests { #[test] fn a_tf_field_claims_the_tf_topic() { - let mut builder = builder_with_topics(&[("tf", "/tf#tf2_msgs.TFMessage")]); + let mut builder = builder_with_topics(&[ + ("tf", "/tf#tf2_msgs.TFMessage"), + ("tf_static", "/tf_static#tf2_msgs.TFMessage"), + ]); builder.tf(); builder.enforce_topics_match_ports().expect("tf claimed"); } @@ -993,14 +999,14 @@ mod tests { #[test] fn io_uses_mapped_topic() { let mut builder = builder_with_topics(&[("cmd", "/robot/cmd")]); - let io = builder.io("cmd", |b| Ok(b.to_vec()), |b: &Vec| b.clone()); + let io = builder.io("cmd", |b| Ok(b.to_vec()), |b: &Vec| Ok(b.clone())); assert_eq!(io.topic, "/robot/cmd"); } #[test] fn io_registers_one_route_and_one_publisher_on_the_same_topic() { let mut builder = builder_with_topics(&[("cmd", "/robot/cmd")]); - let _io = builder.io("cmd", |b| Ok(b.to_vec()), |b: &Vec| b.clone()); + let _io = builder.io("cmd", |b| Ok(b.to_vec()), |b: &Vec| Ok(b.clone())); assert_eq!(builder.routes.get("/robot/cmd").map(Vec::len), Some(1)); assert_eq!(builder.outputs.len(), 1); assert_eq!(builder.outputs[0].0, "/robot/cmd"); @@ -1009,7 +1015,7 @@ mod tests { #[tokio::test] async fn io_receives_on_its_route_and_publishes_to_its_queue() { let mut builder = builder_with_topics(&[("cmd", "/robot/cmd")]); - let mut io = builder.io("cmd", |b| Ok(b.to_vec()), |b: &Vec| b.clone()); + let mut io = builder.io("cmd", |b| Ok(b.to_vec()), |b: &Vec| Ok(b.clone())); builder.routes["/robot/cmd"][0].try_dispatch(b"inbound"); assert_eq!(io.recv().await.expect("inbound message"), b"inbound"); @@ -1022,7 +1028,7 @@ mod tests { #[tokio::test] async fn io_publish_errors_when_the_publish_worker_is_gone() { let mut builder = builder_with_topics(&[]); - let io = builder.io("cmd", |b| Ok(b.to_vec()), |b: &Vec| b.clone()); + let io = builder.io("cmd", |b| Ok(b.to_vec()), |b: &Vec| Ok(b.clone())); builder.outputs.clear(); let err = io .publish(&b"x".to_vec()) @@ -1044,10 +1050,44 @@ mod tests { let mut builder = builder_with_topics(&[("tf", "/tf")]); builder.tf(); builder.tf(); - assert_eq!(builder.outputs.len(), 1); + assert_eq!(builder.outputs.len(), 2); assert_eq!(builder.routes.get("/tf").map(Vec::len), Some(1)); } + #[tokio::test] + async fn port_encode_error_does_not_enqueue_or_close_the_port() { + fn encode(value: &Vec) -> io::Result> { + if value == b"bad" { + return Err(io::Error::new( + io::ErrorKind::InvalidInput, + "encoding failed", + )); + } + Ok(value.clone()) + } + let mut builder = builder_with_topics(&[("output", "/out"), ("io", "/io")]); + let output = builder.output("output", encode); + let io = builder.io("io", |bytes| Ok(bytes.to_vec()), encode); + assert_eq!( + output.publish(&b"bad".to_vec()).await.unwrap_err().kind(), + io::ErrorKind::InvalidInput + ); + assert_eq!( + io.publish(&b"bad".to_vec()).await.unwrap_err().kind(), + io::ErrorKind::InvalidInput + ); + for (_, receiver) in &mut builder.outputs { + assert!(matches!( + receiver.try_recv(), + Err(tokio::sync::mpsc::error::TryRecvError::Empty) + )); + } + output.publish(&b"out".to_vec()).await.unwrap(); + io.publish(&b"io".to_vec()).await.unwrap(); + assert_eq!(builder.outputs[0].1.recv().await.unwrap(), b"out"); + assert_eq!(builder.outputs[1].1.recv().await.unwrap(), b"io"); + } + // recv/publish concurrency #[tokio::test(flavor = "multi_thread", worker_threads = 2)] @@ -1065,7 +1105,7 @@ mod tests { let mut builder = Builder::new(topics(&[("data", "/data"), ("out", "/out")])); let _input = builder.input("data", |b| Ok(b.to_vec())); - let output = builder.output("out", |b: &Vec| b.clone()); + let output = builder.output("out", |b: &Vec| Ok(b.clone())); subscribe_routes(&transport, builder.routes).await.unwrap(); let transport = Arc::new(transport); @@ -1115,7 +1155,7 @@ mod tests { } Ok(b.to_vec()) }); - let output = builder.output("out", |b: &Vec| b.clone()); + let output = builder.output("out", |b: &Vec| Ok(b.clone())); subscribe_routes(&transport, builder.routes).await.unwrap(); let transport = Arc::new(transport); @@ -1166,8 +1206,8 @@ mod tests { }); let mut builder = Builder::new(topics(&[("block_out", "/block"), ("fast_out", "/fast")])); - let block_out = builder.output("block_out", |b: &Vec| b.clone()); - let fast_out = builder.output("fast_out", |b: &Vec| b.clone()); + let block_out = builder.output("block_out", |b: &Vec| Ok(b.clone())); + let fast_out = builder.output("fast_out", |b: &Vec| Ok(b.clone())); let _pub_tasks = spawn_publish_tasks(Arc::clone(&transport), builder.outputs); // Wedge the block channel, then publish on the fast channel. @@ -1278,8 +1318,8 @@ mod tests { Ok(Msg(bytes.to_vec())) } - fn encode(msg: &Msg) -> Vec { - msg.0.clone() + fn encode(msg: &Msg) -> io::Result> { + Ok(msg.0.clone()) } #[derive(crate::Module)] 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..28bb70b5e38 100644 --- a/native/rust/dimos-module/src/tf.rs +++ b/native/rust/dimos-module/src/tf.rs @@ -12,1194 +12,641 @@ // See the License for the specific language governing permissions and // limitations under the License. -//! Transform client for native modules. -//! -//! Each `/tf` edge is buffered per `(parent, child)`, and [`Tf::lookup`] composes -//! the shortest path through the frame graph. Lookups are nearest-in-time within -//! a tolerance, not interpolated. [`Tf::publish`] sends transforms onto the same -//! topic and feeds the local graph. - -use std::collections::{HashMap, HashSet, VecDeque}; -use std::io; -use std::sync::atomic::AtomicU64; -use std::sync::{Arc, Mutex, RwLock}; -use std::time::Duration; - -use nalgebra::{Isometry3, Quaternion, Translation3, UnitQuaternion, Vector3}; -use tokio::sync::{mpsc, Notify}; -use tracing::warn; +//! ROS TF messages backed by the `transforms` frame tree. use crate::module::Route; -use crate::time::now_secs; - -/// How many seconds of history each edge keeps. -pub(crate) const DEFAULT_TF_WINDOW_SECS: f64 = 10.0; - -const WARN_INTERVAL: Duration = Duration::from_secs(1); - -/// A rigid transform from `parent` to `child` at a point in time. -/// -/// It maps a point expressed in `child` coordinates into `parent` coordinates. -#[derive(Clone, Debug)] -pub struct Transform { - pub parent: String, - pub child: String, - pub ts: f64, - iso: Isometry3, +use nalgebra::{Isometry3, Quaternion, Translation3, UnitQuaternion}; +use std::{ + io, + sync::{Arc, RwLock}, + time::{Duration, SystemTime, UNIX_EPOCH}, +}; +use tokio::sync::{mpsc, Notify}; +use transforms::{ + geometry::{ + Quaternion as FrameQuaternion, Transform as FrameTransform, Vector3 as FrameVector, + }, + time::Stamp, + Registry, +}; + +pub use dimos_generated_messages::{ + builtin_interfaces::msg::time::Time, geometry_msgs::msg::transform_stamped::TransformStamped, + tf2_msgs::msg::tf_message::TFMessage, +}; +use dimos_generated_messages::{ + geometry_msgs::msg::{ + quaternion::Quaternion as WireQuaternion, transform::Transform, vector3::Vector3, + }, + std_msgs::msg::header::Header, +}; + +pub(crate) const DEFAULT_TF_WINDOW: Duration = Duration::from_secs(10); + +fn invalid(error: impl std::fmt::Display) -> io::Error { + io::Error::new(io::ErrorKind::InvalidData, error.to_string()) } -impl Transform { - pub fn new( - parent: impl Into, - child: impl Into, - ts: f64, - iso: Isometry3, - ) -> Self { - Self { - parent: parent.into(), - child: child.into(), - ts, - iso, - } - } - - pub fn translation(&self) -> Vector3 { - self.iso.translation.vector - } - - pub fn rotation(&self) -> UnitQuaternion { - self.iso.rotation - } - - fn inverse(&self) -> Transform { - Transform { - parent: self.child.clone(), - child: self.parent.clone(), - ts: self.ts, - iso: self.iso.inverse(), - } - } - - fn compose(&self, other: &Transform) -> Transform { - Transform { - parent: self.parent.clone(), - child: other.child.clone(), - ts: self.ts, - iso: self.iso * other.iso, - } +pub fn nanoseconds(time: &Time) -> io::Result { + if time.nanosec >= 1_000_000_000 { + return Err(invalid("unnormalized ROS time")); } + Ok(i64::from(time.sec) * 1_000_000_000 + i64::from(time.nanosec)) } -struct Sample { - ts: f64, - iso: Isometry3, +pub fn time_from_nanoseconds(ns: i64) -> io::Result