diff --git a/dimos/core/transport.py b/dimos/core/transport.py index 7f844669d87..e9d6d605910 100644 --- a/dimos/core/transport.py +++ b/dimos/core/transport.py @@ -25,6 +25,7 @@ cast, ) +from dimos_cdrgen.registry import message_types import numpy as np from dimos.core.stream import Stream, Transport @@ -199,11 +200,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" - ) + # Generated values require a canonical registry identity and a ROS type adapter. + if hasattr(msg_type, "__msgtype__"): + name = getattr(msg_type, "__msgtype__", None) + if not isinstance(name, str) or message_types().get(name) is not msg_type: + raise TypeError("ROS transport requires a registry-owned generated message class") # rclpy (about 0.1 s with rospubsub): only loaded when a ROSTransport exists. from dimos.protocol.pubsub.impl.rospubsub import ROSTopic @@ -211,9 +212,11 @@ def __init__(self, topic: str, msg_type: type[DimosMsg], **kwargs: Any) -> None: self._kwargs = kwargs def __reduce__(self) -> tuple[Any, ...]: - return (ROSTransport, (self.topic.topic, self.topic.msg_type)) + return (partial(ROSTransport, **self._kwargs), (self.topic.topic, self.topic.msg_type)) def broadcast(self, _: Stream[DimosMsg] | None, msg: DimosMsg) -> None: + if not isinstance(msg, self.topic.msg_type): + raise TypeError("Message does not match the ROS transport declared type") if self._ros is None: self.start() assert self._ros is not None # for type narrowing diff --git a/dimos/hardware/drive_trains/transport/adapter.py b/dimos/hardware/drive_trains/transport/adapter.py index 921cd90d378..d6db93bffb6 100644 --- a/dimos/hardware/drive_trains/transport/adapter.py +++ b/dimos/hardware/drive_trains/transport/adapter.py @@ -24,9 +24,8 @@ from typing import Any from dimos.core.transport_factory import make_transport -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.message_utils.geometry import quaternion_euler +from dimos.messages.geometry_msgs.msg import PoseStamped, Twist, Vector3 from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -141,7 +140,11 @@ def read_enabled(self) -> bool: def _on_odom(self, msg: PoseStamped) -> None: with self._lock: - self._latest_odom = [msg.x, msg.y, msg.yaw] + self._latest_odom = [ + msg.pose.position.x, + msg.pose.position.y, + quaternion_euler(msg.pose.orientation)[2], + ] def transport_lcm_factory(**kwargs: Any) -> TransportTwistAdapter: diff --git a/dimos/hardware/whole_body/transport/adapter.py b/dimos/hardware/whole_body/transport/adapter.py index 2ad2b223b84..5679e73646b 100644 --- a/dimos/hardware/whole_body/transport/adapter.py +++ b/dimos/hardware/whole_body/transport/adapter.py @@ -15,7 +15,11 @@ """Transport-based whole-body adapter: bridges coordinator ↔ Module via pub/sub. Subscribes /{hardware_id}/motor_states + /{hardware_id}/imu, publishes -/{hardware_id}/motor_command. ``network_interface`` is accepted but ignored. +/{hardware_id}/motor_command. The normal factory selects generated CDR on the +configured LCM/Zenoh backend. ``network_interface`` is accepted but ignored. +The existing ``transport_lcm`` registry key keeps its name and API; encoding +follows the canonical message type, while internal MotorState/MotorCommand +numerical values are adapted explicitly at these three wire edges. """ from __future__ import annotations @@ -24,12 +28,14 @@ import threading from typing import Any +import numpy as np + from dimos.core.transport_factory import make_transport from dimos.hardware.spec import JointLimits from dimos.hardware.whole_body.spec import IMUState, MotorCommand, MotorState -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray +from dimos.message_utils.time import header_now +from dimos.messages.dimos_msgs.msg import MotorCommandArray +from dimos.messages.sensor_msgs.msg import Imu, JointState from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -135,11 +141,12 @@ def write_motor_commands(self, commands: list[MotorCommand]) -> bool: return False msg = MotorCommandArray( - q=[c.q for c in commands], - dq=[c.dq for c in commands], - kp=[c.kp for c in commands], - kd=[c.kd for c in commands], - tau=[c.tau for c in commands], + header=header_now(), + q=np.asarray([c.q for c in commands], dtype=np.float64), + dq=np.asarray([c.dq for c in commands], dtype=np.float64), + kp=np.asarray([c.kp for c in commands], dtype=np.float64), + kd=np.asarray([c.kd for c in commands], dtype=np.float64), + tau=np.asarray([c.tau for c in commands], dtype=np.float64), ) self._motor_command_transport.publish(msg) return True diff --git a/dimos/hardware/whole_body/transport/test_adapter.py b/dimos/hardware/whole_body/transport/test_adapter.py new file mode 100644 index 00000000000..0c62c4fcc84 --- /dev/null +++ b/dimos/hardware/whole_body/transport/test_adapter.py @@ -0,0 +1,301 @@ +# 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. + +"""Canonical whole-body wire bridge, with fake external DDS/ROS endpoints.""" + +from contextlib import ExitStack +from copy import deepcopy +from functools import partial +from importlib import import_module +import sys +from threading import Event, Thread +from types import ModuleType, SimpleNamespace + +import numpy as np +import pytest + +from dimos.core import transport as transport_module +from dimos.core.global_config import GlobalConfig +from dimos.core.transport import LCMTransport, ZenohTransport +from dimos.core.transport_factory import make_transport +from dimos.hardware.whole_body.spec import MotorCommand +from dimos.hardware.whole_body.transport.adapter import TransportWholeBodyAdapter +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.dimos_msgs.msg import MotorCommandArray +from dimos.messages.sensor_msgs.msg import Imu, JointState +from dimos.messages.std_msgs.msg import Header +from dimos.protocol.pubsub.impl.lcmpubsub import LCM +from dimos.protocol.pubsub.impl.zenohpubsub import Zenoh +from dimos.robot.galaxea.r1pro.connection import R1ProConnection, _stamp_nanoseconds +from dimos.robot.unitree.g1.wholebody_connection import G1LowStateSnapshot, G1WholeBodyConnection +from dimos.utils.testing.waiting import wait_until + + +@pytest.fixture(params=["lcm", "zenoh"]) +def factory(request, monkeypatch): + monkeypatch.setattr(transport_module, "LCM", partial(LCM, url="memq://")) + monkeypatch.setattr( + transport_module, + "Zenoh", + partial( + Zenoh, + scouting=False, + multicast=False, + gossip=False, + connect=[], + listen=["tcp/127.0.0.1:0"], + ), + ) + build = partial(make_transport, g=GlobalConfig(transport=request.param)) + transports = {} + + def shared(topic, kind): + # ModuleCoordinator shares one transport per logical (name, type) within a worker. + # memq is endpoint-local, so distinct memq instances cannot act as a network. + key = (topic, kind) + if key not in transports: + transports[key] = build(topic, kind) + return transports[key] + + try: + yield shared + finally: + for wire in transports.values(): + wire.stop() + + +def _command(count): + return MotorCommandArray( + Header(Time(17, 1), "coordinator"), *[np.arange(count, dtype=np.float64) for _ in range(5)] + ) + + +def test_adapter_default_factory_reuses_g1_canonical_motor_state_imu_and_command(factory, mocker): + with ExitStack() as cleanup: + connection = G1WholeBodyConnection(soft_start_seconds=0.0) + cleanup.callback(connection.stop) + publisher = mocker.Mock() + crc = mocker.Mock() + crc.Crc.return_value = 17 + low_cmd = SimpleNamespace( + mode_machine=5, + crc=0, + motor_cmd=[SimpleNamespace(mode=1, q=0, dq=0, kp=0, kd=0, tau=0) for _ in range(35)], + ) + mocker.patch.multiple( + connection, _publisher=publisher, _low_cmd=low_cmd, _crc=crc, _mode_machine=5 + ) + written = Event() + values = [] + + def write(value): + values.append(deepcopy(value)) + written.set() + + publisher.Write.side_effect = write + adapter = TransportWholeBodyAdapter(dof=29, hardware_id="g1", transport_cls=factory) + cleanup.callback(adapter.disconnect) + assert adapter.connect() + wires = {} + for name, kind in [ + ("motor_states", JointState), + ("imu", Imu), + ("motor_command", MotorCommandArray), + ]: + wire = factory("/g1/" + name, kind) + cleanup.callback(wire.stop) + wires[name] = wire + connection.set_transport(name, wire) + cleanup.callback(wires["motor_command"].subscribe(connection._on_motor_command)) + sample = G1LowStateSnapshot( + [0.25] * 29, [0.5] * 29, [0.75] * 29, (1, 0, 0, 0), (1, 2, 3), (4, 5, 6) + ) + observed = [] + cleanup.callback(wires["motor_states"].subscribe(observed.append)) + connection._publish_motor_state_and_imu( + now=17.0, frame_id="g1_pelvis", sample=sample, stamp=Time(1_789_654_321, 1) + ) + wait_until( + lambda: ( + adapter.has_motor_states() + and adapter.read_imu().gyroscope == (1, 2, 3) + and bool(observed) + ), + timeout=5, + ) + assert adapter.read_motor_states()[0].q == 0.25 + assert adapter.read_motor_states()[0].dq == 0.5 + assert adapter.read_motor_states()[0].tau == 0.75 + assert adapter.read_imu().quaternion == (1, 0, 0, 0) + assert observed[0].header.stamp == Time(1_789_654_321, 1) + commands = [MotorCommand(q=1, dq=2, kp=3, kd=4, tau=5) for _ in range(29)] + assert adapter.write_motor_commands(commands) + assert written.wait(5) + assert values[0].mode_machine == 5 and values[0].crc == 17 + assert [(v.q, v.dq, v.kp, v.kd, v.tau) for v in values[0].motor_cmd[:29]] == [ + (1, 2, 3, 4, 5) + ] * 29 + + +@pytest.mark.parametrize("field", ["q", "dq", "kp", "kd", "tau"]) +@pytest.mark.parametrize("count,kind", [(29, G1WholeBodyConnection), (18, R1ProConnection)]) +def test_inconsistent_canonical_arrays_are_dropped_before_sdk(field, count, kind, mocker): + connection = kind() + with ExitStack() as cleanup: + cleanup.callback(connection.stop) + command = _command(count) + setattr(command, field, np.zeros(count - 1, dtype=np.float64)) + publisher = mocker.Mock() + if kind is G1WholeBodyConnection: + low_cmd = SimpleNamespace( + mode_machine=5, + crc=0, + motor_cmd=[ + SimpleNamespace(mode=1, q=0, dq=0, kp=0, kd=0, tau=0) for _ in range(35) + ], + ) + mocker.patch.multiple( + connection, + _publisher=publisher, + _low_cmd=low_cmd, + _crc=mocker.Mock(), + _mode_machine=5, + ) + else: + mocker.patch.object(connection, "_ros", publisher) + cleanup.callback(mocker.patch.object, connection, "_ros", None) + # R1 imports ROS only after validation; G1 has live fake DDS state. + connection._on_motor_command(command) + publisher.Write.assert_not_called() + publisher.publish.assert_not_called() + assert connection.inputs["motor_command"].type is MotorCommandArray + + +def test_r1_segment_publication_consumes_generated_command_without_physical_ros( + factory, mocker, monkeypatch +): + ros_module = ModuleType("sensor_msgs.msg") + ros_module.JointState = lambda: SimpleNamespace(header=SimpleNamespace(stamp=None)) + monkeypatch.setitem(sys.modules, "sensor_msgs.msg", ros_module) + stamp = SimpleNamespace(sec=17, nanosec=1) + mocker.patch("dimos.robot.galaxea.r1pro.connection._ros_stamp_now", return_value=stamp) + with ExitStack() as cleanup: + connection = R1ProConnection() + cleanup.callback(connection.stop) + ros = mocker.Mock() + mocker.patch.multiple( + connection, + _ros=ros, + _cmd_torso_topic="torso", + _cmd_left_topic="left", + _cmd_right_topic="right", + ) + cleanup.callback(mocker.patch.object, connection, "_ros", None) + wire = factory("/r1pro/motor_command", MotorCommandArray) + cleanup.callback(wire.stop) + cleanup.callback(wire.subscribe(connection._on_motor_command)) + written = Event() + ros.publish.side_effect = lambda *args: ( + written.set() if ros.publish.call_count == 3 else None + ) + wire.publish(_command(18)) + assert written.wait(5) + assert [call.args[0] for call in ros.publish.call_args_list] == ["torso", "left", "right"] + assert [call.args[1].position for call in ros.publish.call_args_list] == [ + list(range(4)), + list(range(4, 11)), + list(range(11, 18)), + ] + assert all(call.args[1].header.stamp is stamp for call in ros.publish.call_args_list) + assert connection.outputs["motor_states"].type is JointState + assert connection.outputs["imu_chassis"].type is Imu + + +def test_r1_feedback_stamp_preserves_epoch_nanoseconds_and_existing_zero_fallback(mocker): + mocker.patch("dimos.robot.galaxea.r1pro.connection.time.time_ns", return_value=99) + source = SimpleNamespace( + header=SimpleNamespace(stamp=SimpleNamespace(sec=1_789_654_321, nanosec=1)) + ) + assert _stamp_nanoseconds(source) == 1_789_654_321_000_000_001 + source.header.stamp.sec = 0 + source.header.stamp.nanosec = 0 + assert _stamp_nanoseconds(source) == 99 + + +def test_r1_fused_feedback_preserves_oldest_exact_segment_stamp(factory): + with ExitStack() as cleanup: + connection = R1ProConnection() + cleanup.callback(connection.stop) + wire = factory("/r1pro/motor_states", JointState) + cleanup.callback(wire.stop) + connection.set_transport("motor_states", wire) + observed = [] + ready = Event() + + def receive(value): + observed.append(value) + connection._stop_event.set() + ready.set() + + cleanup.callback(wire.subscribe(receive)) + for callback, count, nanos in [ + (connection._on_feedback_torso, 4, 3), + (connection._on_feedback_left, 7, 1), + (connection._on_feedback_right, 7, 2), + ]: + message = SimpleNamespace( + header=SimpleNamespace(stamp=SimpleNamespace(sec=1_789_654_321, nanosec=nanos)), + position=[float(nanos)] * count, + velocity=[0.5] * count, + effort=[0.75] * count, + ) + callback(message, None) + thread = Thread(target=connection._publish_loop) + + def close_thread(): + connection._stop_event.set() + thread.join(timeout=5) + assert not thread.is_alive() + + cleanup.callback(close_thread) + thread.start() + assert ready.wait(5) + assert observed[0].header.stamp == Time(1_789_654_321, 1) + assert observed[0].position.tolist() == [3.0] * 4 + [1.0] * 7 + [2.0] * 7 + assert observed[0].velocity.tolist() == [0.5] * 18 + assert observed[0].effort.tolist() == [0.75] * 18 + + +def test_wholebody_blueprints_bind_canonical_types_without_loading_robot_assets(mocker): + download = mocker.patch( + "dimos.utils.data.get_data", + side_effect=AssertionError("blueprint import must not download assets"), + ) + simple = import_module("dimos.robot.unitree.g1.blueprints.basic.unitree_g1_coordinator") + groot = import_module("dimos.robot.unitree.g1.blueprints.basic.unitree_g1_groot_wbc") + r1 = import_module("dimos.robot.galaxea.r1pro.blueprints.basic.r1pro_coordinator") + for blueprint in (simple.unitree_g1_coordinator, groot.unitree_g1_groot_wbc): + for name, kind in [ + ("motor_states", JointState), + ("motor_command", MotorCommandArray), + ("imu", Imu), + ("joint_command", JointState), + ("g1_joints", JointState), + ]: + spec = blueprint.transport_map[(name, kind)] + assert spec.cls is LCMTransport + assert r1._zenoh_transport("/r1pro/motor_states", JointState).cls is ZenohTransport + assert r1._zenoh_transport("/r1pro/imu", Imu).cls is ZenohTransport + assert r1._zenoh_transport("/r1pro/motor_command", MotorCommandArray).cls is ZenohTransport + download.assert_not_called() diff --git a/dimos/imitation/policy/lerobot/README.md b/dimos/imitation/policy/lerobot/README.md index 1ee7c44087c..2f1887c70a6 100644 --- a/dimos/imitation/policy/lerobot/README.md +++ b/dimos/imitation/policy/lerobot/README.md @@ -8,8 +8,13 @@ The host contract subscribes to: - `color_image: Image` - `coordinator_joint_state: JointState` -- `button_pressed: Buttons` -- `teleop_buttons: Buttons` +- `button_pressed: UInt32` +- `teleop_buttons: UInt32` + +These are generated CDR messages from `dimos.messages.sensor_msgs.msg` and +`dimos.messages.std_msgs.msg`. Images must use `rgb8` at the configured size; +row padding is respected. Joint positions are selected by the configured +names. The packed button bit layout stays the same as WebXR. It submits complete, timestamped action chunks to the existing `joint_trajectory` task through the control coordinator. The state and action @@ -60,3 +65,15 @@ cd native/python/lerobot uv run --isolated --locked --group tests --with-editable ../../.. python -m pytest uv run --isolated --locked --group tests --with-editable ../../.. python -m mypy ``` + +The checks use a fake policy and coordinator, require no checkpoint or robot, +and exercise real CDR memq/loopback Zenoh inputs. Policy images and state are +copied at the input boundary; source Header nanoseconds remain in cached state. +Trajectory RPCs retain their existing interface. + +Runtime projects still require the shared checkout described in +[the isolated Python guide](../../../experimental/isolated_python/README.md). +Installing a DimOS wheel does not ship this optional runtime project or pin +that shared checkout to the host revision. Use a matching checkout for a CDR +host and child; the package/checkout alignment gate remains separate from +these encoding and policy-lifecycle checks. diff --git a/dimos/imitation/policy/lerobot/module.py b/dimos/imitation/policy/lerobot/module.py index 9f23719c5e6..2a99e8c2b24 100644 --- a/dimos/imitation/policy/lerobot/module.py +++ b/dimos/imitation/policy/lerobot/module.py @@ -31,8 +31,8 @@ IsolatedPythonModule, IsolatedPythonModuleConfig, ) -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.messages.sensor_msgs.msg import Image, JointState +from dimos.messages.std_msgs.msg import UInt32 from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory from dimos.spec.utils import Spec from dimos.teleop.webxr.controller_types import BUTTON_ALIASES, Buttons @@ -119,8 +119,8 @@ class LeRobotPolicyModule(IsolatedPythonModule): color_image: In[Image] coordinator_joint_state: In[JointState] - button_pressed: In[Buttons] - teleop_buttons: In[Buttons] + button_pressed: In[UInt32] + teleop_buttons: In[UInt32] _control: PolicyControlSpec diff --git a/dimos/msgs/driver.py b/dimos/msgs/driver.py new file mode 100644 index 00000000000..3d5a0b49421 --- /dev/null +++ b/dimos/msgs/driver.py @@ -0,0 +1,148 @@ +# 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. + +"""Explicit adapters from internal numerical driver values to canonical wire values.""" + +from copy import deepcopy + +import numpy as np + +from dimos.message_utils.image import image_from_array +from dimos.message_utils.time import time_from_seconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import ( + Quaternion, + Transform as CdrTransform, + TransformStamped, + Vector3, +) +from dimos.messages.sensor_msgs.msg import ( + CameraInfo as CdrCameraInfo, + Image as CdrImage, + Imu as CdrImu, + PointCloud2 as CdrCloud, + PointField, + RegionOfInterest, +) +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo +from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + + +def camera_info_message(info: CameraInfo) -> CdrCameraInfo: + return CdrCameraInfo( + header=Header(time_from_seconds(info.ts), info.frame_id), + height=info.height, + width=info.width, + distortion_model=info.distortion_model, + d=np.asarray(info.D, dtype=np.float64).copy(), + k=np.asarray(info.K, dtype=np.float64).copy(), + r=np.asarray(info.R, dtype=np.float64).copy(), + p=np.asarray(info.P, dtype=np.float64).copy(), + binning_x=info.binning_x, + binning_y=info.binning_y, + roi=RegionOfInterest(0, 0, 0, 0, False), + ) + + +def cloud_message(cloud: PointCloud2) -> CdrCloud: + """Adapt the driver's numerical cloud, preserving every supported point attribute.""" + points, colors = cloud.as_numpy() + arrays = {name: points[:, index] for index, name in enumerate(("x", "y", "z"))} + types = {name: (" CdrImage: + encodings = { + "RGB": "rgb8", + "BGR": "bgr8", + "RGBA": "rgba8", + "BGRA": "bgra8", + "GRAY": "mono8", + "GRAY16": "mono16", + "DEPTH": "32FC1", + "DEPTH16": "16UC1", + } + return image_from_array( + image.data, + encoding=encodings[image.format.value], + header=Header(time_from_seconds(image.ts), image.frame_id), + ) + + +def transform_message(value: Transform, stamp: Time | None = None) -> TransformStamped: + """Keep local geometry intact, copying the full frames/pose at the wire edge.""" + t, q = value.translation, value.rotation + return TransformStamped( + header=Header( + deepcopy(stamp) if stamp is not None else time_from_seconds(value.ts), value.frame_id + ), + child_frame_id=value.child_frame_id, + transform=CdrTransform(Vector3(t.x, t.y, t.z), Quaternion(q.x, q.y, q.z, q.w)), + ) + + +def imu_message(value: Imu) -> CdrImu: + q, g, a = value.orientation, value.angular_velocity, value.linear_acceleration + return CdrImu( + header=Header(time_from_seconds(value.ts), value.frame_id), + orientation=Quaternion(q.x, q.y, q.z, q.w), + orientation_covariance=np.asarray(value.orientation_covariance, dtype=np.float64).copy(), + angular_velocity=Vector3(g.x, g.y, g.z), + angular_velocity_covariance=np.asarray( + value.angular_velocity_covariance, dtype=np.float64 + ).copy(), + linear_acceleration=Vector3(a.x, a.y, a.z), + linear_acceleration_covariance=np.asarray( + value.linear_acceleration_covariance, dtype=np.float64 + ).copy(), + ) diff --git a/dimos/protocol/pubsub/impl/rospubsub.py b/dimos/protocol/pubsub/impl/rospubsub.py index fde9cb29204..41ca5e95ebb 100644 --- a/dimos/protocol/pubsub/impl/rospubsub.py +++ b/dimos/protocol/pubsub/impl/rospubsub.py @@ -14,6 +14,7 @@ from collections.abc import Callable from dataclasses import dataclass +import logging import threading from typing import Any, Protocol, runtime_checkable @@ -38,7 +39,6 @@ import uuid from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT -from dimos.msgs.protocol import DimosMsg from dimos.protocol.pubsub.impl.rospubsub_conversion import ( derive_ros_type, dimos_to_ros, @@ -46,6 +46,8 @@ ) from dimos.protocol.pubsub.spec import PubSub +logger = logging.getLogger(__name__) + @runtime_checkable class ROSMessage(Protocol): @@ -68,7 +70,7 @@ class ROSTopic: """Topic descriptor for DimosROS pubsub (uses dimos message types).""" topic: str - msg_type: type[DimosMsg] + msg_type: type[Any] qos: "QoSProfile | None" = None @@ -176,6 +178,7 @@ def _spin(self) -> None: try: executor.spin_once(timeout_sec=0.01) except Exception: + logger.debug("ROS executor stopped after spin failure", exc_info=True) break def _get_or_create_publisher(self, topic: RawROSTopic) -> Any: @@ -247,7 +250,7 @@ def unsubscribe() -> None: return unsubscribe -class DimosROS(PubSub[ROSTopic, DimosMsg]): +class DimosROS(PubSub[ROSTopic, Any]): """ROS PubSub with automatic dimos.msgs ↔ ROS message conversion. Uses ROSTopic (with dimos msg_type) instead of RawROSTopic (with ros_type). @@ -277,7 +280,7 @@ def _to_raw_topic(self, topic: ROSTopic) -> RawROSTopic: ros_type = derive_ros_type(topic.msg_type) return RawROSTopic(topic=topic.topic, ros_type=ros_type, qos=topic.qos) - def publish(self, topic: ROSTopic, message: DimosMsg) -> None: + def publish(self, topic: ROSTopic, message: Any) -> None: """Publish a dimos message to a ROS topic. Args: @@ -289,7 +292,7 @@ def publish(self, topic: ROSTopic, message: DimosMsg) -> None: self._raw.publish(raw_topic, ros_message) def subscribe( - self, topic: ROSTopic, callback: Callable[[DimosMsg, ROSTopic], None] + self, topic: ROSTopic, callback: Callable[[Any, ROSTopic], None] ) -> Callable[[], None]: """Subscribe to a ROS topic with automatic dimos message conversion. diff --git a/dimos/protocol/pubsub/impl/rospubsub_conversion.py b/dimos/protocol/pubsub/impl/rospubsub_conversion.py index 6853c57ca91..77eeac48563 100644 --- a/dimos/protocol/pubsub/impl/rospubsub_conversion.py +++ b/dimos/protocol/pubsub/impl/rospubsub_conversion.py @@ -14,27 +14,49 @@ """Conversion functions between dimos messages and ROS messages. -This module provides conversion functions between dimos message types and ROS messages. -It handles three categories of types: - -1. Complex types (different internal representation) - use LCM roundtrip -2. Simple types (field structures match) - use direct field copy -3. No dimos.msgs equivalent - return dimos_lcm type +Registry-owned generated types use ROS 2's CDR serialization APIs. Both peers +need the same ROS schema and installed ROS message package. Explicit legacy +classes retain their existing field-copy / LCM conversion until retirement; +the generated path never falls back to those codecs. """ from __future__ import annotations import importlib import re -from typing import TYPE_CHECKING, Any, cast +from typing import TYPE_CHECKING, Any, TypeVar, cast + +from dimos_cdrgen.registry import encode as encode_cdr, message_types from dimos.msgs.helpers import lcm_msg_type +from dimos.protocol.pubsub.codec import CDRCodec if TYPE_CHECKING: from dimos.msgs.protocol import DimosMsg from dimos.protocol.pubsub.impl.rospubsub import ROSMessage +_Message = TypeVar("_Message") + + +def _generated_name(message_type: type[Any]) -> str | None: + name = getattr(message_type, "__msgtype__", None) + if name is None: + return None + if not isinstance(name, str) or message_types().get(name) is not message_type: + raise TypeError("Expected a registry-owned generated message class") + return name + + +def _serialization() -> Any: + try: + return importlib.import_module("rclpy.serialization") + except ImportError as error: + raise ImportError( + "Generated ROS conversion requires rclpy and installed ROS message packages" + ) from error + + # Complex types that need LCM roundtrip (explicit list) # These types have different internal representations in dimos vs ROS/LCM COMPLEX_TYPES: set[str] = { @@ -108,7 +130,7 @@ def derive_lcm_type(dimos_type: type[DimosMsg]) -> type[Any]: return lcm_type -def derive_ros_type(dimos_type: type[DimosMsg]) -> type[ROSMessage]: +def derive_ros_type(dimos_type: type[Any]) -> type[ROSMessage]: """Derive the ROS message type from a dimos message type. Args: @@ -120,7 +142,8 @@ def derive_ros_type(dimos_type: type[DimosMsg]) -> type[ROSMessage]: Example: msg_name = "geometry_msgs.Vector3" -> geometry_msgs.msg.Vector3 """ - msg_name = dimos_type.msg_name # e.g., "geometry_msgs.Vector3" + name = _generated_name(dimos_type) + msg_name = name.replace("/msg/", ".") if name is not None else dimos_type.msg_name parts = msg_name.split(".") if len(parts) != 2: raise ValueError(f"Invalid msg_name format: {msg_name}, expected 'package.MessageName'") @@ -304,7 +327,7 @@ def _create_ros_instance_for_lcm_msg(lcm_msg: Any, ros_type_hint: str) -> Any: raise ValueError(f"Cannot determine ROS type for LCM message: {lcm_type}") -def dimos_to_ros(msg: DimosMsg, ros_type: type[ROSMessage]) -> ROSMessage: +def dimos_to_ros(msg: Any, ros_type: type[ROSMessage]) -> ROSMessage: """Convert a dimos message to a ROS message. For complex types (PointCloud2, Image, CameraInfo), uses LCM roundtrip @@ -318,6 +341,10 @@ def dimos_to_ros(msg: DimosMsg, ros_type: type[ROSMessage]) -> ROSMessage: Returns: ROS message instance """ + if _generated_name(type(msg)) is not None: + if derive_ros_type(type(msg)) is not ros_type: + raise TypeError("Target ROS class does not match the generated message type") + return cast("ROSMessage", _serialization().deserialize_message(encode_cdr(msg), ros_type)) msg_name = type(msg).msg_name if msg_name in COMPLEX_TYPES: @@ -335,7 +362,7 @@ def dimos_to_ros(msg: DimosMsg, ros_type: type[ROSMessage]) -> ROSMessage: return ros_msg -def ros_to_dimos(msg: Any, dimos_type: type[DimosMsg]) -> DimosMsg: +def ros_to_dimos(msg: Any, dimos_type: type[_Message]) -> _Message: """Convert a ROS message to a dimos message. For complex types (PointCloud2, Image, CameraInfo), uses LCM roundtrip @@ -349,16 +376,21 @@ def ros_to_dimos(msg: Any, dimos_type: type[DimosMsg]) -> DimosMsg: Returns: Dimos message instance """ - msg_name = dimos_type.msg_name + if _generated_name(dimos_type) is not None: + if not isinstance(msg, derive_ros_type(dimos_type)): + raise TypeError("Source ROS class does not match the generated message type") + return CDRCodec(dimos_type).decode(bytes(_serialization().serialize_message(msg))) + legacy_type = cast("type[DimosMsg]", dimos_type) + msg_name = legacy_type.msg_name if msg_name in COMPLEX_TYPES: # Complex: ROS → LCM → encode → decode → dimos - lcm_type = derive_lcm_type(dimos_type) + lcm_type = derive_lcm_type(legacy_type) lcm_msg = lcm_type() _copy_ros_to_lcm_recursive(msg, lcm_msg) - return dimos_type.lcm_decode(lcm_msg.lcm_encode()) + return cast("_Message", legacy_type.lcm_decode(lcm_msg.lcm_encode())) # Simple type: recursive field copy (handles nested messages) - dimos_msg = dimos_type() + dimos_msg = legacy_type() _copy_ros_to_lcm_recursive(msg, dimos_msg) - return dimos_msg + return cast("_Message", dimos_msg) diff --git a/dimos/protocol/pubsub/impl/test_cdr_ros_conversion.py b/dimos/protocol/pubsub/impl/test_cdr_ros_conversion.py new file mode 100644 index 00000000000..1a98f36e17c --- /dev/null +++ b/dimos/protocol/pubsub/impl/test_cdr_ros_conversion.py @@ -0,0 +1,251 @@ +# 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. + +"""Generated ROS bridge; mock only ROS runtime using an independent CDR oracle. + +These tests do not claim a real ROS node installation or discovery acceptance. +""" + +from contextlib import ExitStack +import pickle +import queue +import sys +from types import ModuleType, SimpleNamespace + +from dimos_cdrgen.registry import encode, schema +import numpy as np +import pytest +from rosbags.typesys import Stores, get_types_from_msg, get_typestore + +from dimos.core.transport import LCMTransport, ROSTransport +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Vector3 +from dimos.messages.sensor_msgs.msg import Image, Imu +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.geometry_msgs.Vector3 import Vector3 as LegacyVector +from dimos.protocol.pubsub.impl import rospubsub, rospubsub_conversion as conversion +from dimos.robot.galaxea.r1pro.connection import R1ProConnection +from dimos.utils.testing.waiting import wait_until + + +@pytest.fixture +def ros_boundary(mocker, monkeypatch): + definitions = {} + for value in (PoseStamped, Image, Imu): + definitions.update(get_types_from_msg(schema(value.__msgtype__), value.__msgtype__)) + # A separate rosbags store replaces its global usertypes module on registration. + # Restore that module immediately: generated classes must retain pickle identity. + with monkeypatch.context() as isolated: + isolated.setitem(sys.modules, "rosbags.usertypes", sys.modules["rosbags.usertypes"]) + oracle = get_typestore(Stores.EMPTY) + oracle.register(definitions) + for package in ("geometry_msgs", "sensor_msgs"): + module = ModuleType(package + ".msg") + for name, cls in oracle.types.items(): + if name.startswith(package + "/msg/"): + setattr(module, name.rsplit("/", 1)[1], cls) + monkeypatch.setitem(sys.modules, package + ".msg", module) + serialization = ModuleType("rclpy.serialization") + serialization.serialize_message = mocker.Mock( + side_effect=lambda value: bytes(oracle.serialize_cdr(value, value.__msgtype__)) + ) + serialization.deserialize_message = mocker.Mock( + side_effect=lambda data, cls: oracle.deserialize_cdr(data, cls.__msgtype__) + ) + monkeypatch.setitem(sys.modules, "rclpy.serialization", serialization) + mocker.patch.object( + conversion, + "derive_lcm_type", + side_effect=AssertionError("generated values must never use LCM"), + ) + return SimpleNamespace(oracle=oracle, serialization=serialization) + + +def _sample(kind): + header = Header(Time(1_789_654_321, 1), "sensor") + if kind is PoseStamped: + return PoseStamped(header, Pose(Point(1, 2, 3), Quaternion(0, 0, 0, 1))) + if kind is Image: + return Image(header, 2, 2, "rgb8", 0, 8, np.arange(16, dtype=np.uint8)) + return Imu( + header, + Quaternion(0, 0, 0, 1), + np.arange(9, dtype=np.float64), + Vector3(0.1, 0.2, 0.3), + np.arange(9, dtype=np.float64) + 10, + Vector3(1, 2, 3), + np.arange(9, dtype=np.float64) + 20, + ) + + +@pytest.mark.parametrize("kind", [PoseStamped, Image, Imu]) +@pytest.mark.parametrize("little_endian", [True, False]) +def test_generated_values_reuse_ros_cdr_without_legacy_roundtrip(ros_boundary, kind, little_endian): + source = _sample(kind) + oracle = ros_boundary.oracle + cls = oracle.types[kind.__msgtype__] + ros = conversion.dimos_to_ros(source, cls) + assert type(ros) is cls + assert ros.header.stamp.nanosec == 1 + assert bytes(oracle.serialize_cdr(ros, kind.__msgtype__)) == encode(source) + ros_boundary.serialization.serialize_message.side_effect = lambda value: bytes( + oracle.serialize_cdr(value, value.__msgtype__, little_endian=little_endian) + ) + restored = conversion.ros_to_dimos(ros, kind) + assert type(restored) is kind + assert type(restored.header) is Header + assert encode(restored) == encode(source) + restored.header.frame_id = "changed" + assert source.header.frame_id == "sensor" and ros.header.frame_id == "sensor" + + +def test_wrong_ros_schema_is_rejected_before_serialization(ros_boundary): + source = _sample(PoseStamped) + with pytest.raises(TypeError, match="Target ROS class"): + conversion.dimos_to_ros(source, ros_boundary.oracle.types[Image.__msgtype__]) + with pytest.raises(TypeError, match="Source ROS class"): + conversion.ros_to_dimos(source, PoseStamped) + ros_boundary.serialization.serialize_message.assert_not_called() + ros_boundary.serialization.deserialize_message.assert_not_called() + + +def test_imposter_generated_class_is_rejected_before_importing_ros(): + class Imposter: + __msgtype__ = "geometry_msgs/msg/PoseStamped" + + with pytest.raises(TypeError, match="registry-owned"): + conversion.derive_ros_type(Imposter) + + +def test_ros_cdr_validation_rejects_malformed_payload_without_fallback(ros_boundary): + ros = conversion.dimos_to_ros( + _sample(PoseStamped), ros_boundary.oracle.types[PoseStamped.__msgtype__] + ) + ros_boundary.serialization.serialize_message.return_value = b"malformed" + ros_boundary.serialization.serialize_message.side_effect = None + with pytest.raises(ValueError, match="XCDR1"): + conversion.ros_to_dimos(ros, PoseStamped) + + +def test_transport_keeps_existing_ros_wrapper_lifecycle_and_generated_values(ros_boundary, mocker): + raw = mocker.Mock() + factory = mocker.patch.object(rospubsub, "RawROS", return_value=raw) + with ExitStack() as cleanup: + transport = ROSTransport("/camera", Image, node_name="cdr_offline_test") + cleanup.callback(transport.stop) + values = [] + cleanup.callback(transport.subscribe(values.append)) + source = _sample(Image) + transport.publish(source) + topic, ros = raw.publish.call_args.args + assert topic.topic == "/camera" and topic.ros_type is type(ros) + raw.subscribe.call_args.args[1](ros, topic) + assert len(values) == 1 and type(values[0]) is Image + assert encode(values[0]) == encode(source) + raw.start.assert_called_once_with() + factory.assert_called_once_with("cdr_offline_test", None) + raw.stop.assert_called_once_with() + raw.subscribe.return_value.assert_called_once_with() + + +def test_wrong_generated_transport_value_fails_before_ros_start(ros_boundary, mocker): + factory = mocker.patch.object(rospubsub, "RawROS") + transport = ROSTransport("/camera", Image) + with pytest.raises(TypeError, match="declared type"): + transport.publish(_sample(PoseStamped)) + factory.assert_not_called() + + +def test_pickled_ros_transport_preserves_declared_type_and_node_options(ros_boundary): + transport = ROSTransport("/camera", Image, node_name="offline") + restored = pickle.loads(pickle.dumps(transport)) + assert restored.topic.topic == "/camera" and restored.topic.msg_type is Image + assert restored._kwargs == {"node_name": "offline"} + + +def test_explicit_legacy_flat_conversion_stays_separate(mocker): + target = SimpleNamespace( + get_fields_and_field_types=lambda: {"x": "double", "y": "double", "z": "double"}, + x=1.0, + y=2.0, + z=3.0, + ) + serializer = mocker.patch.object( + conversion, "_serialization", side_effect=AssertionError("legacy path must not select CDR") + ) + value = conversion.ros_to_dimos(target, LegacyVector) + assert (value.x, value.y, value.z) == (1, 2, 3) + serializer.assert_not_called() + + +def test_ros_transport_rejects_imposter_before_constructing_runtime(mocker): + factory = mocker.patch.object(rospubsub, "RawROS") + + class Imposter: + __msgtype__ = Image.__msgtype__ + + with pytest.raises(TypeError, match="registry-owned"): + ROSTransport("/camera", Imposter) + factory.assert_not_called() + + +def test_missing_ros_serialization_reports_runtime_dependency(ros_boundary, monkeypatch): + monkeypatch.delitem(sys.modules, "rclpy.serialization") + with pytest.raises(ImportError, match="rclpy and installed ROS message packages"): + conversion.dimos_to_ros(_sample(Image), ros_boundary.oracle.types[Image.__msgtype__]) + + +def test_depth_and_imu_workers_reuse_ros_cdr_preserving_layout_and_ns(ros_boundary): + image = Image( + Header(Time(1_700_000_000, 1), "depth"), 2, 2, "16UC1", 1, 6, np.arange(12, dtype=np.uint8) + ) + imu = Imu( + Header(Time(1_700_000_000, 1), "imu"), + Quaternion(0.0, 0.0, 0.0, 1.0), + np.arange(9, dtype=np.float64), + Vector3(1.0, 2.0, 3.0), + np.arange(9, dtype=np.float64) + 10, + Vector3(4.0, 5.0, 6.0), + np.arange(9, dtype=np.float64) + 20, + ) + with ExitStack() as cleanup: + module = R1ProConnection() + cleanup.callback(module.stop) + depth = LCMTransport("/head_depth", Image, url="memq://") + cleanup.callback(depth.stop) + module.set_transport("head_depth", depth) + images = [] + cleanup.callback(depth.subscribe(images.append)) + for kind, source in [(Image, image), (Imu, imu)]: + pending = queue.Queue() + pending.put( + conversion.dimos_to_ros(source, ros_boundary.oracle.types[kind.__msgtype__]) + ) + pending.put(None) + if kind is Image: + module._convert_loop("head_depth", pending, Image) + else: + module._imu_loop("imu_chassis", pending) + wait_until(lambda: bool(images), timeout=5) + value = images[0] + assert (value.step, value.is_bigendian, value.encoding) == (6, 1, "16UC1") + np.testing.assert_array_equal(value.data, image.data) + assert value.header.stamp.nanosec == 1 + stored = module._latest_imu_chassis + assert stored.header.stamp.nanosec == 1 + assert stored.angular_velocity.y == 2.0 + np.testing.assert_array_equal( + stored.linear_acceleration_covariance, imu.linear_acceleration_covariance + ) + assert module.sensor_stats()["head_depth"]["decoded"] == 1 diff --git a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py index 5a79cc60dbc..1e22f3e728b 100644 --- a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py +++ b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python3 # Copyright 2025-2026 Dimensional Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -36,15 +35,10 @@ from dimos.core.transport import ZenohTransport from dimos.hardware.sensors.lidar.pointlio.module import PointLio from dimos.hardware.sensors.lidar.pointlio.pointlio_blueprints import mid360_for_pointlio -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.CompressedImage import CompressedImage -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray +from dimos.messages.dimos_msgs.msg import MotorCommandArray +from dimos.messages.geometry_msgs.msg import PoseStamped, Twist +from dimos.messages.nav_msgs.msg import Odometry +from dimos.messages.sensor_msgs.msg import CameraInfo, CompressedImage, Image, Imu, JointState from dimos.protocol.pubsub.impl.zenohpubsub import QOS_LATEST_WINS, Topic as ZenohTopic, Zenoh from dimos.robot.galaxea.r1pro.config import ( R1PRO_CHASSIS_LIDAR_HOST_IP, diff --git a/dimos/robot/galaxea/r1pro/connection.py b/dimos/robot/galaxea/r1pro/connection.py index cb7d063e1f3..fecbbe1c707 100644 --- a/dimos/robot/galaxea/r1pro/connection.py +++ b/dimos/robot/galaxea/r1pro/connection.py @@ -40,27 +40,39 @@ import time from typing import TYPE_CHECKING, Any +import numpy as np from pydantic import Field from reactivex.disposable import Disposable if TYPE_CHECKING: from dimos.protocol.pubsub.impl.rospubsub import RawROS, RawROSTopic +from copy import deepcopy + from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.hardware.whole_body.spec import VEL_STOP -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.CompressedImage import CompressedImage -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.message_utils.time import time_from_nanoseconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.dimos_msgs.msg import MotorCommandArray +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.sensor_msgs.msg import CompressedImage, Image, Imu, JointState +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage from dimos.robot.galaxea.r1pro.joints import UPPER_BODY_JOINTS, coordinator_name from dimos.utils.logging_config import setup_logger @@ -115,10 +127,11 @@ def _make_qos() -> Any: ) -def _stamp_secs(msg: Any) -> float: - """Header stamp in seconds, falling back to wall clock if the driver left it 0.""" - ts = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9 - return ts if ts > 0 else time.time() +def _stamp_nanoseconds(msg: Any) -> int: + """Exact source nanoseconds; retain the driver's existing zero/negative fallback.""" + stamp = msg.header.stamp + value = int(stamp.sec) * 1_000_000_000 + int(stamp.nanosec) + return value if value > 0 else time.time_ns() def _ros_stamp_now() -> Any: @@ -206,9 +219,9 @@ def __init__(self, **kwargs: Any) -> None: self._latest_right_dq: list[float] = [0.0] * 7 self._latest_right_eff: list[float] = [0.0] * 7 self._torso_seen = False - self._ts_torso = 0.0 - self._ts_left = 0.0 - self._ts_right = 0.0 + self._ts_torso_ns = 0 + self._ts_left_ns = 0 + self._ts_right_ns = 0 self._left_seen = False self._right_seen = False self._latest_imu_chassis: Imu | None = None @@ -516,8 +529,10 @@ def sensor_stats(self) -> dict[str, Any]: # Control input handlers def _on_motor_command(self, msg: MotorCommandArray) -> None: - if msg.num_joints != _NUM_MOTORS: - logger.warning(f"Expected {_NUM_MOTORS} motor commands, got {msg.num_joints}; ignoring") + if any(len(values) != _NUM_MOTORS for values in (msg.q, msg.dq, msg.kp, msg.kd, msg.tau)): + logger.warning( + "Expected %d complete motor commands; ignoring inconsistent frame", _NUM_MOTORS + ) return from sensor_msgs.msg import JointState as RosJointState @@ -537,8 +552,8 @@ def _on_motor_command(self, msg: MotorCommandArray) -> None: cmd = RosJointState() cmd.header.stamp = stamp cmd.name = [""] - cmd.position = list(msg.q[sl]) - cmd.velocity = self._tracking_velocities(msg.dq[sl]) + cmd.position = msg.q[sl].tolist() + cmd.velocity = self._tracking_velocities(msg.dq[sl].tolist()) cmd.effort = [0.0] ros.publish(topic, cmd) @@ -568,7 +583,7 @@ def _on_feedback_torso(self, msg: Any, _topic: Any) -> None: self._copy_segment( msg, self._latest_torso_q, self._latest_torso_dq, self._latest_torso_eff ) - self._ts_torso = _stamp_secs(msg) + self._ts_torso_ns = _stamp_nanoseconds(msg) self._torso_seen = True def _on_feedback_left(self, msg: Any, _topic: Any) -> None: @@ -576,7 +591,7 @@ def _on_feedback_left(self, msg: Any, _topic: Any) -> None: self._copy_segment( msg, self._latest_left_q, self._latest_left_dq, self._latest_left_eff ) - self._ts_left = _stamp_secs(msg) + self._ts_left_ns = _stamp_nanoseconds(msg) self._left_seen = True def _on_feedback_right(self, msg: Any, _topic: Any) -> None: @@ -584,7 +599,7 @@ def _on_feedback_right(self, msg: Any, _topic: Any) -> None: self._copy_segment( msg, self._latest_right_q, self._latest_right_dq, self._latest_right_eff ) - self._ts_right = _stamp_secs(msg) + self._ts_right_ns = _stamp_nanoseconds(msg) self._right_seen = True @staticmethod @@ -620,28 +635,39 @@ def _on_chassis_speed(self, msg: Any, _topic: Any) -> None: self._odom_y += (sy * vx + cy * vy) * dt self._odom_yaw += wz * dt - from dimos.msgs.geometry_msgs.Pose import Pose - from dimos.msgs.geometry_msgs.Quaternion import Quaternion - from dimos.msgs.geometry_msgs.Vector3 import Vector3 - half = self._odom_yaw * 0.5 - position = Vector3(self._odom_x, self._odom_y, 0.0) + position = Point(self._odom_x, self._odom_y, 0.0) orientation = Quaternion(0.0, 0.0, math.sin(half), math.cos(half)) frame_id = self.config.odom_frame_id base = self.config.frame_id - pose = PoseStamped(ts=now, frame_id=frame_id, position=position, orientation=orientation) + header = Header(Time(msg.header.stamp.sec, msg.header.stamp.nanosec), frame_id) + pose = PoseStamped(deepcopy(header), Pose(position, orientation)) self.odometry.publish( Odometry( - ts=now, - frame_id=frame_id, + header=deepcopy(header), child_frame_id=base, - pose=Pose(position, orientation), - twist=Twist(Vector3(vx, vy, 0.0), Vector3(0.0, 0.0, wz)), + pose=PoseWithCovariance(deepcopy(pose.pose), np.zeros(36, dtype=np.float64)), + twist=TwistWithCovariance( + Twist(Vector3(vx, vy, 0.0), Vector3(0.0, 0.0, wz)), + np.zeros(36, dtype=np.float64), + ), ) ) if self.config.publish_odom_tf: self.odom.publish(pose) - self.tf.publish(TFMessage(Transform.from_pose(base, pose))) + self.tf.publish( + TFMessage( + [ + TransformStamped( + deepcopy(header), + base, + Transform( + Vector3(position.x, position.y, position.z), deepcopy(orientation) + ), + ) + ] + ) + ) # Aggregated motor_states publish loop @@ -680,17 +706,16 @@ def _publish_loop(self) -> None: imu_torso = self._latest_imu_torso # Oldest of the three segments: the fused snapshot is only # as fresh as its stalest part. - ts = min(self._ts_torso, self._ts_left, self._ts_right) + ts = min(self._ts_torso_ns, self._ts_left_ns, self._ts_right_ns) if bootstrapped: self.motor_states.publish( JointState( - ts=ts, - frame_id=frame_id, + header=Header(time_from_nanoseconds(ts), frame_id), name=R1PRO_UPPER_BODY_JOINTS, - position=positions, # type: ignore[arg-type] - velocity=velocities, - effort=efforts, + position=np.asarray(positions, dtype=np.float64), + velocity=np.asarray(velocities, dtype=np.float64), + effort=np.asarray(efforts, dtype=np.float64), ) ) if imu_chassis is not None: @@ -724,14 +749,12 @@ def _compressed_image_loop(self, stream: str, q: queue.Queue[Any]) -> None: continue last_pub = t0 try: - stamp = msg.header.stamp - ts = stamp.sec + stamp.nanosec * 1e-9 + stamp = time_from_nanoseconds(_stamp_nanoseconds(msg)) out.publish( CompressedImage( - data=bytes(msg.data), + header=Header(stamp, msg.header.frame_id or stream), + data=np.frombuffer(bytes(msg.data), dtype=np.uint8).copy(), format="jpeg", - frame_id=msg.header.frame_id or stream, - ts=ts if ts > 0 else time.time(), ) ) self._record_decode(stream, (time.perf_counter() - t0) * 1e3, ok=True) diff --git a/dimos/robot/galaxea/r1pro/head_cameras.py b/dimos/robot/galaxea/r1pro/head_cameras.py index 7efc718b4ee..6673be59d20 100644 --- a/dimos/robot/galaxea/r1pro/head_cameras.py +++ b/dimos/robot/galaxea/r1pro/head_cameras.py @@ -31,6 +31,7 @@ from __future__ import annotations +from copy import deepcopy import threading import time from typing import Any @@ -40,6 +41,9 @@ from dimos.core.stream import Out from dimos.hardware.sensors.camera.utils.fsync_trigger import trigger_gmsl_cameras from dimos.hardware.sensors.camera.v4l2.module import V4L2Camera, V4L2CameraConfig +from dimos.message_utils.time import time_from_nanoseconds +from dimos.messages.sensor_msgs.msg import CameraInfo as CdrCameraInfo +from dimos.msgs.driver import camera_info_message from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.utils.logging_config import setup_logger @@ -144,8 +148,8 @@ class HeadCameraInfo(Module): config: HeadCameraInfoConfig - left_info: Out[CameraInfo] - right_info: Out[CameraInfo] + left_info: Out[CdrCameraInfo] + right_info: Out[CdrCameraInfo] def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) @@ -179,7 +183,9 @@ def _run(self) -> None: if self._stop.wait(_CALIBRATION_RETRY_S): return while not self._stop.is_set(): + stamp = time_from_nanoseconds(time.time_ns()) for out, info in ((self.left_info, left), (self.right_info, right)): - info.ts = time.time() - out.publish(info) + message = camera_info_message(info) + message.header.stamp = deepcopy(stamp) + out.publish(message) self._stop.wait(1.0 / self.config.publish_hz) diff --git a/dimos/robot/galaxea/r1pro/lio.py b/dimos/robot/galaxea/r1pro/lio.py index 1aef950ee6a..5b28e2a2c62 100644 --- a/dimos/robot/galaxea/r1pro/lio.py +++ b/dimos/robot/galaxea/r1pro/lio.py @@ -16,12 +16,24 @@ from __future__ import annotations +from copy import deepcopy from functools import cache from reactivex.disposable import Disposable from dimos.core.core import rpc from dimos.core.stream import In, Out +from dimos.message_utils.time import to_seconds +from dimos.messages.geometry_msgs.msg import ( + Point, + Pose as CdrPoseValue, + PoseStamped as CdrPose, + Quaternion as CdrQuaternion, + TransformStamped, +) +from dimos.messages.nav_msgs.msg import Odometry as CdrOdometry +from dimos.msgs.driver import transform_message +from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform @@ -88,11 +100,11 @@ class R1ProLio(StaticTfPublisher): config: R1ProLioConfig - odometry: In[Odometry] - pose: Out[PoseStamped] + odometry: In[CdrOdometry] + pose: Out[CdrPose] - def transforms(self) -> list[Transform]: - return mount_transforms(self.config) + def transforms(self) -> list[TransformStamped]: + return [transform_message(value) for value in mount_transforms(self.config)] @rpc def start(self) -> None: @@ -100,5 +112,19 @@ def start(self) -> None: self._lidar_to_base = mount_transforms(self.config)[0] self.register_disposable(Disposable(self.odometry.subscribe(self._on_odometry))) - def _on_odometry(self, msg: Odometry) -> None: - self.pose.publish(base_pose(msg, self._lidar_to_base)) + def _on_odometry(self, msg: CdrOdometry) -> None: + p, q = msg.pose.pose.position, msg.pose.pose.orientation + # Keep the existing mount composition math behind an explicit value adapter. + internal = Odometry( + ts=to_seconds(msg.header.stamp), + frame_id=msg.header.frame_id, + pose=Pose(position=[p.x, p.y, p.z], orientation=[q.x, q.y, q.z, q.w]), + ) + result = base_pose(internal, self._lidar_to_base) + p, q = result.position, result.orientation + self.pose.publish( + CdrPose( + deepcopy(msg.header), + CdrPoseValue(Point(p.x, p.y, p.z), CdrQuaternion(q.x, q.y, q.z, q.w)), + ) + ) diff --git a/dimos/robot/galaxea/r1pro/test_cdr_streams.py b/dimos/robot/galaxea/r1pro/test_cdr_streams.py new file mode 100644 index 00000000000..d6ff6c1f4ba --- /dev/null +++ b/dimos/robot/galaxea/r1pro/test_cdr_streams.py @@ -0,0 +1,293 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +from contextlib import ExitStack +from functools import partial +import queue +import sys +from types import ModuleType, SimpleNamespace + +import cv2 +import numpy as np +import pytest + +from dimos.core import transport as transport_module +from dimos.core.global_config import GlobalConfig +from dimos.core.transport_factory import make_transport +from dimos.hardware.drive_trains.transport.adapter import TransportTwistAdapter +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + PoseWithCovariance, + Quaternion, + Twist, + TwistWithCovariance, + Vector3, +) +from dimos.messages.nav_msgs.msg import Odometry +from dimos.messages.sensor_msgs.msg import CompressedImage +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.msgs.geometry_msgs.Transform import Transform as MathTransform +from dimos.msgs.geometry_msgs.Vector3 import Vector3 as MathVector +from dimos.protocol.pubsub.impl.lcmpubsub import LCM +from dimos.protocol.pubsub.impl.zenohpubsub import Zenoh +from dimos.robot.galaxea.r1pro import lio +from dimos.robot.galaxea.r1pro.connection import R1ProConnection +from dimos.utils.testing.waiting import wait_until + + +@pytest.fixture(params=["lcm", "zenoh"]) +def wires(request, monkeypatch): + monkeypatch.setattr(transport_module, "LCM", partial(LCM, url="memq://")) + monkeypatch.setattr( + transport_module, + "Zenoh", + partial( + Zenoh, + scouting=False, + multicast=False, + gossip=False, + connect=[], + listen=["tcp/127.0.0.1:0"], + ), + ) + build = partial(make_transport, g=GlobalConfig(transport=request.param)) + owned = {} + + def shared(topic, kind): + # memq is endpoint-local; share each logical wire as the coordinator does. + key = topic, kind + if key not in owned: + owned[key] = build(topic, kind) + return owned[key] + + try: + yield shared + finally: + for wire in owned.values(): + wire.stop() + + +def _speed(sec, nanosec=1): + return SimpleNamespace( + header=SimpleNamespace(stamp=SimpleNamespace(sec=sec, nanosec=nanosec)), + twist=SimpleNamespace(linear=SimpleNamespace(x=1.0, y=0.5), angular=SimpleNamespace(z=0.2)), + ) + + +def test_wheel_feedback_preserves_exact_source_stamp_and_tf_ownership(wires): + with ExitStack() as cleanup: + connection = R1ProConnection() + cleanup.callback(connection.stop) + received = {} + for port, kind in [("odometry", Odometry), ("odom", PoseStamped), ("tf", TFMessage)]: + wire = wires("/chassis/" + port, kind) + connection.set_transport(port, wire) + received[port] = [] + cleanup.callback(wire.subscribe(received[port].append)) + adapter = TransportTwistAdapter(hardware_id="chassis", transport_cls=wires) + cleanup.callback(adapter.disconnect) + assert adapter.connect() + connection._on_chassis_speed(_speed(1_700_000_000), None) + source = _speed(1_700_000_000, 500_000_001) + connection._on_chassis_speed(source, None) + wait_until( + lambda: all(received.values()) and adapter.read_odometry() is not None, timeout=5 + ) + odom, pose, tf = (received[name][0] for name in ("odometry", "odom", "tf")) + assert (odom.header.stamp.sec, odom.header.stamp.nanosec) == (1_700_000_000, 500_000_001) + assert pose.pose.position.x == pytest.approx(0.5) + assert adapter.read_odometry() == pytest.approx([0.5, 0.25, 0.1]) + assert odom.child_frame_id == tf.transforms[0].child_frame_id == connection.config.frame_id + assert tf.transforms[0].transform.translation.x == pose.pose.position.x + np.testing.assert_array_equal(odom.pose.covariance, np.zeros(36)) + assert odom.twist.twist.linear.y == 0.5 + source.header.stamp.nanosec = 99 + assert odom.header.stamp.nanosec == 500_000_001 + connection._on_chassis_speed(_speed(1_700_000_000, 500_000_001), None) + connection._on_chassis_speed(_speed(1_700_000_010), None) + assert connection._odom_x == pytest.approx(0.5) + + +def test_pointlio_mode_keeps_wheel_feedback_without_second_tf_parent(mocker): + with ExitStack() as cleanup: + connection = R1ProConnection(publish_odom_tf=False) + cleanup.callback(connection.stop) + odometry = mocker.patch.object(connection.odometry, "publish") + pose = mocker.patch.object(connection.odom, "publish") + tf = mocker.patch.object(connection.tf, "publish") + connection._on_chassis_speed(_speed(17), None) + connection._on_chassis_speed(_speed(17, 500_000_001), None) + assert odometry.call_args.args[0].pose.pose.position.x == pytest.approx(0.5) + pose.assert_not_called() + tf.assert_not_called() + + +def test_chassis_command_roundtrip_uses_existing_ros_scalar_contract(wires, mocker, monkeypatch): + ros_types = ModuleType("geometry_msgs.msg") + ros_types.TwistStamped = lambda: SimpleNamespace( + header=SimpleNamespace(stamp=None), + twist=SimpleNamespace(linear=SimpleNamespace(x=0.0, y=0.0), angular=SimpleNamespace(z=0.0)), + ) + monkeypatch.setitem(sys.modules, "geometry_msgs.msg", ros_types) + mocker.patch("dimos.robot.galaxea.r1pro.connection._ros_stamp_now", return_value="ros-clock") + with ExitStack() as cleanup: + connection = R1ProConnection() + cleanup.callback(connection.stop) + ros = mocker.Mock() + mocker.patch.multiple(connection, _ros=ros, _speed_topic="chassis-speed") + wire = wires("/chassis/cmd_vel", Twist) + connection.set_transport("cmd_vel", wire) + cleanup.callback(wire.subscribe(connection._on_cmd_vel)) + adapter = TransportTwistAdapter(hardware_id="chassis", transport_cls=wires) + cleanup.callback(adapter.disconnect) + assert adapter.connect() + assert not adapter.write_velocities([1.0, 2.0, 3.0]) + assert adapter.write_enable(True) + assert adapter.write_velocities([1.0, 2.0, 3.0]) + wait_until(lambda: ros.publish.call_count == 1, timeout=5) + topic, value = ros.publish.call_args.args + assert (topic, value.header.stamp) == ("chassis-speed", "ros-clock") + assert (value.twist.linear.x, value.twist.linear.y, value.twist.angular.z) == ( + 1.0, + 2.0, + 3.0, + ) + assert adapter.write_enable(False) + wait_until(lambda: ros.publish.call_count == 2, timeout=5) + stop = ros.publish.call_args.args[1].twist + assert (stop.linear.x, stop.linear.y, stop.angular.z) == (0.0, 0.0, 0.0) + assert not adapter.write_velocities([1.0, 2.0, 3.0]) + + +@pytest.mark.parametrize("frame", ["", "wrist-optical"]) +def test_jpeg_worker_preserves_encoded_bytes_source_ns_and_frame_fallback(wires, frame): + success, jpeg = cv2.imencode(".jpg", np.full((2, 3, 3), 71, dtype=np.uint8)) + assert success + with ExitStack() as cleanup: + connection = R1ProConnection(color_publish_hz=0) + cleanup.callback(connection.stop) + wire = wires("/wrist_left_color", CompressedImage) + connection.set_transport("wrist_left_color", wire) + values = [] + cleanup.callback(wire.subscribe(values.append)) + source = SimpleNamespace( + header=SimpleNamespace( + stamp=SimpleNamespace(sec=1_700_000_000, nanosec=1), frame_id=frame + ), + data=jpeg.tobytes(), + ) + pending = queue.Queue() + connection._make_rx_cb("wrist_left_color", pending)(source, None) + pending.put(None) + connection._compressed_image_loop("wrist_left_color", pending) + wait_until(lambda: bool(values), timeout=5) + value = values[0] + assert value.data.tobytes() == jpeg.tobytes() + assert value.format == "jpeg" + assert value.header.frame_id == (frame or "wrist_left_color") + assert (value.header.stamp.sec, value.header.stamp.nanosec) == (1_700_000_000, 1) + stats = connection.sensor_stats()["wrist_left_color"] + assert (stats["received"], stats["decoded"], stats["errors"], stats["bytes_in"]) == ( + 1, + 1, + 0, + len(jpeg), + ) + + +def test_lio_mount_math_preserves_exact_header_and_source_ownership(wires, mocker): + with ExitStack() as cleanup: + module = lio.R1ProLio() + cleanup.callback(module.stop) + mocker.patch.object( + module, + "_lidar_to_base", + MathTransform( + translation=MathVector(-0.5, 0.25, 0.0), + frame_id="lidar", + child_frame_id="base_link", + ), + create=True, + ) + wire = wires("/chassis/pose", PoseStamped) + module.set_transport("pose", wire) + poses = [] + cleanup.callback(wire.subscribe(poses.append)) + source = Odometry( + Header(Time(1_700_000_000, 1), "world"), + "lidar", + PoseWithCovariance( + Pose(Point(2.0, 3.0, 4.0), Quaternion(0.0, 0.0, 0.0, 1.0)), np.zeros(36) + ), + TwistWithCovariance( + Twist(Vector3(0.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), np.zeros(36) + ), + ) + module._on_odometry(source) + wait_until(lambda: bool(poses), timeout=5) + value = poses[0] + assert (value.pose.position.x, value.pose.position.y, value.pose.position.z) == ( + 1.5, + 3.25, + 4.0, + ) + assert (value.header.stamp.sec, value.header.stamp.nanosec, value.header.frame_id) == ( + 1_700_000_000, + 1, + "world", + ) + source.header.stamp.nanosec = 2 + assert value.header.stamp.nanosec == 1 + + +def test_lio_static_mounts_are_canonical_without_loading_external_urdf(mocker): + model = SimpleNamespace( + joints=[ + SimpleNamespace( + child_link="lidar_chassis_left_link", + parent_link="base_link", + origin_xyz=[0.5, 0.25, 0.75], + origin_rpy=[0.0, 0.0, 0.0], + ), + SimpleNamespace( + child_link="lidar_pointlio_link", + parent_link="lidar_chassis_left_link", + origin_xyz=[0.0, 0.0, 0.0], + origin_rpy=[0.0, 0.0, 0.0], + ), + ] + ) + mocker.patch.object(lio, "lio_model", return_value=model) + with ExitStack() as cleanup: + module = lio.R1ProLio() + cleanup.callback(module.stop) + moving_mount, chassis_mount = module.transforms() + assert (moving_mount.header.frame_id, moving_mount.child_frame_id) == ( + "lidar_pointlio_link", + "base_link", + ) + t = moving_mount.transform.translation + assert (t.x, t.y, t.z) == (-0.5, -0.25, -0.75) + assert (chassis_mount.header.frame_id, chassis_mount.child_frame_id) == ( + "base_link", + "lidar_chassis_left_link", + ) + t = chassis_mount.transform.translation + assert (t.x, t.y, t.z) == (0.5, 0.25, 0.75) diff --git a/dimos/robot/galaxea/r1pro/test_head_cameras.py b/dimos/robot/galaxea/r1pro/test_head_cameras.py index dceb9189216..4e47623b71b 100644 --- a/dimos/robot/galaxea/r1pro/test_head_cameras.py +++ b/dimos/robot/galaxea/r1pro/test_head_cameras.py @@ -14,9 +14,12 @@ from __future__ import annotations +from contextlib import ExitStack from pathlib import Path -from dimos.msgs.sensor_msgs.CompressedImage import CompressedImage +from dimos.core.transport import LCMTransport +from dimos.messages.sensor_msgs.msg import CameraInfo, CompressedImage +from dimos.robot.galaxea.r1pro import head_cameras from dimos.robot.galaxea.r1pro.blueprints.basic.r1pro_coordinator import r1pro_control from dimos.robot.galaxea.r1pro.connection import R1ProConnection from dimos.robot.galaxea.r1pro.head_cameras import ( @@ -26,6 +29,7 @@ HeadRightCameraConfig, head_camera_infos, ) +from dimos.utils.testing.waiting import wait_until # The ISX031's only real mode: width, height (px), pixel format. _SENSOR_MODE = (1920, 1536, "UYVY") @@ -98,3 +102,29 @@ def test_r1pro_control_publishes_both_eyes_intrinsics() -> None: blueprint = r1pro_control() assert {"head_left_info", "head_right_info"} <= set(blueprint.remapping_map.values()) + + +def test_head_calibration_publishes_both_canonical_eyes_at_one_exact_clock(tmp_path, monkeypatch): + calibration = tmp_path / "stereo.yaml" + calibration.write_text(_CALIBRATION) + monkeypatch.setattr(head_cameras.time, "time_ns", lambda: 1_700_000_000_000_000_001) + with ExitStack() as cleanup: + module = head_cameras.HeadCameraInfo(calibration_path=str(calibration)) + cleanup.callback(module.stop) + eyes = [] + for name in ("left_info", "right_info"): + wire = LCMTransport("/" + name, CameraInfo, url="memq://") + cleanup.callback(wire.stop) + module.set_transport(name, wire) + cleanup.callback(wire.subscribe(eyes.append)) + # Stop at the first complete publication pair; no thread or fixed wait needed. + monkeypatch.setattr(module._stop, "wait", lambda _: module._stop.set()) + module._run() + wait_until(lambda: len(eyes) == 2, timeout=5) + values = {v.header.frame_id: v for v in eyes} + left, right = values[head_cameras.HEAD_LEFT_FRAME], values[head_cameras.HEAD_RIGHT_FRAME] + for eye in (left, right): + assert (eye.header.stamp.sec, eye.header.stamp.nanosec) == (1_700_000_000, 1) + assert (eye.width, eye.height, len(eye.d)) == (1920, 1536, 8) + assert (left.k[0], right.k[2]) == (1012.5, 958.7) + assert left.p[3] == right.p[3] == 0.0 diff --git a/dimos/robot/unitree/dimsim_connection.py b/dimos/robot/unitree/dimsim_connection.py index 8b30d74fd99..f0c6ba8f76f 100644 --- a/dimos/robot/unitree/dimsim_connection.py +++ b/dimos/robot/unitree/dimsim_connection.py @@ -13,6 +13,7 @@ # limitations under the License. from collections.abc import Callable +from copy import deepcopy import functools import threading from typing import Any @@ -20,17 +21,21 @@ from reactivex import Observable, Subject from dimos.core.global_config import TransportBackend -from dimos.core.transport import PubSubTransport -from dimos.core.transport_factory import make_transport +from dimos.core.transport import LCMTransport, PubSubTransport +from dimos.messages.geometry_msgs.msg import ( + PoseStamped as CdrPoseStamped, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.simulation.dimsim.config import DimSimConfig from dimos.simulation.dimsim.dimsim_process import DimSimProcess from dimos.utils.logging_config import setup_logger @@ -58,8 +63,10 @@ def __init__(self, config: DimSimConfig, *, transport: TransportBackend) -> None self.config = config self._transport_backend = transport self._dimsim_process: DimSimProcess = DimSimProcess(config) - self._odom_transport: PubSubTransport[PoseStamped] = make_transport("/odom", PoseStamped) - self._tf_transport: PubSubTransport[TFMessage] = make_transport("/tf", TFMessage) + self._odom_transport: PubSubTransport[CdrPoseStamped] = LCMTransport( + "/odom", CdrPoseStamped + ) + self._tf_transport: PubSubTransport[TFMessage] = LCMTransport("/tf", TFMessage) self._unsubscribe_odom: Callable[[], None] | None = None self._first_odom = threading.Event() @@ -84,7 +91,8 @@ def stop(self) -> None: if self._unsubscribe_odom is not None: self._unsubscribe_odom() self._unsubscribe_odom = None - self._odom_transport.stop() + self._odom_transport.stop() + self._tf_transport.stop() self._dimsim_process.stop() @functools.cache @@ -137,43 +145,32 @@ def switch_joystick(self, enable: bool = True) -> bool: def publish_request(self, topic: str, data: dict[str, Any]) -> dict[Any, Any]: return {} - def _handle_odom(self, msg: PoseStamped) -> None: + def _handle_odom(self, msg: CdrPoseStamped) -> None: self._first_odom.set() - self._tf_transport.publish(TFMessage(*_odom_to_tf(msg))) + self._tf_transport.publish(TFMessage(_odom_to_tf(msg))) -def _odom_to_tf(odom: PoseStamped) -> list[Transform]: - """Build transform chain from odometry pose. - - Transform tree: world -> base_link -> {camera_link -> camera_optical, lidar_link} - """ - camera_link = Transform( - translation=Vector3(0.3, 0.0, 0.0), # camera 30cm forward - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", - child_frame_id="camera_link", - ts=odom.ts, - ) - - camera_optical = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), - frame_id="camera_link", - child_frame_id="camera_optical", - ts=odom.ts, - ) - - lidar_link = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", - child_frame_id="lidar_link", - ts=odom.ts, - ) - +def _odom_to_tf(odom: CdrPoseStamped) -> list[TransformStamped]: + """Keep the existing world/base/camera/lidar mounts with exact source time.""" + stamp = odom.header.stamp + p, q = odom.pose.position, odom.pose.orientation return [ - Transform.from_pose("base_link", odom), - camera_link, - camera_optical, - lidar_link, + TransformStamped( + deepcopy(odom.header), "base_link", Transform(Vector3(p.x, p.y, p.z), deepcopy(q)) + ), + TransformStamped( + Header(deepcopy(stamp), "base_link"), + "camera_link", + Transform(Vector3(0.3, 0.0, 0.0), Quaternion(0.0, 0.0, 0.0, 1.0)), + ), + TransformStamped( + Header(deepcopy(stamp), "camera_link"), + "camera_optical", + Transform(Vector3(0.0, 0.0, 0.0), Quaternion(-0.5, 0.5, -0.5, 0.5)), + ), + TransformStamped( + Header(deepcopy(stamp), "base_link"), + "lidar_link", + Transform(Vector3(0.0, 0.0, 0.0), Quaternion(0.0, 0.0, 0.0, 1.0)), + ), ] diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py index 35408e812ba..927607a0b2d 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python3 # Copyright 2025-2026 Dimensional Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -28,9 +27,8 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.stream import Out from dimos.core.transport import LCMTransport -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray +from dimos.messages.dimos_msgs.msg import MotorCommandArray +from dimos.messages.sensor_msgs.msg import Imu, JointState from dimos.robot.unitree.g1.wholebody_connection import G1WholeBodyConnection _g1_joints = make_humanoid_joints("g1") @@ -71,13 +69,13 @@ class _G1Coordinator(ControlCoordinator): # collide with ControlCoordinator's (joint_state/joint_command/...). .transports( { - ("motor_states", JointState): LCMTransport("/g1/motor_states", JointState), - ("imu", Imu): LCMTransport("/g1/imu", Imu), - ("motor_command", MotorCommandArray): LCMTransport( + ("motor_states", JointState): LCMTransport.spec("/g1/motor_states", JointState), + ("imu", Imu): LCMTransport.spec("/g1/imu", Imu), + ("motor_command", MotorCommandArray): LCMTransport.spec( "/g1/motor_command", MotorCommandArray ), - ("joint_command", JointState): LCMTransport("/g1/joint_command", JointState), - ("g1_joints", JointState): LCMTransport("/g1/joints", JointState), + ("joint_command", JointState): LCMTransport.spec("/g1/joint_command", JointState), + ("g1_joints", JointState): LCMTransport.spec("/g1/joints", JointState), } ) ) diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py index 0d71bb043de..d28c090c987 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py @@ -69,11 +69,10 @@ from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.mapping.costmapper import CostMapper from dimos.mapping.pointclouds.occupancy import HeightCostConfig -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.Path import Path as NavPath -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray +from dimos.messages.dimos_msgs.msg import MotorCommandArray +from dimos.messages.geometry_msgs.msg import Twist +from dimos.messages.nav_msgs.msg import Path as NavPath +from dimos.messages.sensor_msgs.msg import Imu, JointState from dimos.navigation.go2.replanning_a_star.module import ReplanningAStarPlanner from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.robot.unitree.g1.config import G1 @@ -87,6 +86,7 @@ from dimos.robot.unitree.g1.teleop_ik import G1PinkPoseTargetSolver from dimos.simulation.scene_assets.spec import ScenePackage from dimos.utils.data import LfsPath +from dimos.visualization.rerun.cdr import path_archetype from dimos.visualization.rerun.scene_package import scene_package_static_entities from dimos.visualization.vis_module import vis_module @@ -377,7 +377,7 @@ def _g1_groot_rerun_blueprint() -> Any: def _g1_nav_path(path: NavPath) -> Any: - return path.to_rerun(z_offset=0.3) + return path_archetype(path, z_offset=0.3) # Mesh root: sim roots under the /odom transform; real hw under the LIO's @@ -550,16 +550,18 @@ def _viewer() -> Any: ], ).transports( { - ("joint_command", JointState): LCMTransport("/g1/joint_command", JointState), - ("g1_joints", JointState): LCMTransport(_G1_JOINTS_TOPIC, JointState), - ("cmd_vel", Twist): LCMTransport(_cmd_vel_topic, Twist), + ("joint_command", JointState): LCMTransport.spec("/g1/joint_command", JointState), + ("g1_joints", JointState): LCMTransport.spec(_G1_JOINTS_TOPIC, JointState), + ("cmd_vel", Twist): LCMTransport.spec(_cmd_vel_topic, Twist), # Real-hw only: the transport_lcm adapter speaks to # G1WholeBodyConnection over these topics. autoconnect already # matches by (name, type) so sim doesn't need them; they're # harmless when the sim engine doesn't expose those ports. - ("motor_states", JointState): LCMTransport("/g1/motor_states", JointState), - ("imu", Imu): LCMTransport("/g1/imu", Imu), - ("motor_command", MotorCommandArray): LCMTransport("/g1/motor_command", MotorCommandArray), + ("motor_states", JointState): LCMTransport.spec("/g1/motor_states", JointState), + ("imu", Imu): LCMTransport.spec("/g1/imu", Imu), + ("motor_command", MotorCommandArray): LCMTransport.spec( + "/g1/motor_command", MotorCommandArray + ), } ) diff --git a/dimos/robot/unitree/g1/connection.py b/dimos/robot/unitree/g1/connection.py index 8d61f1eb225..a203a16b296 100644 --- a/dimos/robot/unitree/g1/connection.py +++ b/dimos/robot/unitree/g1/connection.py @@ -21,7 +21,9 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In +from dimos.messages.geometry_msgs.msg import Twist as CdrTwist from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.robot.unitree.config import connection_type from dimos.robot.unitree.connection import UnitreeWebRTCConnection from dimos.utils.logging_config import setup_logger @@ -29,6 +31,15 @@ logger = setup_logger() +def _driver_twist(value: Twist | CdrTwist) -> Twist: + if isinstance(value, CdrTwist): + return Twist( + linear=Vector3(value.linear.x, value.linear.y, value.linear.z), + angular=Vector3(value.angular.x, value.angular.y, value.angular.z), + ) + return value + + class G1Config(ModuleConfig): ip: str = Field(default_factory=lambda m: m["g"].robot_ip) connection_type: str = Field( @@ -62,7 +73,7 @@ def stop(self) -> None: @rpc @abstractmethod - def move(self, twist: Twist, duration: float = 0.0) -> None: ... + def move(self, twist: Twist | CdrTwist, duration: float = 0.0) -> None: ... @rpc @abstractmethod @@ -71,7 +82,7 @@ def publish_request(self, topic: str, data: dict[str, Any]) -> dict[Any, Any]: . class G1Connection(G1ConnectionBase): config: G1Config - cmd_vel: In[Twist] + cmd_vel: In[CdrTwist] connection: UnitreeWebRTCConnection | None = None @rpc @@ -104,9 +115,9 @@ def stop(self) -> None: super().stop() @rpc - def move(self, twist: Twist, duration: float = 0.0) -> None: + def move(self, twist: Twist | CdrTwist, duration: float = 0.0) -> None: assert self.connection is not None - self.connection.move(twist, duration) + self.connection.move(_driver_twist(twist), duration) @rpc def publish_request(self, topic: str, data: dict[str, Any]) -> dict[Any, Any]: diff --git a/dimos/robot/unitree/g1/g1_rerun.py b/dimos/robot/unitree/g1/g1_rerun.py index dca02403f61..fe445158ef3 100644 --- a/dimos/robot/unitree/g1/g1_rerun.py +++ b/dimos/robot/unitree/g1/g1_rerun.py @@ -20,6 +20,12 @@ import numpy as np +from dimos.message_utils.occupancy import occupancy_view +from dimos.message_utils.time import to_seconds +from dimos.messages.geometry_msgs.msg import PoseStamped as CdrPose +from dimos.messages.nav_msgs.msg import OccupancyGrid as CdrGrid +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid from dimos.visualization.rerun.urdf_robot import ( UrdfRobotJointStateRerunFactory, UrdfRobotStaticRerunFactory, @@ -43,6 +49,16 @@ def g1_costmap(grid: Any, z_offset: float = 0.02) -> Any: The default z_offset lifts the mesh 2cm off the floor plane to avoid z-fighting with the ground. """ + if isinstance(grid, CdrGrid): + p, q = grid.info.origin.position, grid.info.origin.orientation + # Preserve the existing renderer's numerical and palette contract. + grid = OccupancyGrid( + grid=occupancy_view(grid), + resolution=grid.info.resolution, + origin=Pose(position=[p.x, p.y, p.z], orientation=[q.x, q.y, q.z, q.w]), + frame_id=grid.header.frame_id, + ts=to_seconds(grid.header.stamp), + ) return grid.to_rerun(color_lookup_table=_COSTMAP_LOOKUP_TABLE, z_offset=z_offset) @@ -82,14 +98,19 @@ def g1_odometry_tf_override(odom: Any) -> Any: """ import rerun as rr + if isinstance(odom, CdrPose): + position, orientation = odom.pose.position, odom.pose.orientation + x, y = position.x, position.y + else: + x, y, orientation = odom.x, odom.y, odom.orientation tf = rr.Transform3D( - translation=[odom.x, odom.y, 0.0], + translation=[x, y, 0.0], rotation=rr.Quaternion( xyzw=[ - odom.orientation.x, - odom.orientation.y, - odom.orientation.z, - odom.orientation.w, + orientation.x, + orientation.y, + orientation.z, + orientation.w, ] ), parent_frame="tf#/map", diff --git a/dimos/robot/unitree/g1/g1_tf_publisher.py b/dimos/robot/unitree/g1/g1_tf_publisher.py index 7237a76218e..ee0e40d5520 100644 --- a/dimos/robot/unitree/g1/g1_tf_publisher.py +++ b/dimos/robot/unitree/g1/g1_tf_publisher.py @@ -23,7 +23,6 @@ import asyncio import math import threading -import time from typing import Any from pydantic import Field @@ -32,10 +31,12 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import Out +from dimos.message_utils.time import header_now +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.msgs.driver import transform_message from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -182,7 +183,8 @@ def _init_lowstate_subscriber(self) -> Any: ChannelFactoryInitialize(0) except Exception as e: logger.warning( - f"ChannelFactoryInitialize failed - publishing rest-pose waist only: {e}" + f"ChannelFactoryInitialize failed - publishing rest-pose waist only: {e}", + exc_info=True, ) return None subscriber = ChannelSubscriber("rt/lowstate", LowState_) @@ -212,8 +214,6 @@ async def _publish_loop(self) -> None: with self._waist_lock: waist_yaw, waist_roll, waist_pitch = self._waist transforms = mount_transforms(waist_yaw, waist_roll, waist_pitch) - now = time.time() - for transform in transforms: - transform.ts = now - self.tf.publish(TFMessage(*transforms)) + stamp = header_now().stamp + self.tf.publish(TFMessage([transform_message(value, stamp) for value in transforms])) await asyncio.sleep(period) diff --git a/dimos/robot/unitree/g1/mujoco_sim.py b/dimos/robot/unitree/g1/mujoco_sim.py index fe777a8812c..6507e152b27 100644 --- a/dimos/robot/unitree/g1/mujoco_sim.py +++ b/dimos/robot/unitree/g1/mujoco_sim.py @@ -24,16 +24,29 @@ from dimos.core.core import rpc from dimos.core.module import ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.message_utils.time import time_from_seconds +from dimos.messages.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion as CdrQuaternion, + Twist as CdrTwist, +) +from dimos.messages.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.msgs.driver import ( + camera_info_message, + cloud_message, + image_message, + transform_message, +) +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped as DriverPose from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.robot.unitree.g1.connection import G1ConnectionBase +from dimos.robot.unitree.g1.connection import G1ConnectionBase, _driver_twist from dimos.robot.unitree.mujoco_connection import MujocoConnection from dimos.robot.unitree.type.odometry import Odometry as SimOdometry from dimos.simulation.mujoco.config import MujocoConfig @@ -49,7 +62,7 @@ class G1SimConfig(ModuleConfig): class G1SimConnection(G1ConnectionBase): config: G1SimConfig - cmd_vel: In[Twist] + cmd_vel: In[CdrTwist] lidar: Out[PointCloud2] odom: Out[PoseStamped] color_image: Out[Image] @@ -73,8 +86,16 @@ def start(self) -> None: self.register_disposable(Disposable(self.cmd_vel.subscribe(self.move))) self.register_disposable(self.connection.odom_stream().subscribe(self._publish_sim_odom)) - self.register_disposable(self.connection.lidar_stream().subscribe(self.lidar.publish)) - self.register_disposable(self.connection.video_stream().subscribe(self.color_image.publish)) + self.register_disposable( + self.connection.lidar_stream().subscribe( + lambda value: self.lidar.publish(cloud_message(value)) + ) + ) + self.register_disposable( + self.connection.video_stream().subscribe( + lambda value: self.color_image.publish(image_message(value)) + ) + ) self._camera_info_thread = Thread( target=self._publish_camera_info_loop, @@ -93,13 +114,18 @@ def stop(self) -> None: def _publish_camera_info_loop(self) -> None: assert self.connection is not None - info = self.connection.camera_info_static + info = camera_info_message(self.connection.camera_info_static) while not self._stop_event.is_set(): self.camera_info.publish(info) self._stop_event.wait(1.0) - def _publish_tf(self, msg: PoseStamped) -> None: - self.odom.publish(msg) + def _publish_tf(self, msg: DriverPose) -> None: + p, q = msg.position, msg.orientation + from_pose = PoseStamped( + Header(time_from_seconds(msg.ts), msg.frame_id), + Pose(Point(p.x, p.y, p.z), CdrQuaternion(q.x, q.y, q.z, q.w)), + ) + self.odom.publish(from_pose) base_link = Transform.from_pose("base_link", msg) @@ -128,11 +154,18 @@ def _publish_tf(self, msg: PoseStamped) -> None: ts=time.time(), ) - self.tf.publish(TFMessage(base_link, camera_link, camera_optical, map_to_world)) + self.tf.publish( + TFMessage( + [ + transform_message(value) + for value in (base_link, camera_link, camera_optical, map_to_world) + ] + ) + ) def _publish_sim_odom(self, msg: SimOdometry) -> None: self._publish_tf( - PoseStamped( + DriverPose( ts=msg.ts, frame_id=msg.frame_id, position=msg.position, @@ -141,9 +174,9 @@ def _publish_sim_odom(self, msg: SimOdometry) -> None: ) @rpc - def move(self, twist: Twist, duration: float = 0.0) -> None: + def move(self, twist: Twist | CdrTwist, duration: float = 0.0) -> None: assert self.connection is not None - self.connection.move(twist, duration) + self.connection.move(_driver_twist(twist), duration) @rpc def publish_request(self, topic: str, data: dict[str, Any]) -> dict[Any, Any]: diff --git a/dimos/robot/unitree/g1/test_cdr_boundaries.py b/dimos/robot/unitree/g1/test_cdr_boundaries.py new file mode 100644 index 00000000000..b8b7e1ad63d --- /dev/null +++ b/dimos/robot/unitree/g1/test_cdr_boundaries.py @@ -0,0 +1,186 @@ +# 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. + +"""Real G1 message ports with simulated inputs and mocked external SDKs.""" + +import asyncio +from contextlib import ExitStack +from copy import deepcopy +from functools import partial +from threading import Condition, Event + +import numpy as np +import open3d as o3d +import pytest +from reactivex import Subject + +from dimos.core.module import Module +from dimos.core.transport import LCMTransport, ZenohTransport +from dimos.message_utils.image import image_view +from dimos.message_utils.pointcloud import pointcloud_rgb, pointcloud_xyz +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Twist, Vector3 +from dimos.msgs.driver import transform_message +from dimos.msgs.sensor_msgs.Image import Image as DriverImage, ImageFormat +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 as DriverCloud +from dimos.robot.unitree.g1.connection import G1Connection +from dimos.robot.unitree.g1.g1_tf_publisher import G1TfPublisher, mount_transforms +from dimos.robot.unitree.g1.mujoco_sim import G1SimConnection +from dimos.robot.unitree.mujoco_connection import MujocoConnection +from dimos.robot.unitree.type.odometry import Odometry + + +def _options(backend): + return ( + {"url": "memq://"} + if backend == "lcm" + else { + "scouting": False, + "multicast": False, + "gossip": False, + "connect": [], + "listen": ["tcp/127.0.0.1:0"], + } + ) + + +@pytest.mark.parametrize("backend", ["lcm", "zenoh"]) +def test_simulated_sensor_and_drive_ports_use_cdr_without_launching_simulator(mocker, backend): + driver = mocker.Mock() + driver.camera_info_static = MujocoConnection.camera_info_static + factory = mocker.patch( + "dimos.robot.unitree.g1.mujoco_sim.MujocoConnection", return_value=driver + ) + odom, clouds, images = Subject(), Subject(), Subject() + driver.odom_stream.return_value = odom + driver.lidar_stream.return_value = clouds + driver.video_stream.return_value = images + received = {} + condition = Condition() + moved = Event() + driver.move.side_effect = lambda value, duration: moved.set() + + def receive(name, value): + with condition: + received[name] = value + condition.notify_all() + + with ExitStack() as cleanup: + module = G1SimConnection() + cleanup.callback(module.stop) + for name, port in {**module.inputs, **module.outputs}.items(): + wire = ( + LCMTransport("g1_sim_" + name, port.type, **_options(backend)) + if backend == "lcm" + else ZenohTransport("g1_sim_" + name, port.type, **_options(backend)) + ) + cleanup.callback(wire.stop) + module.set_transport(name, wire) + if name in module.outputs: + cleanup.callback(wire.subscribe(partial(receive, name))) + module.start() + pose = Odometry( + ts=0.0, frame_id="world", position=[1.0, 2.0, 3.0], orientation=[0, 0, 0, 1] + ) + odom.on_next(pose) + points = np.array([[1, 2, 3], [4, 5, 6]], dtype=np.float32) + colors = np.array([[1, 0, 0], [0, 1, 0]], dtype=np.float64) + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(points) + pcd.colors = o3d.utility.Vector3dVector(colors) + source_cloud = DriverCloud(pcd, ts=0.0) + clouds.on_next(source_cloud) + pixels = np.full((2, 3, 3), [255, 0, 0], dtype=np.uint8) + images.on_next( + DriverImage(data=pixels, format=ImageFormat.RGB, ts=0.0, frame_id="camera_optical") + ) + with condition: + assert condition.wait_for(lambda: len(received) == 5, timeout=5) + assert received["odom"].header.stamp == Time(0, 0) + assert received["odom"].pose.position.x == 1 + assert [(v.header.frame_id, v.child_frame_id) for v in received["tf"].transforms] == [ + ("world", "base_link"), + ("base_link", "camera_link"), + ("camera_link", "camera_optical"), + ("map", "world"), + ] + assert received["tf"].transforms[0].header.stamp == Time(0, 0) + np.testing.assert_array_equal(pointcloud_xyz(received["lidar"]), points) + np.testing.assert_array_equal(pointcloud_rgb(received["lidar"]), [[255, 0, 0], [0, 255, 0]]) + np.testing.assert_array_equal(image_view(received["color_image"]), pixels) + assert received["color_image"].header.stamp == Time(0, 0) + assert received["camera_info"].width == driver.camera_info_static.width + command = Twist(Vector3(0.3, 0, 0), Vector3(0, 0, 0.5)) + module.cmd_vel.transport.publish(command) + assert moved.wait(5) + value, duration = driver.move.call_args.args + assert value.linear.x == 0.3 and value.angular.z == 0.5 and duration == 0 + assert command.linear.x == 0.3 + factory.assert_called_once() + driver.stop.assert_called_once() + + +@pytest.mark.parametrize("backend", ["lcm", "zenoh"]) +def test_hardware_module_command_decodes_cdr_before_mocked_sdk(mocker, backend): + driver = mocker.Mock() + factory = mocker.patch( + "dimos.robot.unitree.g1.connection.UnitreeWebRTCConnection", return_value=driver + ) + ready = Event() + driver.move.side_effect = lambda value, duration: ready.set() + with ExitStack() as cleanup: + module = G1Connection(connection_type="webrtc") + cleanup.callback(Module.stop, module) + # stop() requires a constructed driver, so register after successful start. + wire = ( + LCMTransport("g1_hardware_cmd", Twist, **_options(backend)) + if backend == "lcm" + else ZenohTransport("g1_hardware_cmd", Twist, **_options(backend)) + ) + cleanup.callback(wire.stop) + module.set_transport("cmd_vel", wire) + module.start() + cleanup.callback(module.stop) + source = Twist(Vector3(0.2, 0, 0), Vector3(0, 0, -0.4)) + wire.publish(source) + assert ready.wait(5) + value, duration = driver.move.call_args.args + assert value.linear.x == 0.2 and value.angular.z == -0.4 and duration == 0 + factory.assert_called_once() + driver.stop.assert_called_once() + + +@pytest.mark.asyncio +async def test_mount_publisher_emits_canonical_transforms_with_one_exact_epoch_stamp(mocker): + mocker.patch("dimos.message_utils.time.time.time_ns", return_value=1_789_654_321_000_000_001) + with ExitStack() as cleanup: + publisher = G1TfPublisher() + cleanup.callback(publisher.stop) + wire = LCMTransport("g1_mounts", publisher.tf.type, url="memq://") + cleanup.callback(wire.stop) + publisher.set_transport("tf", wire) + received = [] + + def receive(value): + received.append(value) + publisher._stop_event.set() + + cleanup.callback(wire.subscribe(receive)) + await asyncio.wait_for(publisher._publish_loop(), timeout=2) + assert len(received) == 1 + expected = [transform_message(v, Time(1_789_654_321, 1)) for v in mount_transforms()] + assert received[0].transforms == expected + snapshot = deepcopy(received[0]) + received[0].transforms[0].header.frame_id = "modified" + assert received[0].transforms[1].header == snapshot.transforms[1].header diff --git a/dimos/robot/unitree/g1/test_g1_tf_publisher.py b/dimos/robot/unitree/g1/test_g1_tf_publisher.py index 222e26d1b51..78e3f250e7f 100644 --- a/dimos/robot/unitree/g1/test_g1_tf_publisher.py +++ b/dimos/robot/unitree/g1/test_g1_tf_publisher.py @@ -16,6 +16,9 @@ import math +from dimos.message_utils.geometry import inverse_transform, quaternion_euler, rotate_vector +from dimos.messages.geometry_msgs.msg import Vector3 as CdrVector3 +from dimos.msgs.driver import transform_message from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.protocol.tf.tf import MultiTBuffer @@ -43,9 +46,14 @@ def _buffer( live: Transform | None = None, ) -> MultiTBuffer: buffer = MultiTBuffer() - buffer.receive_transform(*mount_transforms(waist_yaw, waist_roll, waist_pitch)) + buffer.receive_transform( + *( + transform_message(value) + for value in mount_transforms(waist_yaw, waist_roll, waist_pitch) + ) + ) if live is not None: - buffer.receive_transform(live) + buffer.receive_transform(transform_message(live)) return buffer @@ -53,16 +61,16 @@ def test_rest_pose_offsets_match_urdf() -> None: """The lidar sits MOUNT_Z above base_link, the offset every ground projection uses.""" leg = _buffer().get("mid360_link", "base_link") assert leg is not None - base_to_sensor = -leg - assert abs(base_to_sensor.translation.z - MOUNT_Z) < 1e-6 - assert abs(base_to_sensor.translation.x - MOUNT_X) < 1e-6 + base_to_sensor = inverse_transform(leg) + assert abs(base_to_sensor.transform.translation.z - MOUNT_Z) < 1e-6 + assert abs(base_to_sensor.transform.translation.x - MOUNT_X) < 1e-6 def test_mid360_frame_is_upside_down() -> None: """The inverted mount points the sensor's z axis at the floor in the base frame.""" leg = _buffer().get("base_link", "mid360_link") assert leg is not None - z_axis = leg.rotation.rotate_vector(Vector3(0.0, 0.0, 1.0)) + z_axis = rotate_vector(leg.transform.rotation, CdrVector3(0.0, 0.0, 1.0)) assert z_axis.z < -0.99 @@ -76,11 +84,11 @@ def test_flipped_level_sensor_yields_level_base() -> None: ) base = _buffer(live=live).get("world", "base_link") assert base is not None - euler = base.rotation.euler - assert abs(euler.x) < 1e-6 - assert abs(euler.y) < 1e-6 - assert abs(euler.z) < 1e-6 - assert math.isclose(base.translation.z, LIDAR_HEIGHT - MOUNT_Z, abs_tol=1e-6) + euler = quaternion_euler(base.transform.rotation) + assert abs(euler[0]) < 1e-6 + assert abs(euler[1]) < 1e-6 + assert abs(euler[2]) < 1e-6 + assert math.isclose(base.transform.translation.z, LIDAR_HEIGHT - MOUNT_Z, abs_tol=1e-6) def test_waist_yaw_rotates_base_link_against_the_torso() -> None: @@ -88,14 +96,14 @@ def test_waist_yaw_rotates_base_link_against_the_torso() -> None: yaw = math.pi / 4 leg = _buffer(waist_yaw=yaw).get("torso_link", "base_link") assert leg is not None - assert abs(leg.rotation.euler.z - (-yaw)) < 1e-6 + assert abs(quaternion_euler(leg.transform.rotation)[2] - (-yaw)) < 1e-6 def test_waist_pitch_rotates_base_link_against_the_torso() -> None: pitch = 0.3 leg = _buffer(waist_pitch=pitch).get("torso_link", "base_link") assert leg is not None - assert abs(leg.rotation.euler.y - (-pitch)) < 1e-6 + assert abs(quaternion_euler(leg.transform.rotation)[1] - (-pitch)) < 1e-6 def test_rest_pose_base_to_torso_matches_urdf_offsets() -> None: @@ -111,9 +119,9 @@ def test_d435_hangs_off_base_link() -> None: """The tree is rooted at mid360_link, so the camera edge is reachable by composition.""" camera = _buffer().get("base_link", "d435_link") assert camera is not None - assert abs(camera.translation.x - (PELVIS_TORSO_X + 0.0576235)) < 1e-6 - assert abs(camera.translation.z - (PELVIS_TORSO_Z + 0.42987)) < 1e-6 - assert abs(camera.rotation.euler.y - D435_PITCH) < 1e-6 + assert abs(camera.transform.translation.x - (PELVIS_TORSO_X + 0.0576235)) < 1e-6 + assert abs(camera.transform.translation.z - (PELVIS_TORSO_Z + 0.42987)) < 1e-6 + assert abs(quaternion_euler(camera.transform.rotation)[1] - D435_PITCH) < 1e-6 def test_pelvis_height_matches_config_note() -> None: diff --git a/dimos/robot/unitree/g1/test_wholebody_connection.py b/dimos/robot/unitree/g1/test_wholebody_connection.py index 35c41badabc..78be70c98c3 100644 --- a/dimos/robot/unitree/g1/test_wholebody_connection.py +++ b/dimos/robot/unitree/g1/test_wholebody_connection.py @@ -17,10 +17,12 @@ from collections.abc import Iterator from types import SimpleNamespace +import numpy as np from pydantic import ValidationError import pytest -from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray +from dimos.message_utils.time import header_now +from dimos.messages.dimos_msgs.msg import MotorCommandArray from dimos.robot.unitree.g1.wholebody_connection import ( _NUM_MOTOR_SLOTS, _NUM_MOTORS, @@ -69,11 +71,12 @@ def _wire(connection, soft_start_seconds): def _command(): return MotorCommandArray( - q=[1.0] * _NUM_MOTORS, - dq=[0.0] * _NUM_MOTORS, - kp=[100.0] * _NUM_MOTORS, - kd=[5.0] * _NUM_MOTORS, - tau=[8.0] * _NUM_MOTORS, + header=header_now(), + q=np.full(_NUM_MOTORS, 1.0, dtype=np.float64), + dq=np.full(_NUM_MOTORS, 0.0, dtype=np.float64), + kp=np.full(_NUM_MOTORS, 100.0, dtype=np.float64), + kd=np.full(_NUM_MOTORS, 5.0, dtype=np.float64), + tau=np.full(_NUM_MOTORS, 8.0, dtype=np.float64), ) @@ -118,7 +121,9 @@ def test_soft_start_disabled_passes_through(connection: G1WholeBodyConnection): def test_wrong_joint_count_is_dropped(connection: G1WholeBodyConnection): publisher = _wire(connection, soft_start_seconds=0.0) - connection._on_motor_command(MotorCommandArray(q=[0.0] * 5)) + command = _command() + command.q = np.zeros(5, dtype=np.float64) + connection._on_motor_command(command) assert publisher.frames == [] diff --git a/dimos/robot/unitree/g1/wholebody_connection.py b/dimos/robot/unitree/g1/wholebody_connection.py index 2f185d62e8b..f3bbbc69013 100644 --- a/dimos/robot/unitree/g1/wholebody_connection.py +++ b/dimos/robot/unitree/g1/wholebody_connection.py @@ -21,12 +21,14 @@ from __future__ import annotations +from copy import deepcopy from dataclasses import dataclass import threading from threading import Thread import time from typing import TYPE_CHECKING, Any +import numpy as np from pydantic import Field from reactivex.disposable import Disposable @@ -41,11 +43,12 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.hardware.whole_body.spec import POS_STOP, VEL_STOP -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray +from dimos.message_utils.time import time_from_nanoseconds, time_from_seconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.dimos_msgs.msg import MotorCommandArray +from dimos.messages.geometry_msgs.msg import Quaternion, Vector3 +from dimos.messages.sensor_msgs.msg import Imu, JointState +from dimos.messages.std_msgs.msg import Header from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -71,14 +74,17 @@ def _imu_from_unitree_wxyz( *, frame_id: str, ts: float, + stamp: Time | None = None, ) -> Imu: _w, x, y, z = quaternion return Imu( orientation=Quaternion(x, y, z, _w), angular_velocity=Vector3(*gyroscope), linear_acceleration=Vector3(*accelerometer), - frame_id=frame_id, - ts=ts, + header=Header(deepcopy(stamp) if stamp is not None else time_from_seconds(ts), frame_id), + orientation_covariance=np.zeros(9, dtype=np.float64), + angular_velocity_covariance=np.zeros(9, dtype=np.float64), + linear_acceleration_covariance=np.zeros(9, dtype=np.float64), ) @@ -170,7 +176,9 @@ def start(self) -> None: ChannelFactoryInitialize(0) except Exception as e: # Idempotent - already initialised by a sibling participant is fine. - logger.debug(f"ChannelFactoryInitialize raised (likely already init'd): {e}") + logger.debug( + f"ChannelFactoryInitialize raised (likely already init'd): {e}", exc_info=True + ) self._publisher = ChannelPublisher("rt/lowcmd", LowCmd_) self._publisher.Init() @@ -337,16 +345,17 @@ def _snapshot_motor_imu(self) -> G1LowStateSnapshot | None: ) def _publish_motor_state_and_imu( - self, now: float, frame_id: str, sample: G1LowStateSnapshot + self, now: float, frame_id: str, sample: G1LowStateSnapshot, stamp: Time | None = None ) -> None: self.motor_states.publish( JointState( - ts=now, - frame_id=frame_id, - name=G1_JOINT_NAMES, - position=sample.positions, - velocity=sample.velocities, - effort=sample.efforts, + header=Header( + deepcopy(stamp) if stamp is not None else time_from_seconds(now), frame_id + ), + name=list(G1_JOINT_NAMES), + position=np.asarray(sample.positions, dtype=np.float64), + velocity=np.asarray(sample.velocities, dtype=np.float64), + effort=np.asarray(sample.efforts, dtype=np.float64), ) ) # Unitree reports quaternions as (w,x,y,z); Imu/Quaternion stores (x,y,z,w). @@ -357,6 +366,7 @@ def _publish_motor_state_and_imu( sample.accelerometer, frame_id=frame_id, ts=now, + stamp=stamp, ) ) @@ -369,7 +379,12 @@ def _publish_loop(self) -> None: self._drain_low_state() sample = self._snapshot_motor_imu() if sample is not None: - self._publish_motor_state_and_imu(now=time.time(), frame_id=frame_id, sample=sample) + self._publish_motor_state_and_imu( + now=time.time(), + frame_id=frame_id, + sample=sample, + stamp=time_from_nanoseconds(time.time_ns()), + ) next_tick += period sleep_for = next_tick - time.perf_counter() @@ -396,8 +411,10 @@ def _soft_start_scale(self, now: float) -> float: return scale def _on_motor_command(self, msg: MotorCommandArray) -> None: - if msg.num_joints != _NUM_MOTORS: - logger.warning(f"Expected {_NUM_MOTORS} motor commands, got {msg.num_joints}; ignoring") + if any(len(values) != _NUM_MOTORS for values in (msg.q, msg.dq, msg.kp, msg.kd, msg.tau)): + logger.warning( + "Expected %d complete motor commands; ignoring inconsistent frame", _NUM_MOTORS + ) return with self._lock: diff --git a/dimos/robot/unitree/go2/blueprints/basic/test_unitree_go2_basic.py b/dimos/robot/unitree/go2/blueprints/basic/test_unitree_go2_basic.py new file mode 100644 index 00000000000..73ddeb74112 --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/basic/test_unitree_go2_basic.py @@ -0,0 +1,61 @@ +# 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. + +"""Go2 blueprint rendering consumes the same generated values as live ports.""" + +import numpy as np + +from dimos.message_utils.occupancy import occupancy_from_array +from dimos.message_utils.pointcloud import pointcloud_from_xyz +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, PoseArray, PoseStamped, Quaternion +from dimos.messages.std_msgs.msg import Header +from dimos.robot.unitree.go2.blueprints.basic import unitree_go2_basic as view +from dimos.robot.unitree.go2.connection import GO2Connection + + +def test_camera_and_pose_overrides_accept_generated_port_values(): + camera = view._convert_camera_info(GO2Connection.camera_info_static) + assert camera[0][0] == "/world/color_image" + assert camera[0][1].resolution.as_arrow_array().to_pylist() == [[1280.0, 720.0]] + pose = Pose(Point(1.0, 2.0, 3.0), Quaternion(0.0, 0.0, 0.0, 1.0)) + entries = dict(view._plot_odom(PoseStamped(Header(Time(0, 0), "world"), pose))) + assert "world/odom/message" in entries + assert "world/odom" in entries + assert entries["plots/odom/x"].scalars.as_arrow_array().to_pylist() == [1.0] + assert entries["plots/odom/y"].scalars.as_arrow_array().to_pylist() == [2.0] + + +def test_map_and_pgo_overrides_preserve_rendered_geometry(): + header = Header(Time(0, 0), "world") + cloud = pointcloud_from_xyz(np.array([[1.0, 2.0, 0.2], [9.0, 8.0, -0.1]]), header=header) + points = view._convert_global_map(cloud).positions.as_arrow_array().to_pylist() + np.testing.assert_allclose(points, [[1.0, 2.0, 0.2]]) + pose = Pose(Point(1.0, 2.0, 3.0), Quaternion(0.0, 0.0, 0.0, 1.0)) + keyframes = dict(view._convert_pgo_keyframes(PoseArray(header, [pose]))) + assert keyframes["world/pgo/keyframes"].positions.as_arrow_array().to_pylist() == [ + [1.0, 2.0, 3.0] + ] + edges = LineSegments3D(header, [LineSegment3D(Point(1.0, 2.0, 3.0), Point(4.0, 5.0, 6.0), 0.9)]) + loops = dict(view._convert_pgo_loops(edges)) + assert loops["world/pgo/loops"].strips.as_arrow_array().to_pylist() == [ + [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]] + ] + grid = occupancy_from_array(np.array([[0, 100]], dtype=np.int8), header=header, resolution=0.5) + mesh = view._convert_navigation_costmap(grid) + np.testing.assert_allclose( + mesh.vertex_positions.as_arrow_array().to_pylist(), + [[0.0, 0.0, 0.015], [1.0, 0.0, 0.015], [1.0, 0.5, 0.015], [0.0, 0.5, 0.015]], + ) diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py index 2a0cbe8830c..9dbc1f2bbdb 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_basic.py @@ -23,7 +23,11 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config +from dimos.message_utils.occupancy import occupancy_view +from dimos.msgs.geometry_msgs.Pose import Pose as NumericalPose +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid as NumericalGrid from dimos.robot.unitree.go2.connection import GO2Connection +from dimos.visualization.rerun.cdr import camera_pinhole, cloud_archetype, generated_archetypes from dimos.visualization.vis_module import vis_module if TYPE_CHECKING: @@ -32,30 +36,32 @@ from rerun._baseclasses import Archetype from rerun.blueprint import Blueprint - from dimos.msgs.geometry_msgs.PoseArray import PoseArray - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - from dimos.msgs.geometry_msgs.Twist import Twist - from dimos.msgs.nav_msgs.LineSegments3D import LineSegments3D - from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid - from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 - from dimos.msgs.std_msgs.Float32 import Float32 + from dimos.messages.dimos_msgs.msg import LineSegments3D + from dimos.messages.geometry_msgs.msg import PoseArray, PoseStamped, Twist + from dimos.messages.nav_msgs.msg import OccupancyGrid + from dimos.messages.sensor_msgs.msg import CameraInfo, PointCloud2 + from dimos.messages.std_msgs.msg import Float32 from dimos.visualization.rerun.bridge import RerunData, RerunMulti def _convert_camera_info(camera_info: CameraInfo) -> RerunData: - return camera_info.to_rerun( - image_topic="/world/color_image", - optical_frame="camera_optical", - ) + return [("/world/color_image", camera_pinhole(camera_info))] def _convert_global_map(grid: PointCloud2) -> Archetype: - return grid.to_rerun(bottom_cutoff=0) + return cloud_archetype(grid, bottom_cutoff=0) def _convert_navigation_costmap(grid: OccupancyGrid) -> Archetype: - return grid.to_rerun( + # Reuse the existing numerical texture renderer behind an explicit value adapter. + p, q = grid.info.origin.position, grid.info.origin.orientation + numerical = NumericalGrid( + grid=occupancy_view(grid), + resolution=grid.info.resolution, + origin=NumericalPose(p.x, p.y, p.z, q.x, q.y, q.z, q.w), + frame_id=grid.header.frame_id, + ) + return numerical.to_rerun( colormap="Accent", z_offset=0.015, opacity=0.2, @@ -66,7 +72,10 @@ def _convert_navigation_costmap(grid: OccupancyGrid) -> Archetype: def _convert_pgo_keyframes(keyframes: PoseArray) -> RerunMulti: import rerun as rr - positions = keyframes.positions() + positions = np.asarray( + [[pose.position.x, pose.position.y, pose.position.z] for pose in keyframes.poses], + dtype=np.float32, + ).reshape(-1, 3) return [ ("world/pgo/keyframes", rr.Points3D(positions, colors=[[255, 0, 0]], radii=[0.025])), ("world/pgo/path", rr.LineStrips3D([positions], colors=[[255, 255, 255]], radii=[0.01])), @@ -76,7 +85,13 @@ def _convert_pgo_keyframes(keyframes: PoseArray) -> RerunMulti: def _convert_pgo_loops(edges: LineSegments3D) -> RerunMulti: import rerun as rr - segments = edges.segments.astype(np.float32) + segments = np.asarray( + [ + [[edge.start.x, edge.start.y, edge.start.z], [edge.end.x, edge.end.y, edge.end.z]] + for edge in edges.segments + ], + dtype=np.float32, + ).reshape(-1, 2, 3) strips = rr.LineStrips3D(segments, colors=[[231, 76, 60]], radii=[0.008]) return [("world/pgo/loops", strips)] @@ -90,10 +105,12 @@ def _plot_telemetry(name: str, msg: Float32) -> RerunMulti: def _plot_odom(odom: PoseStamped) -> RerunMulti: import rerun as rr + entries = generated_archetypes(odom, "world/odom") + assert entries is not None return [ - ("world/odom", odom.to_rerun()), - ("plots/odom/x", rr.Scalars(odom.x)), - ("plots/odom/y", rr.Scalars(odom.y)), + *entries, + ("plots/odom/x", rr.Scalars(odom.pose.position.x)), + ("plots/odom/y", rr.Scalars(odom.pose.position.y)), ] diff --git a/dimos/robot/unitree/go2/connection.py b/dimos/robot/unitree/go2/connection.py index a9e92feb8c4..c38061b20ba 100644 --- a/dimos/robot/unitree/go2/connection.py +++ b/dimos/robot/unitree/go2/connection.py @@ -34,6 +34,29 @@ from dimos.core.stream import In, Out from dimos.memory.replay import Replay, ReplayStream, resolve_db_path from dimos.memory.store.sqlite import SqliteStore +from dimos.message_utils.time import time_from_seconds +from dimos.messages.geometry_msgs.msg import ( + Point as CdrPoint, + Pose as CdrPose, + PoseStamped as CdrPoseStamped, + Quaternion as CdrQuaternion, + Transform as CdrTransform, + TransformStamped as CdrTransformStamped, + Twist as CdrTwist, + Vector3 as CdrVector3, +) +from dimos.messages.sensor_msgs.msg import ( + CameraInfo as CdrCameraInfo, + Image as CdrImage, + PointCloud2 as CdrCloud, +) +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage +from dimos.msgs.driver import ( + camera_info_message as _camera_info_message, + cloud_message as _cloud_message, + image_message, +) from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform @@ -42,7 +65,6 @@ from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.robot.unitree.config import connection_type from dimos.robot.unitree.connection import UnitreeWebRTCConnection from dimos.robot.unitree.type.lowstate import LowStateMsg @@ -309,7 +331,7 @@ def _request_shutdown(self) -> None: finally: client.stop() except Exception: - logger.error("Failed to request coordinator shutdown", exc_info=True) + logger.exception("Failed to request coordinator shutdown") @simple_mcache def lidar_stream(self) -> Observable[PointCloud2]: @@ -342,23 +364,20 @@ def publish_request(self, topic: str, data: dict): # type: ignore[no-untyped-de return {"status": "ok", "message": "Fake publish"} -_Config = TypeVar("_Config", bound=ConnectionConfig, default=ConnectionConfig) - - class GO2Connection(Module, Camera, Pointcloud): dedicated_worker = True config: ConnectionConfig - cmd_vel: In[Twist] - pointcloud: Out[PointCloud2] - odom: Out[PoseStamped] - lidar: Out[PointCloud2] - color_image: Out[Image] - camera_info: Out[CameraInfo] + cmd_vel: In[CdrTwist] + pointcloud: Out[CdrCloud] + odom: Out[CdrPoseStamped] + lidar: Out[CdrCloud] + color_image: Out[CdrImage] + camera_info: Out[CdrCameraInfo] tf: Out[TFMessage] connection: Go2ConnectionProtocol - camera_info_static: CameraInfo = _camera_info_static() + camera_info_static: CdrCameraInfo = _camera_info_message(_camera_info_static()) _camera_info_thread: Thread | None = None _latest_lowstate: LowStateMsg | None = None @@ -384,15 +403,20 @@ def __init__(self, **kwargs: Any) -> None: ) if hasattr(self.connection, "camera_info_static"): - self.camera_info_static = self.connection.camera_info_static + self.camera_info_static = _camera_info_message(self.connection.camera_info_static) - if self.config.frame_id_prefix and self.camera_info_static.frame_id: + if self.config.frame_id_prefix and self.camera_info_static.header.frame_id: # Copy so the class-level default is not mutated. - self.camera_info_static = copy.copy(self.camera_info_static) - self.camera_info_static.frame_id = _prefixed( - self.config.frame_id_prefix, self.camera_info_static.frame_id + self.camera_info_static = copy.deepcopy(self.camera_info_static) + self.camera_info_static.header.frame_id = _prefixed( + self.config.frame_id_prefix, self.camera_info_static.header.frame_id ) + def _publish_image(self, image: Image) -> None: + message = image_message(image) + message.header.frame_id = _prefixed(self.config.frame_id_prefix, message.header.frame_id) + self.color_image.publish(message) + @rpc def start(self) -> None: super().start() @@ -400,18 +424,18 @@ def start(self) -> None: return self.connection.start() - def onimage(image: Image) -> None: - image.frame_id = _prefixed(self.config.frame_id_prefix, image.frame_id) - self.color_image.publish(image) - if self.config.lidar: - self.register_disposable(self.connection.lidar_stream().subscribe(self.lidar.publish)) + self.register_disposable( + self.connection.lidar_stream() + .pipe(ops.map(_cloud_message)) + .subscribe(self.lidar.publish) + ) self.register_disposable(self.connection.odom_stream().subscribe(self._publish_tf)) self.register_disposable(self.connection.lowstate_stream().subscribe(self._on_lowstate)) self.register_disposable(Disposable(self.cmd_vel.subscribe(self.move))) if self.config.camera: - self.register_disposable(self.connection.video_stream().subscribe(onimage)) + self.register_disposable(self.connection.video_stream().subscribe(self._publish_image)) self._camera_info_thread = Thread( target=self.publish_camera_info, daemon=True, @@ -443,7 +467,9 @@ def stop(self) -> None: try: self.liedown() except Exception: - logger.warning("liedown on stop failed (link already down?) — continuing teardown") + logger.warning( + "liedown on stop failed (link already down?) — continuing teardown", exc_info=True + ) if self.connection: try: @@ -486,18 +512,44 @@ def _publish_tf(self, msg: PoseStamped) -> None: msg.frame_id = self.config.odom_frame_id if self.config.publish_tf: transforms = self._odom_to_tf(msg, prefix=self.config.frame_id_prefix or "") - self.tf.publish(TFMessage(*transforms)) + self.tf.publish( + TFMessage( + [ + CdrTransformStamped( + Header(time_from_seconds(transform.ts), transform.frame_id), + transform.child_frame_id, + CdrTransform( + CdrVector3(*transform.translation), + CdrQuaternion(*transform.rotation), + ), + ) + for transform in transforms + ] + ) + ) if self.odom.transport: - self.odom.publish(msg) + self.odom.publish( + CdrPoseStamped( + Header(time_from_seconds(msg.ts), msg.frame_id), + CdrPose(CdrPoint(*msg.position), CdrQuaternion(*msg.orientation)), + ) + ) def publish_camera_info(self) -> None: while True: - self.camera_info.publish(self.camera_info_static.with_ts(time.time())) + info = copy.deepcopy(self.camera_info_static) + info.header.stamp = time_from_seconds(time.time()) + self.camera_info.publish(info) time.sleep(1.0) @rpc - def move(self, twist: Twist, duration: float = 0.0) -> bool: + def move(self, twist: CdrTwist | Twist, duration: float = 0.0) -> bool: """Send movement command to robot.""" + if isinstance(twist, CdrTwist): + twist = Twist( + linear=Vector3(twist.linear.x, twist.linear.y, twist.linear.z), + angular=Vector3(twist.angular.x, twist.angular.y, twist.angular.z), + ) return self.connection.move(twist, duration) @rpc diff --git a/dimos/robot/unitree/go2/test_connection.py b/dimos/robot/unitree/go2/test_connection.py index 4707cdde7cb..7ec3c32874d 100644 --- a/dimos/robot/unitree/go2/test_connection.py +++ b/dimos/robot/unitree/go2/test_connection.py @@ -19,20 +19,43 @@ """ from collections.abc import Callable, Iterator +from contextlib import ExitStack +from functools import partial import json from pathlib import Path import threading import time from unittest.mock import MagicMock +from dimos_cdrgen.registry import decode, encode +import numpy as np +import open3d as o3d import pytest from pytest_mock import MockerFixture from reactivex import Subject +from dimos.core import transport as transport_module from dimos.core.coordination.blueprint_config.parser import BlueprintConfigParser from dimos.core.global_config import GlobalConfig +from dimos.core.transport_factory import make_transport +from dimos.message_utils.image import image_view +from dimos.message_utils.pointcloud import pointcloud_view +from dimos.message_utils.time import to_seconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import ( + PoseStamped as CdrPoseStamped, + Twist as CdrTwist, + Vector3, +) +from dimos.messages.sensor_msgs.msg import PointCloud2 as CdrCloud +from dimos.messages.std_msgs.msg import Header +from dimos.messages.tf2_msgs.msg import TFMessage from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo +from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo as DriverCameraInfo +from dimos.msgs.sensor_msgs.Image import Image as DriverImage, ImageFormat +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.protocol.pubsub.impl.lcmpubsub import LCM +from dimos.protocol.rpc.pubsubrpc import LCMRPC from dimos.robot.unitree.go2 import connection as go2_conn from dimos.robot.unitree.go2.connection import ConnectionConfig, GO2Connection from dimos.simulation.mujoco.config import MujocoConfig @@ -43,6 +66,7 @@ def stub_webrtc(monkeypatch: pytest.MonkeyPatch) -> MagicMock: """Replace UnitreeWebRTCConnection in go2.connection so the webrtc branch runs without dialing out.""" stub = MagicMock(name="UnitreeWebRTCConnection") + stub.return_value.camera_info_static = DriverCameraInfo(frame_id="camera_optical", ts=0.0) monkeypatch.setattr(go2_conn, "UnitreeWebRTCConnection", stub) return stub @@ -174,7 +198,9 @@ def test_odom_to_tf_unprefixed_by_default() -> None: @pytest.fixture -def connection(stub_webrtc: MagicMock) -> Iterator[Callable[[bool], GO2Connection]]: +def connection( + stub_webrtc: MagicMock, mocker: MockerFixture +) -> Iterator[Callable[[bool], GO2Connection]]: """Build GO2Connections with the tf and odom ports stubbed, and stop them after.""" built: list[GO2Connection] = [] @@ -184,8 +210,8 @@ def build(publish_tf: bool) -> GO2Connection: publish_tf=publish_tf, odom_frame_id="go2_odom", ) - conn.tf = MagicMock() - conn.odom = MagicMock() + mocker.patch.object(conn, "tf") + mocker.patch.object(conn, "odom") built.append(conn) return conn @@ -220,8 +246,10 @@ def test_camera_info_is_restamped_on_each_publish( ) -> None: """A frozen stamp collapses a recording's camera_info onto one instant before the run.""" conn = connection(publish_tf=True) - conn.camera_info = MagicMock() - conn.camera_info_static = CameraInfo(frame_id="camera_optical", ts=1.0) + mocker.patch.object(conn, "camera_info") + conn.camera_info_static = go2_conn._camera_info_message( + DriverCameraInfo(ts=1.0, frame_id="camera_optical") + ) def sleep(_seconds: float) -> None: if conn.camera_info.publish.call_count >= 2: @@ -233,9 +261,11 @@ def sleep(_seconds: float) -> None: conn.publish_camera_info() published = [call.args[0] for call in conn.camera_info.publish.call_args_list] - assert [info.ts >= before for info in published] == [True, True] - assert [info.frame_id for info in published] == [conn.camera_info_static.frame_id] * 2 - assert conn.camera_info_static.ts == 1.0 + assert [to_seconds(info.header.stamp) >= before for info in published] == [True, True] + assert [info.header.frame_id for info in published] == [ + conn.camera_info_static.header.frame_id + ] * 2 + assert conn.camera_info_static.header.stamp == Time(1, 0) def test_odom_to_tf_prefixed() -> None: @@ -252,3 +282,136 @@ def test_odom_to_tf_prefixed() -> None: "robot0/camera_link", "robot0/camera_optical", ) + + +def test_driver_cloud_boundary_preserves_all_supported_attributes(): + source = PointCloud2.from_numpy( + points=np.array([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]]), + intensities=np.array([0.25, 0.75]), + offset_times=np.array([3, 9], dtype=np.uint32), + tags=np.array([1, 2], dtype=np.uint8), + lines=np.array([2, 3], dtype=np.uint8), + stamps=np.array([10.0, 10.1]), + timestamp=0.0, + frame_id="lidar", + ) + source.pointcloud_tensor.point["colors"] = o3d.core.Tensor( + np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]], dtype=np.float32) + ) + generated = go2_conn._cloud_message(source) + restored = decode(encode(generated), CdrCloud) + records = pointcloud_view(restored) + assert restored.header == Header(Time(0, 0), "lidar") + assert records["intensity"].ravel().tolist() == [0.25, 0.75] + assert records["offset_time"].ravel().tolist() == [3, 9] + assert records["tag"].ravel().tolist() == [1, 2] + assert records["line"].ravel().tolist() == [2, 3] + assert records["stamps"].ravel().tolist() == [10.0, 10.1] + assert records["rgb"].ravel().tolist() == [0xFF0000, 0x00FF00] + np.testing.assert_array_equal(records["x"].ravel(), [1.0, 4.0]) + np.testing.assert_array_equal(source.as_numpy()[0], [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]]) + + +def test_driver_odometry_boundary_publishes_generated_tf_and_pose(connection): + conn = connection(publish_tf=True) + conn._publish_tf(PoseStamped(ts=0.0, frame_id="source")) + pose = conn.odom.publish.call_args.args[0] + transforms = conn.tf.publish.call_args.args[0] + assert type(pose) is CdrPoseStamped + assert type(transforms) is TFMessage + assert pose.header == Header(Time(0, 0), "go2_odom") + assert [(t.header.frame_id, t.child_frame_id) for t in transforms.transforms] == [ + ("go2_odom", "base_link"), + ("base_link", "camera_link"), + ("camera_link", "camera_optical"), + ] + assert all(t.header.stamp == Time(0, 0) for t in transforms.transforms) + + +def test_generated_command_is_adapted_only_at_driver_boundary(connection): + conn = connection(publish_tf=False) + conn.move(CdrTwist(Vector3(1.0, 2.0, 3.0), Vector3(4.0, 5.0, 6.0)), 0.5) + command, duration = conn.connection.move.call_args.args + assert command.linear.x == 1.0 + assert command.linear.y == 2.0 + assert command.linear.z == 3.0 + assert command.angular.x == 4.0 + assert command.angular.y == 5.0 + assert command.angular.z == 6.0 + assert duration == 0.5 + + +@pytest.mark.parametrize( + "format,encoding,dtype", + [ + (ImageFormat.RGB, "rgb8", np.uint8), + (ImageFormat.DEPTH16, "16UC1", np.uint16), + ], +) +def test_driver_image_boundary_copies_pixels_and_prefixes_frame( + connection, mocker, format, encoding, dtype +): + conn = connection(publish_tf=False) + conn.config.frame_id_prefix = "robot0" + mocker.patch.object(conn, "color_image") + shape = (2, 3, 3) if format is ImageFormat.RGB else (2, 3) + pixels = np.arange(np.prod(shape), dtype=dtype).reshape(shape) + source = DriverImage(data=pixels, format=format, ts=0.0, frame_id="camera_optical") + conn._publish_image(source) + message = conn.color_image.publish.call_args.args[0] + assert message.header == Header(Time(0, 0), "robot0/camera_optical") + assert message.encoding == encoding + np.testing.assert_array_equal(image_view(message), pixels) + pixels[:] = 0 + assert image_view(message).max() > 0 + assert source.frame_id == "camera_optical" + + +class MemqRPC(LCMRPC): + def __init__(self, **kwargs): + super().__init__(url="memq://", **kwargs) + + +def test_go2_default_ports_deliver_cdr_without_robot(stub_webrtc, mocker, monkeypatch): + mocker.patch("dimos.core.module.get_loop", return_value=(None, None)) + mocker.patch.object(LCMRPC, "start") + mocker.patch.object(LCMRPC, "serve_module_rpc") + monkeypatch.setattr(transport_module, "LCM", partial(LCM, url="memq://")) + config = GlobalConfig(transport="lcm", robot_ip="127.0.0.1") + received = {} + condition = threading.Condition() + + def on_message(name, value): + with condition: + received[name] = value + condition.notify_all() + + with ExitStack() as cleanup: + conn = GO2Connection(g=config, rpc_transport=MemqRPC) + cleanup.callback(conn.stop) + for name in ("tf", "odom", "color_image", "lidar"): + port = conn.outputs[name] + transport = make_transport(name, port.type, g=config) + cleanup.callback(transport.stop) + conn.set_transport(name, transport) + cleanup.callback(transport.subscribe(partial(on_message, name))) + source = DriverImage( + data=np.arange(18, dtype=np.uint8).reshape(2, 3, 3), + format=ImageFormat.RGB, + ts=0.0, + frame_id="camera_optical", + ) + conn._publish_image(source) + conn._publish_tf(PoseStamped(ts=0.0, frame_id="world")) + cloud = PointCloud2.from_numpy(np.array([[1.0, 2.0, 3.0]]), timestamp=0.0) + conn.lidar.publish(go2_conn._cloud_message(cloud)) + with condition: + assert condition.wait_for(lambda: len(received) == 4, timeout=5) + assert type(received["odom"]) is CdrPoseStamped + assert type(received["tf"]) is TFMessage + assert type(received["lidar"]) is CdrCloud + assert received["color_image"].encoding == "rgb8" + assert received["odom"].header.stamp == Time(0, 0) + assert len(received["tf"].transforms) == 3 + np.testing.assert_array_equal(image_view(received["color_image"]), source.data) + np.testing.assert_array_equal(pointcloud_view(received["lidar"])["z"], [[3.0]]) diff --git a/dimos/robot/unitree/test_dimsim_connection.py b/dimos/robot/unitree/test_dimsim_connection.py new file mode 100644 index 00000000000..8cf4acd1166 --- /dev/null +++ b/dimos/robot/unitree/test_dimsim_connection.py @@ -0,0 +1,209 @@ +# 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. + +"""Canonical DimSim browser/Deno/Python sensor and control interoperability.""" + +from contextlib import ExitStack +from copy import deepcopy +from functools import partial +import json +import os +from pathlib import Path +import subprocess + +from dimos_cdrgen.registry import decode, encode, schema +import numpy as np +import pytest + +from dimos.core import transport as transport_module +from dimos.message_utils.image import image_view +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Twist, Vector3 +from dimos.messages.sensor_msgs.msg import Image, PointCloud2, PointField +from dimos.messages.std_msgs.msg import Header +from dimos.protocol.pubsub.impl.lcmpubsub import LCM +from dimos.robot.unitree import dimsim_connection +from dimos.robot.unitree.dimsim_connection import DimSimConnection, _odom_to_tf +from dimos.simulation.dimsim.config import DimSimConfig +from dimos.utils.testing.waiting import wait_until + +_ROOT = Path(__file__).resolve().parents[3] +_DIMSIM = _ROOT / "misc/DimSim" + + +def _samples(): + stamp = Header(Time(1_700_000_000, 1), "world") + return [ + PoseStamped(deepcopy(stamp), Pose(Point(1.0, 2.0, 3.0), Quaternion(0.0, 0.0, 0.0, 1.0))), + Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.2)), + Image(deepcopy(stamp), 2, 2, "rgb8", 0, 8, np.arange(16, dtype=np.uint8)), + PointCloud2( + deepcopy(stamp), + 1, + 1, + [ + PointField(name, index * 4, 7, 1) + for index, name in enumerate(("x", "y", "z", "intensity")) + ], + False, + 16, + 16, + np.asarray([1.0, 2.0, 3.0, 0.5], dtype=" 0.0 and pose.pose.orientation.z > 0.0 + assert (stopped.pose.position.x, stopped.pose.position.y) == ( + pose.pose.position.x, + pose.pose.position.y, + ) + assert (pose.header.stamp.nanosec, stopped.header.stamp.nanosec) == (1_000_000, 502_000_000) + assert cloud.header.frame_id == pose.header.frame_id == "world" + assert 0 < cloud.width <= 15000 and cloud.row_step == cloud.width * 16 + assert [(field.name, field.offset, field.datatype, field.count) for field in cloud.fields] == [ + (name, index * 4, 7, 1) for index, name in enumerate(("x", "y", "z", "intensity")) + ] + points = np.frombuffer(cloud.data, dtype=" None: @@ -389,20 +410,20 @@ def _camera_info_ts(self) -> float: return time.time() if ts is None else ts @rpc - def get_color_camera_info(self) -> CameraInfo | None: + def get_color_camera_info(self) -> CdrCameraInfo | None: with self._state_lock: base = self._camera_info_base if base is None: return None - return base.with_ts(self._camera_info_ts()) + return camera_info_message(base.with_ts(self._camera_info_ts())) @rpc - def get_depth_camera_info(self) -> CameraInfo | None: + def get_depth_camera_info(self) -> CdrCameraInfo | None: with self._state_lock: base = self._camera_info_base if base is None: return None - return base.with_ts(self._camera_info_ts()) + return camera_info_message(base.with_ts(self._camera_info_ts())) @rpc def get_depth_scale(self) -> float: @@ -494,13 +515,13 @@ def add_camera( # Hooks are installed via set_step_hooks() after gripper detection # below, since they depend on the resolved gripper index. - engine_kwargs: dict[str, Any] = dict( - headless=self.config.headless, - cameras=cameras, - raycast_lidars=raycast_lidars, - robot_sim_spec=self.config.robot_sim_spec, - reset_joint_positions=self.config.reset_joint_positions, - ) + engine_kwargs: dict[str, Any] = { + "headless": self.config.headless, + "cameras": cameras, + "raycast_lidars": raycast_lidars, + "robot_sim_spec": self.config.robot_sim_spec, + "reset_joint_positions": self.config.reset_joint_positions, + } if self.config.robot_mjcf is not None: engine_kwargs["config_path"] = Path(self.config.robot_mjcf) engine_kwargs["model"] = self._compose_model() @@ -687,7 +708,7 @@ def stop(self) -> None: self._engine.disconnect() self._engine = None except Exception as exc: - logger.error("engine.disconnect() failed", error=str(exc)) + logger.exception("engine.disconnect() failed", error=str(exc)) errors.append(("engine.disconnect", exc)) if self._shm is not None: try: @@ -695,7 +716,7 @@ def stop(self) -> None: self._shm.cleanup() self._shm = None except Exception as exc: - logger.error("SHM cleanup failed", error=str(exc)) + logger.exception("SHM cleanup failed", error=str(exc)) errors.append(("shm.cleanup", exc)) self._sim_hooks = None @@ -789,6 +810,7 @@ def _publish_shm_and_lcm(self, engine: MujocoEngine) -> None: if shm is None: return + stamp = time_from_nanoseconds(time.time_ns()) # Odom - when the MJCF has a free-joint root, publish base pose # every step. Without this, downstream consumers (viser viewer, # nav stack) only see joint articulation, not base translation @@ -800,16 +822,17 @@ def _publish_shm_and_lcm(self, engine: MujocoEngine) -> None: self._root_base_qpos_adr + 3 : self._root_base_qpos_adr + 7 ] # (w, x, y, z) per MuJoCo convention self.odom.publish( - PoseStamped( - ts=time.time(), - frame_id="world", - position=Vector3(float(base_pos[0]), float(base_pos[1]), float(base_pos[2])), - orientation=Quaternion( - float(base_quat[1]), - float(base_quat[2]), - float(base_quat[3]), - float(base_quat[0]), - ), # PoseStamped uses x,y,z,w + CdrPose( + Header(deepcopy(stamp), "world"), + CdrPoseValue( + CdrPoint(float(base_pos[0]), float(base_pos[1]), float(base_pos[2])), + CdrQuaternion( + float(base_quat[1]), + float(base_quat[2]), + float(base_quat[3]), + float(base_quat[0]), + ), + ), ) ) @@ -846,9 +869,11 @@ def _publish_shm_and_lcm(self, engine: MujocoEngine) -> None: shm.write_imu(quaternion=quat, gyroscope=gyro, accelerometer=accel) # Also publish on the stream port for downstream consumers. # MuJoCo reports quaternions as (w,x,y,z); Imu/Quaternion stores (x,y,z,w). - self.imu.publish( + imu = imu_message( _imu_from_mujoco_wxyz(quat, gyro, accel, frame_id="pelvis", ts=time.time()) ) + imu.header.stamp = deepcopy(stamp) + self.imu.publish(imu) if not self._shm_ready_signaled: shm.signal_ready(num_joints=len(engine.joint_names), arm_joints=self.config.dof) @@ -904,11 +929,10 @@ def _publish_loop(self) -> None: try: frame = engine.read_camera(self.config.camera_name) except RuntimeError as exc: - logger.error( + logger.exception( "MuJoCo render failed; stopping publish loop", camera_name=self.config.camera_name, error=str(exc), - exc_info=True, ) return @@ -927,7 +951,7 @@ def _publish_loop(self) -> None: frame_id=self._color_optical_frame, ts=ts, ) - self.color_image.publish(color_img) + self.color_image.publish(image_message(color_img)) if self.config.enable_depth: depth_img = Image( @@ -936,7 +960,7 @@ def _publish_loop(self) -> None: frame_id=self._color_optical_frame, ts=ts, ) - self.depth_image.publish(depth_img) + self.depth_image.publish(image_message(depth_img)) self._publish_tf(ts, frame) @@ -969,8 +993,8 @@ def _publish_camera_info(self) -> None: frame_id=base.frame_id, ts=ts, ) - self.camera_info.publish(info) - self.depth_camera_info.publish(info) + self.camera_info.publish(camera_info_message(info)) + self.depth_camera_info.publish(camera_info_message(info)) def _publish_tf(self, ts: float, frame: CameraFrame | None) -> None: if frame is None: @@ -989,25 +1013,30 @@ def _publish_tf(self, ts: float, frame: CameraFrame | None) -> None: self.tf.publish( TFMessage( - _transform_from_matrix( - optical_transform, - frame_id=parent_frame, - child_frame_id=self._color_optical_frame, - ts=ts, - ), - _transform_from_matrix( - optical_transform, - frame_id=parent_frame, - child_frame_id=self._depth_optical_frame, - ts=ts, - ), - _transform_from_matrix( - camera_transform, - frame_id=parent_frame, - child_frame_id=self._camera_link, - ts=ts, - ), - *self._body_transforms(), + [ + transform_message(value) + for value in ( + _transform_from_matrix( + optical_transform, + frame_id=parent_frame, + child_frame_id=self._color_optical_frame, + ts=ts, + ), + _transform_from_matrix( + optical_transform, + frame_id=parent_frame, + child_frame_id=self._depth_optical_frame, + ts=ts, + ), + _transform_from_matrix( + camera_transform, + frame_id=parent_frame, + child_frame_id=self._camera_link, + ts=ts, + ), + *self._body_transforms(), + ) + ] ) ) @@ -1071,9 +1100,9 @@ def _generate_pointcloud(self) -> None: depth_scale=1.0, ) pcd = pcd.voxel_downsample(0.005) - self.pointcloud.publish(pcd) + self.pointcloud.publish(cloud_message(pcd)) except Exception as exc: - logger.error("Pointcloud generation error", error=str(exc)) + logger.exception("Pointcloud generation error", error=str(exc)) def _mujoco_lidar_camera_names(self) -> list[str]: names = self.config.mujoco_lidar_camera_names @@ -1104,6 +1133,6 @@ def _generate_mujoco_lidar_pointcloud(self) -> None: timestamp=latest_ts or time.time(), ) pcd = pcd.voxel_downsample(self.config.mujoco_lidar_voxel_size) - self.pointcloud.publish(pcd) + self.pointcloud.publish(cloud_message(pcd)) except Exception as exc: - logger.error("MuJoCo lidar pointcloud generation error", error=str(exc)) + logger.exception("MuJoCo lidar pointcloud generation error", error=str(exc)) diff --git a/dimos/simulation/engines/test_cdr_streams.py b/dimos/simulation/engines/test_cdr_streams.py new file mode 100644 index 00000000000..e0111db4797 --- /dev/null +++ b/dimos/simulation/engines/test_cdr_streams.py @@ -0,0 +1,235 @@ +# 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. + +"""Actual bounded CPU physics and canonical simulation streams, without assets/GPU.""" + +from contextlib import ExitStack +from functools import partial +from threading import Condition + +import numpy as np +import pytest + +from dimos.core.transport import LCMTransport, ZenohTransport +from dimos.message_utils.image import image_view +from dimos.message_utils.occupancy import occupancy_from_array +from dimos.message_utils.pointcloud import pointcloud_rgb, pointcloud_xyz +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos.messages.nav_msgs.msg import Path +from dimos.messages.sensor_msgs.msg import CameraInfo, Image, Imu, PointCloud2 +from dimos.messages.std_msgs.msg import Header +from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo as InternalCameraInfo +from dimos.robot.unitree.g1.blueprints.basic.unitree_g1_groot_wbc import _g1_nav_path +from dimos.robot.unitree.g1.g1_rerun import g1_costmap, g1_odometry_tf_override +from dimos.simulation.engines.mujoco_engine import CameraFrame, MujocoEngine +from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule + + +@pytest.mark.parametrize("backend", ["lcm", "zenoh"]) +def test_real_headless_physics_emits_canonical_odom_imu_and_owns_cleanup(tmp_path, mocker, backend): + mocker.patch( + "dimos.simulation.engines.mujoco_sim_module.time.time_ns", + return_value=1_789_654_321_000_000_001, + ) + xml = tmp_path / "minimal.xml" + xml.write_text(""" + """) + download = mocker.patch( + "dimos.utils.data.get_data", + side_effect=AssertionError("no model/scene download in this test"), + ) + options = ( + {"url": "memq://"} + if backend == "lcm" + else { + "scouting": False, + "multicast": False, + "gossip": False, + "connect": [], + "listen": ["tcp/127.0.0.1:0"], + } + ) + values = {} + condition = Condition() + + def receive(name, value): + with condition: + values[name] = value + condition.notify_all() + + with ExitStack() as cleanup: + module = MujocoSimModule( + address=xml, + headless=True, + dof=1, + inject_legacy_assets=False, + enable_color=False, + enable_depth=False, + enable_pointcloud=False, + camera_name="", + ) + cleanup.callback(module.stop) + for name, kind in [("odom", PoseStamped), ("imu", Imu)]: + wire = ( + LCMTransport("physics_" + name, kind, **options) + if backend == "lcm" + else ZenohTransport("physics_" + name, kind, **options) + ) + cleanup.callback(wire.stop) + module.set_transport(name, wire) + cleanup.callback(wire.subscribe(partial(receive, name))) + module.start() + engine = module._engine + assert engine is not None + physics_thread = engine._sim_thread + publisher_thread = module._publish_thread + with condition: + assert condition.wait_for(lambda: set(values) == {"odom", "imu"}, timeout=5) + assert type(values["odom"]) is PoseStamped and type(values["imu"]) is Imu + assert values["odom"].header.frame_id == "world" + assert values["odom"].header.stamp == Time(1_789_654_321, 1) + assert values["imu"].header.stamp == Time(1_789_654_321, 1) + assert values["odom"].pose.position.z == pytest.approx(1.0) + assert values["odom"].pose.orientation.w == pytest.approx(1.0) + assert values["imu"].orientation.w == pytest.approx(1.0) + np.testing.assert_array_equal(values["imu"].orientation_covariance, np.zeros(9)) + assert values["imu"].angular_velocity.x == pytest.approx(0.0) + assert module._shm_ready_signaled + assert physics_thread is not None and physics_thread.is_alive() + download.assert_not_called() + assert not physics_thread.is_alive() + assert publisher_thread is not None and not publisher_thread.is_alive() + assert module._engine is None and module._shm is None + + +@pytest.mark.parametrize("backend", ["lcm", "zenoh"]) +def test_actual_rgbd_projection_preserves_pixels_source_calibration_and_cloud_at_cdr_ports( + mocker, backend +): + options = ( + {"url": "memq://"} + if backend == "lcm" + else { + "scouting": False, + "multicast": False, + "gossip": False, + "connect": [], + "listen": ["tcp/127.0.0.1:0"], + } + ) + pixels = np.full((2, 2, 3), [255, 0, 0], dtype=np.uint8) + frame = CameraFrame( + rgb=pixels, + depth=np.ones((2, 2), dtype=np.float32), + cam_pos=np.zeros(3), + cam_mat=np.eye(3), + fovy=60, + timestamp=17.125, + ) + condition = Condition() + values = {} + + def receive(name, value): + with condition: + values[name] = value + condition.notify_all() + + with ExitStack() as cleanup: + module = MujocoSimModule(fps=1000) + cleanup.callback(module.stop) + engine = mocker.Mock(spec=MujocoEngine) + engine.connected = True + mocker.patch.object(module, "_engine", engine) + calibration = InternalCameraInfo.from_intrinsics( + width=2, height=2, fx=1, fy=1, cx=0, cy=0, frame_id=module._color_optical_frame + ) + mocker.patch.object(module, "_camera_info_base", calibration) + for name, kind in [ + ("color_image", Image), + ("depth_image", Image), + ("pointcloud", PointCloud2), + ("camera_info", CameraInfo), + ("depth_camera_info", CameraInfo), + ]: + wire = ( + LCMTransport("rgbd_" + name, kind, **options) + if backend == "lcm" + else ZenohTransport("rgbd_" + name, kind, **options) + ) + cleanup.callback(wire.stop) + module.set_transport(name, wire) + cleanup.callback(wire.subscribe(partial(receive, name))) + calls = [frame] + + def read_camera(name): + if calls: + return calls.pop() + module._stop_event.set() + return None + + engine.read_camera.side_effect = read_camera + module._publish_loop() + module._publish_camera_info() + engine.read_camera.side_effect = None + engine.read_camera.return_value = frame + module._generate_pointcloud() + with condition: + assert condition.wait_for(lambda: len(values) == 5, timeout=5) + assert values["color_image"].encoding == "rgb8" + assert values["depth_image"].encoding == "32FC1" + np.testing.assert_array_equal(image_view(values["color_image"]), pixels) + assert values["color_image"].header.stamp == Time(17, 125_000_000) + assert values["camera_info"].header.stamp == Time(17, 125_000_000) + assert values["depth_camera_info"].header.stamp == Time(17, 125_000_000) + assert type(module.get_color_camera_info()) is CameraInfo + assert type(module.get_depth_camera_info()) is CameraInfo + assert values["pointcloud"].header.stamp == Time(17, 125_000_000) + xyz = pointcloud_xyz(values["pointcloud"]) + xyz = xyz[np.lexsort((xyz[:, 1], xyz[:, 0]))] + np.testing.assert_array_equal(xyz, [[0, 0, 1], [0, 1, 1], [1, 0, 1], [1, 1, 1]]) + np.testing.assert_array_equal( + pointcloud_rgb(values["pointcloud"]), np.tile([255, 0, 0], (4, 1)) + ) + direct = module.get_color_camera_info() + assert direct is not None + direct.k[0] = 100 + assert calibration.K[0] == 1 + assert values["camera_info"].k[0] == 1 + np.testing.assert_array_equal(pixels, np.full((2, 2, 3), [255, 0, 0], dtype=np.uint8)) + + +def test_g1_custom_visualizers_accept_canonical_grid_path_and_pose_values(): + grid = occupancy_from_array( + np.array([[-1, 0, 100]], dtype=np.int8), header=Header(Time(17, 1), "map"), resolution=0.5 + ) + mesh = g1_costmap(grid) + np.testing.assert_allclose( + mesh.vertex_positions.as_arrow_array().to_pylist(), + [[0, 0, 0.02], [1.5, 0, 0.02], [1.5, 0.5, 0.02], [0, 0.5, 0.02]], + ) + pose = PoseStamped(Header(Time(17, 1), "map"), Pose(Point(1, 2, 3), Quaternion(0, 0, 0, 1))) + path = _g1_nav_path(Path(Header(Time(17, 1), "map"), [pose])) + assert path.strips.as_arrow_array().to_pylist() == [[[1.0, 2.0, pytest.approx(3.3)]]] + transform = g1_odometry_tf_override(pose)[0][1] + assert transform.translation.as_arrow_array().to_pylist() == [[1.0, 2.0, 0.0]] + assert grid.data.tolist() == [-1, 0, 100] and pose.pose.position.z == 3 diff --git a/dimos/simulation/engines/test_mujoco_sim_module.py b/dimos/simulation/engines/test_mujoco_sim_module.py index 30761039d3e..d050cd24689 100644 --- a/dimos/simulation/engines/test_mujoco_sim_module.py +++ b/dimos/simulation/engines/test_mujoco_sim_module.py @@ -23,6 +23,8 @@ import numpy as np import pytest +from dimos.message_utils.geometry import vector_array +from dimos.message_utils.time import to_seconds from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.simulation.engines.mujoco_engine import CameraFrame, MujocoEngine from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule, MujocoSimModuleConfig @@ -158,14 +160,14 @@ def disconnect(self) -> None: module._publish_tf(10.0, frame) color_tf, depth_tf, camera_link_tf = messages[-1].transforms - assert color_tf.frame_id == "link7" + assert color_tf.header.frame_id == "link7" assert color_tf.child_frame_id == "wrist_camera_color_optical_frame" - assert np.allclose(color_tf.translation.to_numpy(), [0.0, 0.0, 1.0]) - assert depth_tf.frame_id == "link7" + assert np.allclose(vector_array(color_tf.transform.translation), [0.0, 0.0, 1.0]) + assert depth_tf.header.frame_id == "link7" assert depth_tf.child_frame_id == "wrist_camera_depth_optical_frame" - assert camera_link_tf.frame_id == "link7" + assert camera_link_tf.header.frame_id == "link7" assert camera_link_tf.child_frame_id == "wrist_camera_link" - assert np.allclose(camera_link_tf.translation.to_numpy(), [0.0, 0.0, 1.0]) + assert np.allclose(vector_array(camera_link_tf.transform.translation), [0.0, 0.0, 1.0]) finally: module.stop() @@ -205,9 +207,9 @@ def disconnect(self) -> None: ] assert children[3:] == ["apple"] apple = messages[-1].transforms[3] - assert apple.frame_id == "world" - assert apple.ts == pytest.approx(time.time(), abs=5.0) - assert np.allclose(apple.translation.to_numpy(), [0.4, 0.08, 0.17]) + assert apple.header.frame_id == "world" + assert to_seconds(apple.header.stamp) == pytest.approx(time.time(), abs=5.0) + assert np.allclose(vector_array(apple.transform.translation), [0.4, 0.08, 0.17]) finally: module.stop() @@ -377,13 +379,13 @@ def test_publish_loop_stamps_messages_with_frame_timestamp() -> None: _run_publish_loop(module, frame_ts) module._publish_camera_info() - assert [img.ts for img in color] == frame_ts - assert [img.ts for img in depth] == frame_ts - assert [msg.transforms[0].ts for msg in tf] == frame_ts + assert [to_seconds(img.header.stamp) for img in color] == frame_ts + assert [to_seconds(img.header.stamp) for img in depth] == frame_ts + assert [to_seconds(msg.transforms[0].header.stamp) for msg in tf] == frame_ts # camera_info rides the latest frame's sim clock, not wall time. - assert info[-1].ts == frame_ts[-1] - assert module.get_color_camera_info().ts == frame_ts[-1] - assert module.get_depth_camera_info().ts == frame_ts[-1] + assert to_seconds(info[-1].header.stamp) == frame_ts[-1] + assert to_seconds(module.get_color_camera_info().header.stamp) == frame_ts[-1] + assert to_seconds(module.get_depth_camera_info().header.stamp) == frame_ts[-1] finally: module.stop() diff --git a/dimos/spec/perception.py b/dimos/spec/perception.py index 29c715710be..7ef34e6ebc1 100644 --- a/dimos/spec/perception.py +++ b/dimos/spec/perception.py @@ -15,10 +15,9 @@ from typing import Protocol from dimos.core.stream import Out -from dimos.messages.sensor_msgs.msg import CameraInfo, Image as ImageMsg +from dimos.messages.sensor_msgs.msg import CameraInfo, Image as ImageMsg, PointCloud2 from dimos.msgs.nav_msgs.Odometry import Odometry as OdometryMsg from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 class Image(Protocol): diff --git a/dimos/teleop/hosted/test_go2_driver_cdr.py b/dimos/teleop/hosted/test_go2_driver_cdr.py new file mode 100644 index 00000000000..1dfaf8eab08 --- /dev/null +++ b/dimos/teleop/hosted/test_go2_driver_cdr.py @@ -0,0 +1,146 @@ +# 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. + +"""Unit tests for Go2CommandModule's operator-command handling. + +No robot or signaling: real modules exercise the command plane with only the +external driver/provider and publication boundaries mocked. Covers the safety-relevant paths — sport +allow-list, E-STOP latch + fence, nonce dedup, and the drive guard +(stale/future/reorder). +""" + +from collections.abc import Iterator +from contextlib import ExitStack +from threading import Event +from unittest.mock import MagicMock + +from dimos_cdrgen.registry import encode +import pytest + +from dimos.core.transport import LCMTransport, WebRTCTransport, ZenohTransport +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.geometry_msgs.msg import Twist, TwistStamped, Vector3 +from dimos.messages.std_msgs.msg import Header +from dimos.navigation.movement_manager.movement_manager import MovementManager +from dimos.protocol.pubsub.impl.webrtc.providers.broker import BrokerConfig +from dimos.robot.unitree.go2.connection import GO2Connection +from dimos.teleop.hosted.go2_command import Go2CommandModule +from dimos.web.relay_bridge.protocol import FrameHeader, decode_data_frame, encode_data_frame + + +@pytest.fixture +def module(mocker) -> Iterator[Go2CommandModule]: + """Real module and config; only publication and the external driver are mocked.""" + instance = Go2CommandModule() + mocker.patch.object(instance, "go2", MagicMock(), create=True) + for port in ("cmd_ack", "tele_cmd_vel", "robot_state", "goal_request", "stop_movement"): + mocker.patch.object(getattr(instance, port), "publish") + instance._cmd.start() + yield instance + instance.stop() + + +@pytest.mark.parametrize("backend", ["lcm", "zenoh"]) +def test_framed_operator_drive_crosses_real_cdr_ports_to_driver_adapter(mocker, backend): + driver = MagicMock(spec=["move", "stop"]) + mocker.patch("dimos.robot.unitree.go2.connection.make_connection", return_value=driver) + provider = mocker.Mock() + subscriptions = {} + + def subscribe(topic, callback): + subscriptions.setdefault(topic, []).append(callback) + return lambda: subscriptions[topic].remove(callback) + + provider.subscribe.side_effect = subscribe + mocker.patch.object(BrokerConfig, "provider", return_value=provider) + mocker.patch( + "dimos.teleop.hosted.go2_command.time.time_ns", return_value=1_789_654_321_000_000_100 + ) + delivered = Event() + commands = [] + + def record(value, duration): + commands.append((value, duration)) + delivered.set() + return True + + driver.move.side_effect = record + options = ( + {"url": "memq://"} + if backend == "lcm" + else { + "scouting": False, + "multicast": False, + "gossip": False, + "connect": [], + "listen": ["tcp/127.0.0.1:0"], + } + ) + with ExitStack() as cleanup: + cleanup.callback(provider.stop) + command = Go2CommandModule() + cleanup.callback(command.stop) + manager = MovementManager() + cleanup.callback(manager.stop) + robot = GO2Connection() + cleanup.callback(robot.stop) + ingress = WebRTCTransport("cmd_unreliable", TwistStamped, config=BrokerConfig()) + cleanup.callback(ingress.stop) + cleanup.callback(ingress.subscribe(command._on_cmd_vel_in)) + for module, port in [(command, "tele_cmd_vel"), (manager, "cmd_vel")]: + wire = ( + LCMTransport("hosted_test_" + port, Twist, **options) + if backend == "lcm" + else ZenohTransport("hosted_test_" + port, Twist, **options) + ) + cleanup.callback(wire.stop) + module.set_transport(port, wire) + cleanup.callback(command.tele_cmd_vel.subscribe(manager._on_teleop)) + cleanup.callback(manager.cmd_vel.subscribe(lambda value: robot.move(value, 0.0))) + source = TwistStamped( + Header(Time(1_789_654_321, 1), "operator"), + Twist(Vector3(99.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + ) + + def send(value): + frame = encode_data_frame( + FrameHeader( + ch="twist", + seq=0, + ts=17.0, + delivery="latest", + meta={"encoding": "cdr", "type": value.__msgtype__}, + ), + encode(value), + ) + for callback in list(subscriptions["cmd_unreliable"]): + callback(decode_data_frame(frame).payload, "cmd_unreliable") + + send(source) + assert delivered.wait(5) + assert commands[0][0].linear.x == 1.5 + assert commands[0][1] == 0.0 + assert source.twist.linear.x == 99.0 + delivered.clear() + release = TwistStamped( + Header(Time(1_789_654_321, 2), "operator"), + Twist(Vector3(0.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + ) + send(release) + assert delivered.wait(5) + assert commands[1][0].is_zero() + release.header.stamp.nanosec = 3 + send(release) # idle zeros do not cancel another navigation plan + assert len(commands) == 2 + provider.start.assert_called_once() diff --git a/docs/platforms/humanoid/g1/index.md b/docs/platforms/humanoid/g1/index.md index f7064fce06c..77965dc0ba1 100644 --- a/docs/platforms/humanoid/g1/index.md +++ b/docs/platforms/humanoid/g1/index.md @@ -235,3 +235,25 @@ If the viewer keeps crashing for you, there are two options for now: - [Unitree Developer Docs](https://support.unitree.com/home/en/developer) - [Sport Mode Services](https://support.unitree.com/home/en/developer/sports_services) - [Unitree SDK2 Python](https://github.com/unitreerobotics/unitree_sdk2_python) + + +### CDR stream regression checks + +On the CDR proposal, simulator camera, calibration, cloud, odometry, IMU and TF +ports expose generated values from `dimos.messages`. The internal projection, +voxel and controller math stays behind explicit adapters. G1's custom costmap, +path and odometry viewers accept these same wire values. + +From a configured development checkout with its simulation dependencies: + +```sh +DIMOS_TRANSPORT=lcm LCM_DEFAULT_URL=memq:// uv run pytest --noconftest --import-mode=importlib -o addopts='' -q dimos/simulation/engines/test_cdr_streams.py +``` + +This runs a bounded real CPU MuJoCo model defined inside the test, checks CDR +odom/IMU over LCM memq and loopback Zenoh, and verifies physics/publisher threads +and shared memory are cleaned up. Additional cases feed a deterministic RGBD +frame through the existing projection/downsample math and check canonical +camera/cloud/calibration fields and custom Rerun geometry. It needs no robot, +policy weights, external scenes, GPU rendering or display. Actual XR/hardware +operation and full rendered-scene acceptance are separate checks. diff --git a/misc/DimSim/README.md b/misc/DimSim/README.md index f83c8cc7e2f..fee1e28547e 100644 --- a/misc/DimSim/README.md +++ b/misc/DimSim/README.md @@ -52,3 +52,37 @@ dimsim eval --headless --scene apartment # full headless run (CI) npm install # browser deps (three, rapier, vite) npm run build # → dist/ ``` + +## Canonical message encoding + +The browser and Deno physics/lidar bridge publish generated ROS 2 CDR layouts +over the existing raw LCM/WebSocket transport. Channels include `/cdr/` and the +ROS type name, for example `cmd_vel/cdr/geometry_msgs/msg/Twist`. Color frames +are `rgba8` Images with raw canvas pixels; depth stays clamped uint16 millimeters. +The eval/sidebar JPEG remains a separate presentation path. + +`cli/bridge/cdr_schemas.json` is a generated four-type schema artifact exported +from the installed canonical DimOS message package. The Python acceptance test +checks every complete dependency schema against that package. Browser and Deno +share one codec; no ROS installation is needed. The existing CLI rebuilds an +old cached frontend when the CDR sources, schema artifact or package manifest +change, and prepares the pinned Foxglove dependencies when missing. + +For an offline encoding/physics check with development dependencies installed: + +```bash +cd misc/DimSim +deno install --no-lock +deno run --allow-all --unstable-net cli/cli.ts build +cd ../.. +DIMOS_DIMSIM_DENO="$(command -v deno)" DIMOS_TRANSPORT=lcm LCM_DEFAULT_URL=memq:// \ + uv run pytest -o addopts='' --import-mode=importlib -q \ + dimos/robot/unitree/test_dimsim_connection.py \ + dimos/simulation/dimsim/test_dimsim_process.py +``` + +Dependency preparation can download the declared packages; the tests themselves +run Deno with cached dependencies and no network permission. They exercise +actual browser publishing methods, two bounded CPU Rapier physics steps, the +command deadman, one real lidar scan, independent Python CDR decoding and local +odom-to-TF cleanup. They do not launch a rendered scene or perform a benchmark. diff --git a/misc/DimSim/cli/bridge/cdr.ts b/misc/DimSim/cli/bridge/cdr.ts new file mode 100644 index 00000000000..bdac6673966 --- /dev/null +++ b/misc/DimSim/cli/bridge/cdr.ts @@ -0,0 +1,76 @@ +/** CDR codecs for the generated schema artifact shared by browser and Deno. + * cdr_schemas.json is emitted from the installed canonical DimOS message package; + * the Python acceptance test verifies it against that package's schema registry. + */ +import { parse } from "@foxglove/rosmsg"; +import { MessageReader, MessageWriter } from "@foxglove/rosmsg2-serialization"; +import schemas from "./cdr_schemas.json" with { type: "json" }; + +const writers = new Map(); +const readers = new Map(); +function definition(type: string): string { + const value = schemas[type as keyof typeof schemas]; + if (!value) throw new Error(`Unknown CDR type: ${type}`); + return value; +} +export function channel(topic: string, type: string): string { + definition(type); + return `${topic.replace(/^\/+/, "")}/cdr/${type}`; +} +export function encodeCdr( + type: string, + value: Record, +): Uint8Array { + let writer = writers.get(type); + if (!writer) { + writer = new MessageWriter(parse(definition(type), { ros2: true })); + writers.set(type, writer); + } + return writer.writeMessage(value); +} +export function decodeCdr( + type: string, + data: Uint8Array, +): Record { + if ( + data.length < 4 || data[0] !== 0 || data[1] > 1 || data[2] !== 0 || + data[3] !== 0 + ) { + throw new Error("invalid XCDR1 header"); + } + let reader = readers.get(type); + if (!reader) { + reader = new MessageReader(parse(definition(type), { ros2: true })); + readers.set(type, reader); + } + const value = reader.readMessage(data); + if (reader.lastReadByteLength() !== data.length) { + throw new Error("trailing CDR bytes"); + } + if (value === null || typeof value !== "object" || Array.isArray(value)) { + throw new Error("invalid CDR message object"); + } + return value as Record; +} +export function headerNow(frameId: string) { + const now = Date.now(); + return { + stamp: { sec: Math.floor(now / 1000), nanosec: (now % 1000) * 1_000_000 }, + frame_id: frameId, + }; +} +/** Existing short LCM packet framing over WS; UDP fragmentation stays in the relay. */ +export function sensorPacket( + topic: string, + value: Record, +): Uint8Array { + const split = topic.indexOf("/cdr/"); + if (split < 0) throw new Error("missing CDR channel type"); + const data = encodeCdr(topic.slice(split + 5), value); + const name = new TextEncoder().encode(topic); + const packet = new Uint8Array(8 + name.length + 1 + data.length); + new DataView(packet.buffer).setUint32(0, 0x4c433032, false); + packet.set(name, 8); + packet.set(data, 9 + name.length); + return packet; +} diff --git a/misc/DimSim/cli/bridge/cdr_schemas.json b/misc/DimSim/cli/bridge/cdr_schemas.json new file mode 100644 index 00000000000..053b8a177d4 --- /dev/null +++ b/misc/DimSim/cli/bridge/cdr_schemas.json @@ -0,0 +1,6 @@ +{ + "sensor_msgs/msg/Image": "# This message contains an uncompressed image\n# (0, 0) is at top-left corner of image\n\nstd_msgs/Header header # Header timestamp should be acquisition time of image\n # Header frame_id should be optical frame of camera\n # origin of frame should be optical center of cameara\n # +x should point to the right in the image\n # +y should point down in the image\n # +z should point into to plane of the image\n # If the frame_id here and the frame_id of the CameraInfo\n # message associated with the image conflict\n # the behavior is undefined\n\nuint32 height # image height, that is, number of rows\nuint32 width # image width, that is, number of columns\n\n# The legal values for encoding are in file include/sensor_msgs/image_encodings.hpp\n# If you want to standardize a new string format, join\n# ros-users@lists.ros.org and send an email proposing a new encoding.\n\nstring encoding # Encoding of pixels -- channel meaning, ordering, size\n # taken from the list of strings in include/sensor_msgs/image_encodings.hpp\n\nuint8 is_bigendian # is this data bigendian?\nuint32 step # Full row length in bytes\nuint8[] data # actual matrix data, size is (step * rows)\n================================================================================\nMSG: builtin_interfaces/Time\n# This message communicates ROS Time defined here:\n# https://design.ros2.org/articles/clock_and_time.html\n\n# The seconds component, valid over all int32 values.\nint32 sec\n\n# The nanoseconds component, valid in the range [0, 1e9), to be added to the seconds component. \n# e.g.\n# The time -1.7 seconds is represented as {sec: -2, nanosec: 3e8}\n# The time 1.7 seconds is represented as {sec: 1, nanosec: 7e8}\nuint32 nanosec\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data\n# in a particular coordinate frame.\n\n# Two-integer timestamp that is expressed as seconds and nanoseconds.\nbuiltin_interfaces/Time stamp\n\n# Transform frame with which this data is associated.\nstring frame_id\n", + "sensor_msgs/msg/PointCloud2": "# This message holds a collection of N-dimensional points, which may\n# contain additional information such as normals, intensity, etc. The\n# point data is stored as a binary blob, its layout described by the\n# contents of the \"fields\" array.\n#\n# The point cloud data may be organized 2d (image-like) or 1d (unordered).\n# Point clouds organized as 2d images may be produced by camera depth sensors\n# such as stereo or time-of-flight.\n\n# Time of sensor data acquisition, and the coordinate frame ID (for 3d points).\nstd_msgs/Header header\n\n# 2D structure of the point cloud. If the cloud is unordered, height is\n# 1 and width is the length of the point cloud.\nuint32 height\nuint32 width\n\n# Describes the channels and their layout in the binary data blob.\nPointField[] fields\n\nbool is_bigendian # Is this data bigendian?\nuint32 point_step # Length of a point in bytes\nuint32 row_step # Length of a row in bytes\nuint8[] data # Actual point data, size is (row_step*height)\n\nbool is_dense # True if there are no invalid points\n================================================================================\nMSG: builtin_interfaces/Time\n# This message communicates ROS Time defined here:\n# https://design.ros2.org/articles/clock_and_time.html\n\n# The seconds component, valid over all int32 values.\nint32 sec\n\n# The nanoseconds component, valid in the range [0, 1e9), to be added to the seconds component. \n# e.g.\n# The time -1.7 seconds is represented as {sec: -2, nanosec: 3e8}\n# The time 1.7 seconds is represented as {sec: 1, nanosec: 7e8}\nuint32 nanosec\n================================================================================\nMSG: sensor_msgs/PointField\n# This message holds the description of one point entry in the\n# PointCloud2 message format.\nuint8 INT8 = 1\nuint8 UINT8 = 2\nuint8 INT16 = 3\nuint8 UINT16 = 4\nuint8 INT32 = 5\nuint8 UINT32 = 6\nuint8 FLOAT32 = 7\nuint8 FLOAT64 = 8\n\n# Common PointField names are x, y, z, intensity, rgb, rgba\nstring name # Name of field\nuint32 offset # Offset from start of point struct\nuint8 datatype # Datatype enumeration, see above\nuint32 count # How many elements in the field\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data\n# in a particular coordinate frame.\n\n# Two-integer timestamp that is expressed as seconds and nanoseconds.\nbuiltin_interfaces/Time stamp\n\n# Transform frame with which this data is associated.\nstring frame_id\n", + "geometry_msgs/msg/PoseStamped": "# A Pose with reference coordinate frame and timestamp\n\nstd_msgs/Header header\nPose pose\n================================================================================\nMSG: builtin_interfaces/Time\n# This message communicates ROS Time defined here:\n# https://design.ros2.org/articles/clock_and_time.html\n\n# The seconds component, valid over all int32 values.\nint32 sec\n\n# The nanoseconds component, valid in the range [0, 1e9), to be added to the seconds component. \n# e.g.\n# The time -1.7 seconds is represented as {sec: -2, nanosec: 3e8}\n# The time 1.7 seconds is represented as {sec: 1, nanosec: 7e8}\nuint32 nanosec\n================================================================================\nMSG: geometry_msgs/Point\n# This contains the position of a point in free space\nfloat64 x\nfloat64 y\nfloat64 z\n================================================================================\nMSG: geometry_msgs/Pose\n# A representation of pose in free space, composed of position and orientation.\n\nPoint position\nQuaternion orientation\n================================================================================\nMSG: geometry_msgs/Quaternion\n# This represents an orientation in free space in quaternion form.\n\nfloat64 x 0\nfloat64 y 0\nfloat64 z 0\nfloat64 w 1\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data\n# in a particular coordinate frame.\n\n# Two-integer timestamp that is expressed as seconds and nanoseconds.\nbuiltin_interfaces/Time stamp\n\n# Transform frame with which this data is associated.\nstring frame_id\n", + "geometry_msgs/msg/Twist": "# This expresses velocity in free space broken into its linear and angular parts.\n\nVector3 linear\nVector3 angular\n================================================================================\nMSG: geometry_msgs/Vector3\n# This represents a vector in free space.\n\n# This is semantically different than a point.\n# A vector is always anchored at the origin.\n# When a transform is applied to a vector, only the rotational component is applied.\n\nfloat64 x\nfloat64 y\nfloat64 z\n" +} diff --git a/misc/DimSim/cli/bridge/cdr_test.ts b/misc/DimSim/cli/bridge/cdr_test.ts new file mode 100644 index 00000000000..fdc216fcbc2 --- /dev/null +++ b/misc/DimSim/cli/bridge/cdr_test.ts @@ -0,0 +1,195 @@ +import { channel, decodeCdr, encodeCdr, sensorPacket } from "./cdr.ts"; +import { ServerPhysics } from "./physics.ts"; +import { ServerLidar } from "./lidar.ts"; +import { DimosBridge } from "../../src/bridge.ts"; +import { decodePacket, encodeSmallMessage } from "../vendor/lcm/transport.ts"; +import type { LCM } from "../vendor/lcm/lcm.ts"; +import RAPIER from "@dimforge/rapier3d-compat"; + +function assert(value: unknown, message: string): asserts value { + if (!value) throw new Error(message); +} +function hex(data: Uint8Array): string { + return Array.from(data, (value) => value.toString(16).padStart(2, "0")).join( + "", + ); +} +function bytes(value: string): Uint8Array { + return Uint8Array.from( + value.match(/../g) ?? [], + (part) => parseInt(part, 16), + ); +} +function rejects(operation: () => unknown): void { + let failed = false; + try { + operation(); + } catch { + failed = true; + } + assert(failed, "malformed payload must fail without a legacy fallback"); +} +const input = JSON.parse( + await Deno.readTextFile(Deno.env.get("DIMOS_DIMSIM_CDR_FIXTURES")!), +); +const output: Record = {}; + +Deno.test("Python little/big-endian fixtures retain all fields through canonical CDR", () => { + for (const fixture of input.values) { + const value = decodeCdr(fixture.type, bytes(fixture.hex)); + assert( + hex(encodeCdr(fixture.type, value)) === fixture.canonicalHex, + fixture.type, + ); + } + const kind = "geometry_msgs/msg/Twist"; + rejects(() => decodeCdr(kind, new Uint8Array([0, 2, 0, 0]))); + rejects(() => decodeCdr(kind, new Uint8Array([1, 0, 0, 0]))); + rejects(() => decodeCdr(kind, new Uint8Array([0, 1, 0]))); + rejects(() => decodeCdr(kind, bytes(input.command + "00"))); + rejects(() => encodeCdr("missing/msg/Type", {})); +}); + +Deno.test("actual browser methods publish raw RGBA and clamped millimeter depth", () => { + const previousLocation = Object.getOwnPropertyDescriptor( + globalThis, + "location", + ); + const previousClock = Date.now; + Object.defineProperty(globalThis, "location", { + configurable: true, + value: { protocol: "http:", host: "fixture", search: "" }, + }); + Date.now = () => 1_700_000_000_001; + try { + const packets: Uint8Array[] = []; + const socket = { + readyState: WebSocket.OPEN, + send: (data: Uint8Array) => packets.push(data), + } as unknown as WebSocket; + const bridge = new DimosBridge({ + agent: {}, + sensorSources: { + captureRgb: () => ({ + width: 2, + height: 1, + data: new Uint8Array([1, 2, 3, 255, 4, 5, 6, 128]), + }), + captureDepth: () => ({ + width: 3, + height: 2, + data: new Float32Array([-1, 0, 0.5, 1.25, 100, NaN]), + }), + }, + }); + bridge.wsRgb = socket; + bridge.wsDepth = socket; + bridge._publishImages(); + assert(packets.length === 2, "both sensors must publish"); + for (const packet of packets) { + const decoded = decodePacket(packet); + assert(decoded?.type === "small", "WS retains LC02 envelope"); + const key = decoded.channel.startsWith("color_image/") + ? "color" + : "depth"; + output[key] = hex(decoded.data); + } + const large = sensorPacket( + channel("/color_image", "sensor_msgs/msg/Image"), + { + header: { stamp: { sec: 1, nanosec: 1 }, frame_id: "camera" }, + width: 640, + height: 288, + encoding: "rgba8", + is_bigendian: 0, + step: 2560, + data: new Uint8Array(640 * 288 * 4), + }, + ); + assert( + large.length > 65535 && decodePacket(large)?.type === "small", + "WS envelope must not impose UDP limit", + ); + } finally { + Date.now = previousClock; + if (previousLocation) { + Object.defineProperty(globalThis, "location", previousLocation); + } else Reflect.deleteProperty(globalThis, "location"); + } +}); + +Deno.test("real bounded Rapier command, deadman and lidar emit CDR through raw LCM boundary", async () => { + await RAPIER.init(); + const world = new RAPIER.World({ x: 0, y: -9.81, z: 0 }); + world.createCollider( + RAPIER.ColliderDesc.cuboid(10, 0.1, 10).setTranslation(0, -0.1, 0), + ); + const receivers: ((packet: Uint8Array) => void)[] = []; + const poses: Uint8Array[] = []; + const previousClock = Date.now; + let clock = 1_700_000_000_001; + Date.now = () => clock; + let poseDone!: () => void; + let lidarDone!: () => void; + const poseReady = new Promise((resolve) => { + poseDone = resolve; + }); + const lidarReady = new Promise((resolve) => { + lidarDone = resolve; + }); + const network = { + subscribePacket: (receiver: (packet: Uint8Array) => void) => { + receivers.push(receiver); + return () => {}; + }, + getNextSeq: () => 0, + publishRaw: (name: string, data: Uint8Array) => { + if (name === channel("/odom", "geometry_msgs/msg/PoseStamped")) { + poses.push(data); + // Advance the source clock past the unchanged deadman deadline, without sleeping. + if (poses.length === 1) clock += 501; + if (poses.length === 2) poseDone(); + } else if (name === channel("/lidar", "sensor_msgs/msg/PointCloud2")) { + output.cloud = hex(data); + lidarDone(); + } + return Promise.resolve(); + }, + } as unknown as LCM; + const physics = new ServerPhysics(network, world, RAPIER, new Set()); + const lidar = new ServerLidar(network, world, RAPIER, new Set()); + let timer: ReturnType | undefined; + try { + const deadline = new Promise((_, reject) => { + timer = setTimeout( + () => reject(new Error("bounded physics/lidar did not publish")), + 5000, + ); + }); + physics.start(); + const command = encodeSmallMessage( + channel("/cmd_vel", "geometry_msgs/msg/Twist"), + bytes(input.command), + 1, + ); + for (const receive of receivers) receive(command); + await Promise.race([poseReady, deadline]); + physics.stop(); + output.pose = hex(poses[0]); + output.stoppedPose = hex(poses[1]); + lidar.setExcludeBody(physics.getBody()); + lidar.updatePose(0, 1, 0, 0, 0, 0, 1); + lidar.start(); + await Promise.race([lidarReady, deadline]); + } finally { + if (timer !== undefined) clearTimeout(timer); + physics.stop(); + lidar.stop(); + world.free(); + Date.now = previousClock; + } + await Deno.writeTextFile( + Deno.env.get("DIMOS_DIMSIM_CDR_RESULT")!, + JSON.stringify(output), + ); +}); diff --git a/misc/DimSim/cli/bridge/lidar.ts b/misc/DimSim/cli/bridge/lidar.ts index 334940d2027..9baf5c601c2 100644 --- a/misc/DimSim/cli/bridge/lidar.ts +++ b/misc/DimSim/cli/bridge/lidar.ts @@ -2,14 +2,11 @@ * Server-side LiDAR raycasting using Rapier physics world snapshot. * * Runs 20K Fibonacci-sphere raycasts at 5 Hz on the Deno bridge server, - * encodes PointCloud2 via @dimos/msgs, and publishes directly to LCM — + * encodes canonical PointCloud2 as CDR, and publishes directly to LCM — * no WebSocket hop needed. */ -import { - sensor_msgs, - std_msgs, -} from "@dimos/msgs"; +import { channel, encodeCdr, headerNow } from "./cdr.ts"; import type { LCM } from "../vendor/lcm/lcm.ts"; // -- Lidar constants (must match engine.js) ----------------------------------- @@ -25,7 +22,7 @@ const V_MAX_RAD = (15 * Math.PI) / 180; // drops frames and effective rate sits below the target on slow/CPU-render boxes. const RATE_MS = 200; // 5 Hz default -const CH_LIDAR = "/lidar#sensor_msgs.PointCloud2"; +const CH_LIDAR = channel("/lidar", "sensor_msgs/msg/PointCloud2"); // Agent capsule geometry → lidar mount offset (must match engine.js) const DEFAULT_HALF_HEIGHT = 0.25; @@ -277,35 +274,22 @@ export class ServerLidar { view.setFloat32(off + 12, intensity[i], true); } - const now = Date.now(); - const header = new std_msgs.Header({ - stamp: new std_msgs.Time({ sec: Math.floor(now / 1000), nsec: (now % 1000) * 1_000_000 }), - frame_id: "world", - }); - - const msg = new sensor_msgs.PointCloud2({ - header, - height: 1, - width: n, - fields_length: 4, + const msg = { + header: headerNow("world"), height: 1, width: n, fields: [ - new sensor_msgs.PointField({ name: "x", offset: 0, datatype: 7, count: 1 }), - new sensor_msgs.PointField({ name: "y", offset: 4, datatype: 7, count: 1 }), - new sensor_msgs.PointField({ name: "z", offset: 8, datatype: 7, count: 1 }), - new sensor_msgs.PointField({ name: "intensity", offset: 12, datatype: 7, count: 1 }), + { name: "x", offset: 0, datatype: 7, count: 1 }, + { name: "y", offset: 4, datatype: 7, count: 1 }, + { name: "z", offset: 8, datatype: 7, count: 1 }, + { name: "intensity", offset: 12, datatype: 7, count: 1 }, ], - is_bigendian: false, - point_step: pointStep, - row_step: n * pointStep, - data_length: n * pointStep, - data: new Uint8Array(buf), - is_dense: true, - }); + is_bigendian: false, point_step: pointStep, row_step: n * pointStep, + data: new Uint8Array(buf), is_dense: true, + }; // Mark seq for echo filtering (prevent server re-forwarding to browser WS) this.sentSeqs.add(this.lcm.getNextSeq()); // Publish directly to LCM — no WS hop (await so buffer pressure is felt) - await this.lcm.publish(CH_LIDAR, msg); + await this.lcm.publishRaw(CH_LIDAR, encodeCdr("sensor_msgs/msg/PointCloud2", msg)); if (profile) { const total = performance.now() - scanStart; diff --git a/misc/DimSim/cli/bridge/physics.ts b/misc/DimSim/cli/bridge/physics.ts index ff875522406..ddb760413f8 100644 --- a/misc/DimSim/cli/bridge/physics.ts +++ b/misc/DimSim/cli/bridge/physics.ts @@ -12,7 +12,8 @@ * position updates and moves the visual avatar. */ -import { geometry_msgs, std_msgs } from "@dimos/msgs"; +import { channel, decodeCdr, encodeCdr, headerNow } from "./cdr.ts"; +import { decodePacket } from "../vendor/lcm/transport.ts"; import type { LCM } from "../vendor/lcm/lcm.ts"; @@ -117,8 +118,8 @@ function resolveMotionModel(embodiment?: EmbodimentConfig): string { return "holonomic"; } -const CH_ODOM = "/odom#geometry_msgs.PoseStamped"; -const CH_CMD_VEL = "/cmd_vel#geometry_msgs.Twist"; +const CH_ODOM = channel("/odom", "geometry_msgs/msg/PoseStamped"); +const CH_CMD_VEL = channel("/cmd_vel", "geometry_msgs/msg/Twist"); const CMD_VEL_TIMEOUT_MS = 500; // -- ServerPhysics ------------------------------------------------------------ @@ -333,8 +334,15 @@ export class ServerPhysics { /** Subscribe to cmd_vel on LCM. */ subscribeCmdVel(): void { - this.lcm.subscribe(CH_CMD_VEL, geometry_msgs.Twist, (msg: any) => { - this.handleCmdVel(msg.data); + // Twist fits a short packet; retain the existing raw LCM transport unchanged. + this.lcm.subscribePacket((packet: Uint8Array) => { + const message = decodePacket(packet); + if (message?.type !== "small" || message.channel !== CH_CMD_VEL) return; + try { + this.handleCmdVel(decodeCdr("geometry_msgs/msg/Twist", message.data)); + } catch (error) { + console.warn("[physics] invalid CDR cmd_vel:", error); + } }); // quiet } @@ -476,32 +484,18 @@ export class ServerPhysics { const qw = Math.cos(this.yaw / 2); const qRosZ = Math.sin(this.yaw / 2); // rotation about ROS Z - const now = Date.now(); - - const header = new std_msgs.Header({ - seq: this.seq++, - stamp: new std_msgs.Time({ sec: Math.floor(now / 1000), nsec: (now % 1000) * 1_000_000 }), - frame_id: "world", - }); - - const pose = new geometry_msgs.Pose(); - pose.position = new geometry_msgs.Point(); - pose.position.x = rosX; - pose.position.y = rosY; - pose.position.z = rosZ; - pose.orientation = new geometry_msgs.Quaternion(); - pose.orientation.x = 0; - pose.orientation.y = 0; - pose.orientation.z = qRosZ; - pose.orientation.w = qw; - - const odom = new geometry_msgs.PoseStamped(); - odom.header = header; - odom.pose = pose; + this.seq++; + const odom = { + header: headerNow("world"), + pose: { + position: { x: rosX, y: rosY, z: rosZ }, + orientation: { x: 0, y: 0, z: qRosZ, w: qw }, + }, + }; try { this.sentSeqs.add(this.lcm.getNextSeq()); - this.lcm.publishRaw(CH_ODOM, odom.encode()).catch(() => {}); + this.lcm.publishRaw(CH_ODOM, encodeCdr("geometry_msgs/msg/PoseStamped", odom)).catch(() => {}); } catch (e: unknown) { if (this.seq <= 3) console.warn("[physics] odom publish error:", e); } diff --git a/misc/DimSim/cli/bridge/server.ts b/misc/DimSim/cli/bridge/server.ts index 851c415e632..673399e141f 100644 --- a/misc/DimSim/cli/bridge/server.ts +++ b/misc/DimSim/cli/bridge/server.ts @@ -429,7 +429,7 @@ export async function startBridgeServer(options: BridgeServerOptions) { try { const decoded = decodePacket(packet); if (decoded && decoded.type === "small") { - if (chState.serverPhysics && decoded.channel === "/odom#geometry_msgs.PoseStamped") { + if (chState.serverPhysics && decoded.channel === "odom/cdr/geometry_msgs/msg/PoseStamped") { return; } diff --git a/misc/DimSim/cli/cli.ts b/misc/DimSim/cli/cli.ts index 3ddc3054ad4..acbe8baa0ff 100644 --- a/misc/DimSim/cli/cli.ts +++ b/misc/DimSim/cli/cli.ts @@ -35,6 +35,15 @@ const SCENES_DIR = resolve(PROJECT_DIR, "scenes"); * --no-lock keeps the repo's deno.lock (which tracks only JSR deps for * the CLI) from being polluted with the frontend's npm dep graph. */ +// The CDR producer and browser bundle must move together, including cached checkouts. +async function cdrBuildFingerprint(projectDir: string): Promise { + const inputs = ["src/bridge.ts", "src/engine.js", "cli/bridge/cdr.ts", + "cli/bridge/cdr_schemas.json", "package.json", "package-lock.json"]; + const sources = await Promise.all(inputs.map(async name => [name, await Deno.readTextFile(`${projectDir}/${name}`)])); + const digest = await crypto.subtle.digest("SHA-256", new TextEncoder().encode(JSON.stringify(sources))); + return Array.from(new Uint8Array(digest), value => value.toString(16).padStart(2, "0")).join(""); +} + async function tryBuildFromSource( projectDir: string, distDir: string, @@ -53,6 +62,8 @@ async function tryBuildFromSource( // bare imports (three, rapier, etc.) via node_modules at build time. try { await Deno.stat(`${projectDir}/node_modules`); + await Deno.stat(`${projectDir}/node_modules/@foxglove/rosmsg/package.json`); + await Deno.stat(`${projectDir}/node_modules/@foxglove/rosmsg2-serialization/package.json`); } catch { console.log(`[dimsim] node_modules/ not found — running 'deno install' (one-time)...`); const install = new Deno.Command(Deno.execPath(), { @@ -80,6 +91,7 @@ async function tryBuildFromSource( try { await Deno.stat(`${distDir}/index.html`); + await Deno.writeTextFile(`${distDir}/.dimos-cdr-build.sha256`, await cdrBuildFingerprint(projectDir)); return true; } catch { console.error(`[dimsim] Build completed but ${distDir}/index.html is missing.`); @@ -91,7 +103,8 @@ async function tryBuildFromSource( async function resolveDistDir(): Promise { try { await Deno.stat(`${LOCAL_DIST_DIR}/index.html`); - return LOCAL_DIST_DIR; + const previous = await Deno.readTextFile(`${LOCAL_DIST_DIR}/.dimos-cdr-build.sha256`); + if (previous === await cdrBuildFingerprint(PROJECT_DIR)) return LOCAL_DIST_DIR; } catch { /* not found — fall through to build */ } if (await tryBuildFromSource(PROJECT_DIR, LOCAL_DIST_DIR)) { diff --git a/misc/DimSim/cli/deno.json b/misc/DimSim/cli/deno.json index d3e9506cea6..2f4190d471b 100644 --- a/misc/DimSim/cli/deno.json +++ b/misc/DimSim/cli/deno.json @@ -11,10 +11,11 @@ "eval": "deno run --allow-all cli.ts eval" }, "imports": { - "@dimos/msgs": "jsr:@dimos/msgs@^0.1.4", "@std/path": "jsr:@std/path@^1", "@std/http": "jsr:@std/http@^1", "playwright": "npm:playwright@^1.58", - "@dimforge/rapier3d-compat": "npm:@dimforge/rapier3d-compat@^0.14.0" + "@dimforge/rapier3d-compat": "npm:@dimforge/rapier3d-compat@^0.14.0", + "@foxglove/rosmsg": "npm:@foxglove/rosmsg@5.0.5", + "@foxglove/rosmsg2-serialization": "npm:@foxglove/rosmsg2-serialization@3.1.2" } } diff --git a/misc/DimSim/cli/deno.lock b/misc/DimSim/cli/deno.lock index 7cbc0c61ac8..6a6e631d865 100644 --- a/misc/DimSim/cli/deno.lock +++ b/misc/DimSim/cli/deno.lock @@ -1,55 +1,41 @@ { "version": "5", "specifiers": { - "jsr:@antim/dimsim@*": "0.1.27", - "jsr:@dimos/msgs@~0.1.4": "0.1.4", - "jsr:@std/cli@^1.0.27": "1.0.27", - "jsr:@std/encoding@^1.0.10": "1.0.10", - "jsr:@std/fmt@^1.0.9": "1.0.9", - "jsr:@std/fs@^1.0.22": "1.0.22", - "jsr:@std/html@^1.0.5": "1.0.5", - "jsr:@std/http@1": "1.0.24", - "jsr:@std/internal@^1.0.12": "1.0.12", + "jsr:@std/cli@^1.0.32": "1.0.32", + "jsr:@std/encoding@^1.0.11": "1.0.11", + "jsr:@std/fmt@^1.0.10": "1.0.10", + "jsr:@std/fs@^1.0.24": "1.0.24", + "jsr:@std/html@^1.0.7": "1.0.7", + "jsr:@std/http@1": "1.1.4", + "jsr:@std/internal@^1.0.14": "1.0.14", "jsr:@std/media-types@^1.1.0": "1.1.0", - "jsr:@std/net@^1.0.6": "1.0.6", - "jsr:@std/path@1": "1.1.4", - "jsr:@std/path@^1.1.4": "1.1.4", - "jsr:@std/streams@^1.0.17": "1.0.17", + "jsr:@std/net@^1.0.7": "1.0.7", + "jsr:@std/path@1": "1.1.6", + "jsr:@std/path@^1.1.6": "1.1.6", + "jsr:@std/streams@^1.2.0": "1.2.0", "npm:@dimforge/rapier3d-compat@0.14": "0.14.0", - "npm:playwright@*": "1.58.2", - "npm:playwright@^1.58.0": "1.58.2" + "npm:@foxglove/rosmsg2-serialization@3.1.2": "3.1.2", + "npm:@foxglove/rosmsg@5.0.5": "5.0.5", + "npm:playwright@^1.58.0": "1.63.0" }, "jsr": { - "@antim/dimsim@0.1.27": { - "integrity": "fc7f8f3aaeb0e06b06aaf35e157308f656be344db434a10bb573343c43a4bbbb", - "dependencies": [ - "jsr:@dimos/msgs", - "jsr:@std/http", - "jsr:@std/path@1", - "npm:@dimforge/rapier3d-compat", - "npm:playwright@^1.58.0" - ] - }, - "@dimos/msgs@0.1.4": { - "integrity": "564bc30b4bc41a562c296c257a15055283ca0cbd66d0627991ede5295832d0c4" - }, - "@std/cli@1.0.27": { - "integrity": "eba97edd0891871a7410e835dd94b3c260c709cca5983df2689c25a71fbe04de" + "@std/cli@1.0.32": { + "integrity": "188b3a100d6202d64e3f5bd3d799c7fa4f6d77f92cc65eb7f641c1fa0aa92a66" }, - "@std/encoding@1.0.10": { - "integrity": "8783c6384a2d13abd5e9e87a7ae0520a30e9f56aeeaa3bdf910a3eaaf5c811a1" + "@std/encoding@1.0.11": { + "integrity": "e7cef2f0b3153bccc17431e7c864a1de03a4b6d9647389c43e08aaa775d37385" }, - "@std/fmt@1.0.9": { - "integrity": "2487343e8899fb2be5d0e3d35013e54477ada198854e52dd05ed0422eddcabe0" + "@std/fmt@1.0.10": { + "integrity": "90dfba288802ac6de82fb31d0917eb9e4450b9925b954d5e51fc29ac07419db5" }, - "@std/fs@1.0.22": { - "integrity": "de0f277a58a867147a8a01bc1b181d0dfa80bfddba8c9cf2bacd6747bcec9308" + "@std/fs@1.0.24": { + "integrity": "f3061b45b81673a2bece689da041df32d174be064c89eb6397fb5718d3fb7877" }, - "@std/html@1.0.5": { - "integrity": "4e2d693f474cae8c16a920fa5e15a3b72267b94b84667f11a50c6dd1cb18d35e" + "@std/html@1.0.7": { + "integrity": "175c818905a6e75743c69c251395e273d82698e58cfe7dd76268d70e28b8d8fe" }, - "@std/http@1.0.24": { - "integrity": "4dd59afd7cfd6e2e96e175b67a5a829b449ae55f08575721ec691e5d85d886d4", + "@std/http@1.1.4": { + "integrity": "4f1575226ca7c0df1df38b26d50265fed30869b2a63ab867edc5b92492150c8c", "dependencies": [ "jsr:@std/cli", "jsr:@std/encoding", @@ -58,59 +44,79 @@ "jsr:@std/html", "jsr:@std/media-types", "jsr:@std/net", - "jsr:@std/path@^1.1.4", + "jsr:@std/path@^1.1.6", "jsr:@std/streams" ] }, - "@std/internal@1.0.12": { - "integrity": "972a634fd5bc34b242024402972cd5143eac68d8dffaca5eaa4dba30ce17b027" + "@std/internal@1.0.14": { + "integrity": "291516b3d4c35024d6ffbc0a9df5bf4c64116e05b50012cf846710152d2ffdf7" }, "@std/media-types@1.1.0": { "integrity": "c9d093f0c05c3512932b330e3cc1fe1d627b301db33a4c2c2185c02471d6eaa4" }, - "@std/net@1.0.6": { - "integrity": "110735f93e95bb9feb95790a8b1d1bf69ec0dc74f3f97a00a76ea5efea25500c" + "@std/net@1.0.7": { + "integrity": "e0b995738cfe2ed56f64591d400e55f143cff164efd690343f2deb4cf4d06d86" }, - "@std/path@1.1.4": { - "integrity": "1d2d43f39efb1b42f0b1882a25486647cb851481862dc7313390b2bb044314b5", + "@std/path@1.1.6": { + "integrity": "c68485c2a4dfbb5ae3cc74fae4e8c4e5d874cf8a8ed12927917235c758b46cbe", "dependencies": [ "jsr:@std/internal" ] }, - "@std/streams@1.0.17": { - "integrity": "7859f3d9deed83cf4b41f19223d4a67661b3d3819e9fc117698f493bf5992140" + "@std/streams@1.2.0": { + "integrity": "256b934254778f2bf45f5713b934209ff29e9b4b7fd4ec095219966813758c5f" } }, "npm": { "@dimforge/rapier3d-compat@0.14.0": { "integrity": "sha512-/uHrUzS+CRQ+NQrrJCEDUkhwHlNsAAexbNXgbN9sHY+GwR+SFFAFrxRr8Llf5/AJZzqiLANdQIfJ63Cw4gJVqw==" }, - "fsevents@2.3.2": { - "integrity": "sha512-xiqMQR4xAeHTuB9uWm+fFRcIOgKBMiOBP+eXiyT7jsgVCq1bkVygt00oASowB7EdtpOHaaPgKt812P9ab+DDKA==", - "os": ["darwin"], - "scripts": true + "@foxglove/cdr@3.5.1": { + "integrity": "sha512-Fubm04/xC1SkeNaviwUhbFk8nAiiNvIqlmP5MW7r6IVGFa35rsA0E3/Rg/pGdE77ZXf4XNwShZcQoEmje62ddQ==" + }, + "@foxglove/message-definition@0.5.0": { + "integrity": "sha512-B/2GvWn4wRSsPDywcmy6uNXgAfZRhiOAnmy3qAaS6MHVqiHQqs/RoDsoW6Gw9sXSweJpeH5qAqAihvm1pR9Thg==" + }, + "@foxglove/rosmsg2-serialization@3.1.2": { + "integrity": "sha512-KKtmHuuIQy6I0JLUi01LFhV7hrZb5xd+LegtuGz2+9L9pftBxtVVKhoxO6B/kQREGocIUOLP8494kxLF58LMWw==", + "dependencies": [ + "@foxglove/cdr", + "@foxglove/message-definition", + "@foxglove/rostime" + ] + }, + "@foxglove/rosmsg@5.0.5": { + "integrity": "sha512-+qY6xW+4HdbPI3ItAlFFMjXgAiplXyRsv+kqdBk8OcWGH38BmEd7s6v/VnCkbYovAoB5rfR5giuKfmPRNzwbrA==", + "dependencies": [ + "@foxglove/message-definition", + "md5-typescript" + ] }, - "playwright-core@1.58.2": { - "integrity": "sha512-yZkEtftgwS8CsfYo7nm0KE8jsvm6i/PTgVtB8DL726wNf6H2IMsDuxCpJj59KDaxCtSnrWan2AeDqM7JBaultg==", + "@foxglove/rostime@1.1.3": { + "integrity": "sha512-Gr3Xy4sNbo2NcXg0tBHb/jVUqIcDyZ1kgHj9dB7K+Y1A7AQ0oSCmSXIuf8oqeteSIs0pl8BJOWEwAtLNC99Izg==" + }, + "md5-typescript@1.0.5": { + "integrity": "sha512-ovAc4EtiNt2dY8JPhPr/wkC9h4U5k/nuClNVcG0Ga3V1rMlYpAY24ZaaymFXJlz+ccJ6UMPo3FSaVKe7czBsXw==" + }, + "playwright-core@1.63.0": { + "integrity": "sha512-rYCsBF/M5HjUch52bbtVONEFjv6Xu8sm8h72dNlR5bzIE1fvC/bxgspzkjSfU+MweEMmPM8KJebG6nnyxo5mCg==", "bin": true }, - "playwright@1.58.2": { - "integrity": "sha512-vA30H8Nvkq/cPBnNw4Q8TWz1EJyqgpuinBcHET0YVJVFldr8JDNiU9LaWAE1KqSkRYazuaBhTpB5ZzShOezQ6A==", + "playwright@1.63.0": { + "integrity": "sha512-+7ziBLidS4NaNCdt57SUDT+wYmmd5fmiQejUic/kb+YsYSCPyOOE9sebzMjNmQrsnNpDJqd4WHvV/8lfKfUDUg==", "dependencies": [ "playwright-core" ], - "optionalDependencies": [ - "fsevents" - ], "bin": true } }, "workspace": { "dependencies": [ - "jsr:@dimos/msgs@~0.1.4", "jsr:@std/http@1", "jsr:@std/path@1", "npm:@dimforge/rapier3d-compat@0.14", + "npm:@foxglove/rosmsg2-serialization@3.1.2", + "npm:@foxglove/rosmsg@5.0.5", "npm:playwright@^1.58.0" ] } diff --git a/misc/DimSim/package-lock.json b/misc/DimSim/package-lock.json index f9164eca1dd..e26eae673d3 100644 --- a/misc/DimSim/package-lock.json +++ b/misc/DimSim/package-lock.json @@ -9,6 +9,8 @@ "version": "1.0.0", "dependencies": { "@dimforge/rapier3d-compat": "^0.14.0", + "@foxglove/rosmsg": "5.0.5", + "@foxglove/rosmsg2-serialization": "3.1.2", "three": "^0.168.0" }, "devDependencies": { @@ -412,6 +414,57 @@ "node": ">=12" } }, + "node_modules/@foxglove/cdr": { + "version": "3.5.1", + "resolved": "https://registry.npmjs.org/@foxglove/cdr/-/cdr-3.5.1.tgz", + "integrity": "sha512-Fubm04/xC1SkeNaviwUhbFk8nAiiNvIqlmP5MW7r6IVGFa35rsA0E3/Rg/pGdE77ZXf4XNwShZcQoEmje62ddQ==", + "license": "MIT", + "engines": { + "node": ">= 14" + } + }, + "node_modules/@foxglove/message-definition": { + "version": "0.5.0", + "resolved": "https://registry.npmjs.org/@foxglove/message-definition/-/message-definition-0.5.0.tgz", + "integrity": "sha512-B/2GvWn4wRSsPDywcmy6uNXgAfZRhiOAnmy3qAaS6MHVqiHQqs/RoDsoW6Gw9sXSweJpeH5qAqAihvm1pR9Thg==", + "license": "MIT" + }, + "node_modules/@foxglove/rosmsg": { + "version": "5.0.5", + "resolved": "https://registry.npmjs.org/@foxglove/rosmsg/-/rosmsg-5.0.5.tgz", + "integrity": "sha512-+qY6xW+4HdbPI3ItAlFFMjXgAiplXyRsv+kqdBk8OcWGH38BmEd7s6v/VnCkbYovAoB5rfR5giuKfmPRNzwbrA==", + "license": "MIT", + "dependencies": { + "@foxglove/message-definition": "0.5.0", + "md5-typescript": "^1.0.5" + }, + "engines": { + "node": ">= 14" + } + }, + "node_modules/@foxglove/rosmsg2-serialization": { + "version": "3.1.2", + "resolved": "https://registry.npmjs.org/@foxglove/rosmsg2-serialization/-/rosmsg2-serialization-3.1.2.tgz", + "integrity": "sha512-KKtmHuuIQy6I0JLUi01LFhV7hrZb5xd+LegtuGz2+9L9pftBxtVVKhoxO6B/kQREGocIUOLP8494kxLF58LMWw==", + "license": "MIT", + "dependencies": { + "@foxglove/cdr": "^3.5.1", + "@foxglove/message-definition": "0.5.0", + "@foxglove/rostime": "^1.1.3" + }, + "engines": { + "node": ">= 20" + } + }, + "node_modules/@foxglove/rostime": { + "version": "1.1.3", + "resolved": "https://registry.npmjs.org/@foxglove/rostime/-/rostime-1.1.3.tgz", + "integrity": "sha512-Gr3Xy4sNbo2NcXg0tBHb/jVUqIcDyZ1kgHj9dB7K+Y1A7AQ0oSCmSXIuf8oqeteSIs0pl8BJOWEwAtLNC99Izg==", + "license": "MIT", + "engines": { + "node": ">= 14" + } + }, "node_modules/@rollup/rollup-android-arm-eabi": { "version": "4.60.4", "resolved": "https://registry.npmjs.org/@rollup/rollup-android-arm-eabi/-/rollup-android-arm-eabi-4.60.4.tgz", @@ -823,6 +876,12 @@ "node": "^8.16.0 || ^10.6.0 || >=11.0.0" } }, + "node_modules/md5-typescript": { + "version": "1.0.5", + "resolved": "https://registry.npmjs.org/md5-typescript/-/md5-typescript-1.0.5.tgz", + "integrity": "sha512-ovAc4EtiNt2dY8JPhPr/wkC9h4U5k/nuClNVcG0Ga3V1rMlYpAY24ZaaymFXJlz+ccJ6UMPo3FSaVKe7czBsXw==", + "license": "ISC" + }, "node_modules/nanoid": { "version": "3.3.12", "resolved": "https://registry.npmjs.org/nanoid/-/nanoid-3.3.12.tgz", diff --git a/misc/DimSim/package.json b/misc/DimSim/package.json index 49c1d7aa615..f52bd5fd14a 100644 --- a/misc/DimSim/package.json +++ b/misc/DimSim/package.json @@ -11,6 +11,8 @@ }, "dependencies": { "@dimforge/rapier3d-compat": "^0.14.0", + "@foxglove/rosmsg": "5.0.5", + "@foxglove/rosmsg2-serialization": "3.1.2", "three": "^0.168.0" }, "devDependencies": { diff --git a/misc/DimSim/src/bridge.ts b/misc/DimSim/src/bridge.ts index eaa2bdf36e4..a12050f9954 100644 --- a/misc/DimSim/src/bridge.ts +++ b/misc/DimSim/src/bridge.ts @@ -8,20 +8,15 @@ * wsRgb → /color_image * wsDepth → /depth_image * - * Sensor messages are LCM-encoded binary packets using @dimos/msgs, sent over + * Sensor messages carry canonical CDR values inside raw LCM packets, sent over * WebSocket to the bridge server which relays them to dimos via LCM/UDP. */ -// @ts-ignore — CDN import (runs in browser, no Deno/Node type resolution) -import { - encodePacket, - sensor_msgs, - std_msgs, -} from "https://esm.sh/jsr/@dimos/msgs@0.1.4"; +import { channel, headerNow, sensorPacket } from "../cli/bridge/cdr.ts"; // -- Channels ---------------------------------------------------------------- -const CH_IMAGE = "/color_image#sensor_msgs.Image"; -const CH_DEPTH = "/depth_image#sensor_msgs.Image"; +const CH_IMAGE = channel("/color_image", "sensor_msgs/msg/Image"); +const CH_DEPTH = channel("/depth_image", "sensor_msgs/msg/Image"); // -- Default publish rates (ms) ---------------------------------------------- const DEFAULT_RATES: PublishRates = { images: 200 }; // 5 Hz images @@ -214,11 +209,7 @@ export class DimosBridge { } _makeHeader(frameId: string): any { - const now = Date.now(); - return new std_msgs.Header({ - stamp: new std_msgs.Time({ sec: Math.floor(now / 1000), nsec: (now % 1000) * 1_000_000 }), - frame_id: frameId, - }); + return headerNow(frameId); } _publishImages(): void { @@ -236,7 +227,7 @@ export class DimosBridge { /** Send on a sensor WebSocket (images — large data). */ _sendSensor(ws: WebSocket | null, channel: string, msg: any): void { if (!ws || ws.readyState !== WebSocket.OPEN) return; - ws.send(encodePacket(channel, msg)); + ws.send(sensorPacket(channel, msg)); } // -- RGB -------------------------------------------------------------------- @@ -247,16 +238,15 @@ export class DimosBridge { const frame = this.sensors.captureRgb(); if (!frame) return; - this._sendSensor(this.wsRgb, CH_IMAGE, new sensor_msgs.Image({ + this._sendSensor(this.wsRgb, CH_IMAGE, { header, height: frame.height, width: frame.width, - encoding: "jpeg", + encoding: "rgba8", is_bigendian: 0, - step: 0, // not applicable for compressed format - data_length: frame.data.length, + step: frame.width * 4, data: frame.data, - })); + }); } catch (e) { console.warn("[DimosBridge] RGB publish error:", e); } @@ -285,16 +275,15 @@ export class DimosBridge { } const depthBytes = new Uint8Array(u16.buffer, u16.byteOffset, u16.byteLength); - this._sendSensor(this.wsDepth, CH_DEPTH, new sensor_msgs.Image({ + this._sendSensor(this.wsDepth, CH_DEPTH, { header, height: frame.height, width: frame.width, encoding: "16UC1", is_bigendian: 0, step: frame.width * 2, - data_length: depthBytes.length, data: depthBytes, - })); + }); } catch (e) { console.warn("[DimosBridge] depth publish error:", e); } diff --git a/misc/DimSim/src/engine.js b/misc/DimSim/src/engine.js index 6e02f8f78cc..6184e347ee9 100644 --- a/misc/DimSim/src/engine.js +++ b/misc/DimSim/src/engine.js @@ -5774,16 +5774,12 @@ if (dimosMode) { captureRgb: () => { const frame = _dimosCaptureRgb(); if (!frame) return null; - // Render to canvas → JPEG (used for both LCM publish and eval/sidebar) + // Keep the eval/sidebar JPEG; publish original pixels as canonical RGBA8. _dimosCapCtx.putImageData(new ImageData(new Uint8ClampedArray(frame.data.buffer, frame.data.byteOffset, frame.data.byteLength), frame.width, frame.height), 0, 0); const dataUrl = _dimosCapCvs.toDataURL("image/jpeg", 0.75); _lastRgbBase64 = dataUrl.split("base64,")[1] || null; if (!_lastRgbBase64) return null; - // Decode base64 → Uint8Array for JPEG LCM transport - const bin = atob(_lastRgbBase64); - const jpegBytes = new Uint8Array(bin.length); - for (let i = 0; i < bin.length; i++) jpegBytes[i] = bin.charCodeAt(i); - return { data: jpegBytes, width: frame.width, height: frame.height }; + return { data: frame.data, width: frame.width, height: frame.height }; }, captureDepth: () => _dimosCaptureDepth(), }, diff --git a/native/python/lerobot/dimos_lerobot/runtime.py b/native/python/lerobot/dimos_lerobot/runtime.py index 1751cfe45e1..052ca497338 100644 --- a/native/python/lerobot/dimos_lerobot/runtime.py +++ b/native/python/lerobot/dimos_lerobot/runtime.py @@ -17,6 +17,7 @@ from __future__ import annotations from contextlib import nullcontext +from copy import deepcopy from dataclasses import dataclass from threading import Condition, Event, RLock, Thread, current_thread import time @@ -44,8 +45,10 @@ LeRobotPolicyModule, RolloutStatus, ) -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.message_utils.image import image_view +from dimos.message_utils.time import to_seconds +from dimos.messages.sensor_msgs.msg import Image, JointState +from dimos.messages.std_msgs.msg import UInt32 from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint from dimos.teleop.webxr.controller_types import BUTTON_ALIASES, Buttons @@ -217,35 +220,46 @@ def _status_locked(self) -> RolloutStatus: } def _on_color_image(self, image: Image) -> None: - if image.format != ImageFormat.RGB or image.data.dtype != np.uint8: + if image.encoding != "rgb8": logger.warning("Ignoring non-uint8 RGB policy image", image=str(image)) return + try: + pixels = image_view(image) + except ValueError as error: + logger.warning("Ignoring malformed policy image", error=str(error)) + return expected_shape = (self.config.image_height, self.config.image_width, 3) - if image.data.shape != expected_shape: + if pixels.shape != expected_shape: logger.warning( "Ignoring policy image with unexpected shape", - shape=image.data.shape, + shape=pixels.shape, expected=expected_shape, ) return with self._lock: - self._latest_image = (np.ascontiguousarray(image.data), image.ts) + self._latest_image = ( + np.array(pixels, copy=True, order="C"), + to_seconds(image.header.stamp), + ) def _on_joint_state(self, state: JointState) -> None: with self._observation_changed: - self._latest_joint_state = JointState(state) + self._latest_joint_state = deepcopy(state) self._observation_changed.notify_all() - def _on_teleop_buttons(self, buttons: Buttons) -> None: + def _on_teleop_buttons(self, buttons: UInt32) -> None: with self._observation_changed: - self._manual_control = buttons.left_grip or buttons.right_grip + self._manual_control = bool( + buttons.data + & ((1 << Buttons.BITS["left_grip"]) | (1 << Buttons.BITS["right_grip"])) + ) if self._manual_control and self._active: self._stop_event.set() self._observation_changed.notify_all() - def _on_button_pressed(self, buttons: Buttons) -> None: + def _on_button_pressed(self, buttons: UInt32) -> None: button = BUTTON_ALIASES.get(self.config.rollout_button, self.config.rollout_button) - if not bool(getattr(buttons, button)): + if not (buttons.data & (1 << Buttons.BITS[button])): return with self._lock: active = self._active @@ -267,8 +281,9 @@ def _snapshot_observation( max_age = self.config.max_observation_age_s if now - image_ts > max_age: raise RuntimeError(f"camera image is stale by {now - image_ts:.2f}s") - if now - state.ts > max_age: - raise RuntimeError(f"joint state is stale by {now - state.ts:.2f}s") + state_ts = to_seconds(state.header.stamp) + if now - state_ts > max_age: + raise RuntimeError(f"joint state is stale by {now - state_ts:.2f}s") positions = dict(zip(state.name, state.position, strict=False)) missing = [name for name in self.config.joint_names if name not in positions] @@ -280,7 +295,7 @@ def _snapshot_observation( ) if not np.all(np.isfinite(vector)): raise RuntimeError("joint state contains non-finite positions") - return image.copy(), vector, state.ts + return image.copy(), vector, state_ts def _load_policy(self) -> _LoadedPolicy: register_third_party_plugins() @@ -516,10 +531,12 @@ def _trajectory( def _wait_for_newer_joint_state(self, previous_ts: float) -> None: with self._observation_changed: self._observation_changed.wait_for( - lambda: self._stop_event.is_set() - or ( - self._latest_joint_state is not None - and self._latest_joint_state.ts > previous_ts + lambda: ( + self._stop_event.is_set() + or ( + self._latest_joint_state is not None + and to_seconds(self._latest_joint_state.header.stamp) > previous_ts + ) ) ) diff --git a/native/python/lerobot/dimos_lerobot/runtime_tests.py b/native/python/lerobot/dimos_lerobot/runtime_tests.py index f18e66c1adc..2d7293aa555 100644 --- a/native/python/lerobot/dimos_lerobot/runtime_tests.py +++ b/native/python/lerobot/dimos_lerobot/runtime_tests.py @@ -15,6 +15,7 @@ """Behavior tests for the isolated LeRobot runtime.""" from collections.abc import Callable, Iterator +from contextlib import ExitStack from threading import Event, Thread import time from typing import Any, Protocol @@ -35,9 +36,12 @@ TrajectoryExecutionResult, TrajectoryExecutionStatus, ) -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.protocol.rpc.pubsubrpc import LCMRPC +from dimos.core.transport import LCMTransport, ZenohTransport +from dimos.message_utils.image import image_from_array +from dimos.message_utils.time import time_from_seconds +from dimos.messages.builtin_interfaces.msg import Time +from dimos.messages.sensor_msgs.msg import Image, JointState +from dimos.messages.std_msgs.msg import Header, UInt32 from dimos.teleop.webxr.controller_types import Buttons from dimos.utils.testing.waiting import wait_until @@ -134,11 +138,6 @@ def __call__( @pytest.fixture def make_runtime(mocker: pytest_mock.MockerFixture) -> Iterator[RuntimeFactory]: - mocker.patch("dimos.core.module.get_loop", return_value=(mocker.MagicMock(), None)) - mocker.patch.object(LCMRPC, "__init__", return_value=None) - mocker.patch.object(LCMRPC, "serve_module_rpc", return_value=None) - mocker.patch.object(LCMRPC, "start", return_value=None) - mocker.patch.object(LCMRPC, "stop", return_value=None) built: list[LeRobotPolicyRuntime] = [] def _make( @@ -217,6 +216,16 @@ def _action_chunk(steps: int = 3) -> NDArray[np.float32]: ) / np.float32(10) +def _state(timestamp, names, positions): + return JointState( + Header(time_from_seconds(timestamp), "arm"), + list(names), + np.asarray(positions, dtype=np.float64), + np.empty(0), + np.empty(0), + ) + + def _provide_observation( module: LeRobotPolicyRuntime, *, @@ -229,8 +238,12 @@ def _provide_observation( rgb[..., 2] = 30 values = positions or [float(i) / 10 for i in range(len(JOINTS))] timestamp = time.time() if ts is None else ts - module._on_color_image(Image(data=rgb, format=ImageFormat.RGB, ts=timestamp)) - module._on_joint_state(JointState(ts=timestamp, name=JOINTS, position=values)) + module._on_color_image( + image_from_array( + rgb, encoding="rgb8", header=Header(time_from_seconds(timestamp), "camera") + ) + ) + module._on_joint_state(_state(timestamp, JOINTS, values)) return rgb, values, timestamp @@ -366,10 +379,10 @@ def execute_trajectory(*_args: object, **_kwargs: object) -> TrajectoryExecution pressed.right_primary = True def stop_from_button() -> None: - module._on_button_pressed(pressed) + module._on_button_pressed(UInt32(pressed.data)) stop_finished.set() - module._on_button_pressed(pressed) + module._on_button_pressed(UInt32(pressed.data)) assert execute_started.wait(timeout=1.0) stop_thread = Thread(target=stop_from_button) stop_thread.start() @@ -582,9 +595,13 @@ def test_preflight_rejects_invalid_live_joints( module, control = make_runtime(policy) timestamp = time.time() module._on_color_image( - Image(data=np.zeros((4, 5, 3), dtype=np.uint8), format=ImageFormat.RGB, ts=timestamp) + image_from_array( + np.zeros((4, 5, 3), dtype=np.uint8), + encoding="rgb8", + header=Header(time_from_seconds(timestamp), "camera"), + ) ) - module._on_joint_state(JointState(ts=timestamp, name=names, position=positions)) + module._on_joint_state(_state(timestamp, names, positions)) status = module.preflight_rollout() @@ -596,20 +613,24 @@ def test_preflight_rejects_invalid_live_joints( @pytest.mark.parametrize( ("image", "image_format"), [ - (np.zeros((8, 8, 3), dtype=np.uint8), ImageFormat.RGB), - (np.zeros((4, 5, 3), dtype=np.uint8), ImageFormat.BGR), + (np.zeros((8, 8, 3), dtype=np.uint8), "rgb8"), + (np.zeros((4, 5, 3), dtype=np.uint8), "bgr8"), ], ) def test_preflight_requires_exact_live_rgb_contract( make_runtime: RuntimeFactory, image: NDArray[np.uint8], - image_format: ImageFormat, + image_format: str, ) -> None: policy = FakePolicy(_action_chunk()) module, control = make_runtime(policy) timestamp = time.time() - module._on_color_image(Image(data=image, format=image_format, ts=timestamp)) - module._on_joint_state(JointState(ts=timestamp, name=JOINTS, position=[0.0] * len(JOINTS))) + module._on_color_image( + image_from_array( + image, encoding=image_format, header=Header(time_from_seconds(timestamp), "camera") + ) + ) + module._on_joint_state(_state(timestamp, JOINTS, [0.0] * len(JOINTS))) status = module.preflight_rollout() @@ -671,12 +692,12 @@ def test_grip_takeover_stops_rollout_until_explicit_restart( held = Buttons() setattr(held, grip, True) - module._on_teleop_buttons(held) + module._on_teleop_buttons(UInt32(held.data)) wait_until(lambda: not module.rollout_status()["active"], timeout=1) assert module.start_rollout()["active"] is False assert "release" in (module.rollout_status()["last_error"] or "") submissions = control.execute_trajectory.call_count - module._on_teleop_buttons(Buttons()) + module._on_teleop_buttons(UInt32(0)) assert module.rollout_status()["active"] is False assert control.execute_trajectory.call_count == submissions _provide_observation(module) @@ -704,8 +725,8 @@ def predict(*args: Any, **kwargs: Any) -> NDArray[np.float32]: assert predicting.wait(timeout=1) held = Buttons() held.right_grip = True - module._on_teleop_buttons(held) - module._on_teleop_buttons(Buttons()) + module._on_teleop_buttons(UInt32(held.data)) + module._on_teleop_buttons(UInt32(0)) assert module._stop_event.is_set() control.execute_trajectory.assert_not_called() finally: @@ -722,5 +743,85 @@ def test_stopping_inactive_policy_leaves_planner_trajectory_alone( module.stop_rollout() held = Buttons() held.right_grip = True - module._on_teleop_buttons(held) + module._on_teleop_buttons(UInt32(held.data)) control.cancel_trajectory.assert_not_called() + + +@pytest.mark.parametrize("backend", ["lcm", "zenoh"]) +def test_runtime_consumes_actual_cdr_ports_with_padded_pixels_and_owned_state( + make_runtime, + backend, + mocker, +): + policy = FakePolicy(_action_chunk()) + module, control = make_runtime(policy) + epoch = 1_700_000_000 + mocker.patch.object(policy_runtime.time, "time", return_value=float(epoch)) + options = ( + {"url": "memq://"} + if backend == "lcm" + else { + "scouting": False, + "multicast": False, + "gossip": False, + "connect": [], + "listen": ["tcp/127.0.0.1:0"], + } + ) + with ExitStack() as cleanup: + wires = {} + for name, kind in [ + ("color_image", Image), + ("coordinator_joint_state", JointState), + ("button_pressed", UInt32), + ("teleop_buttons", UInt32), + ]: + wire = ( + LCMTransport("/policy/" + name, kind, **options) + if backend == "lcm" + else ZenohTransport("/policy/" + name, kind, **options) + ) + cleanup.callback(wire.stop) + wires[name] = wire + module.set_transport(name, wire) + module.start() + pixels = np.full((4, 17), 99, dtype=np.uint8) + pixels[:, :15] = np.arange(60, dtype=np.uint8).reshape(4, 15) + image = Image(Header(Time(epoch, 1), "camera"), 4, 5, "rgb8", 0, 17, pixels.ravel()) + state = _state(epoch, JOINTS, [0.0, 0.1, 0.2, 0.3]) + state.header.stamp.nanosec = 1 + wires["color_image"].publish(image) + wires["coordinator_joint_state"].publish(state) + wait_until(lambda: module.rollout_status()["observations_ready"], timeout=5) + observed, joints, _ = module._snapshot_observation(epoch) + np.testing.assert_array_equal(observed, np.arange(60, dtype=np.uint8).reshape(4, 5, 3)) + np.testing.assert_allclose(joints, [0.0, 0.1, 0.2, 0.3]) + assert module._latest_joint_state.header.stamp.nanosec == 1 + # Direct callback ownership also holds before the transport has copied anything. + module._on_color_image(image) + module._on_joint_state(state) + image.data[:] = 0 + state.position[:] = 0 + again, joints, _ = module._snapshot_observation(epoch) + np.testing.assert_array_equal(again, observed) + np.testing.assert_allclose(joints, [0.0, 0.1, 0.2, 0.3]) + wires["teleop_buttons"].publish(UInt32(1 << Buttons.BITS["left_grip"])) + wait_until(lambda: module._manual_control, timeout=5) + control.execute_trajectory.assert_not_called() + + +def test_malformed_canonical_rgb_does_not_replace_valid_policy_observation(make_runtime): + module, _ = make_runtime(FakePolicy(_action_chunk())) + pixels, _, timestamp = _provide_observation(module) + malformed = Image( + Header(time_from_seconds(timestamp), "camera"), + 4, + 5, + "rgb8", + 0, + 15, + np.zeros(1, dtype=np.uint8), + ) + module._on_color_image(malformed) + observed, _, _ = module._snapshot_observation(timestamp) + np.testing.assert_array_equal(observed, pixels)