diff --git a/.github/workflows/cdr-native.yml b/.github/workflows/cdr-native.yml new file mode 100644 index 00000000000..4b36e8845e8 --- /dev/null +++ b/.github/workflows/cdr-native.yml @@ -0,0 +1,37 @@ +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 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/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/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/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/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/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