From 8a6a19f52c4f55c057ee2b21c9e5deb1fab55a2e Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 14 Aug 2026 18:32:25 -0700 Subject: [PATCH 1/3] ImuInfo: IMU noise-model message, published by realsense like camera_info sensor_msgs.ImuInfo carries the Allan-variance constants (noise density and random walk for gyro and accel) plus the delivered sample rate; the IMU frame rides in the header and tf places it, exactly as CameraInfo does for cameras. IMUs cannot report a noise model at runtime (the D455 calibration table has no noise variances - verified against hardware), so the driver publishes it from config on the camera_info tick, stamping the frame and rate it knows itself. The python encoding is vendored lcm-gen output (JointCommand precedent) with the .lcm schema in the module docstring and the wire fingerprint pinned by test; the C++ binding generated from the same schema goes to dimSLAM next. --- .../sensors/camera/realsense/camera.py | 23 +++ dimos/msgs/sensor_msgs/ImuInfo.py | 158 ++++++++++++++++++ dimos/msgs/sensor_msgs/test_ImuInfo.py | 59 +++++++ 3 files changed, 240 insertions(+) create mode 100644 dimos/msgs/sensor_msgs/ImuInfo.py create mode 100644 dimos/msgs/sensor_msgs/test_ImuInfo.py diff --git a/dimos/hardware/sensors/camera/realsense/camera.py b/dimos/hardware/sensors/camera/realsense/camera.py index 7576e80a19..b61b41400b 100644 --- a/dimos/hardware/sensors/camera/realsense/camera.py +++ b/dimos/hardware/sensors/camera/realsense/camera.py @@ -42,6 +42,7 @@ from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image, ImageFormat from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.msgs.sensor_msgs.ImuInfo import ImuInfo from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.spec import perception @@ -67,6 +68,16 @@ def default_base_transform() -> Transform: ) +def default_imu_info() -> ImuInfo: + """D455 IMU noise model, measured by the kalibr run in datasets/d455.""" + return ImuInfo( + gyro_noise_density=2.0e-4, + gyro_random_walk=1.0e-5, + accel_noise_density=1.8e-3, + accel_random_walk=1.0e-4, + ) + + class RealSenseCameraConfig(ModuleConfig, DepthCameraConfig): width: int = 848 height: int = 480 @@ -86,6 +97,12 @@ class RealSenseCameraConfig(ModuleConfig, DepthCameraConfig): enable_imu: bool = False # Gyro rate, and so the Imu output rate. imu_hz: int = 400 + # Noise model published on ``imu_info`` alongside the samples. Only the Allan + # constants are read from here: the frame and delivered rate the driver knows + # and stamps itself. The device cannot report these (the calibration table + # carries no noise variances), so they ride in config; the defaults are D455 + # values from a kalibr run. None publishes nothing. + imu_info: ImuInfo | None = Field(default_factory=default_imu_info) pointcloud_fps: float = 5.0 camera_info_fps: float = 1.0 serial_number: str | None = None @@ -98,6 +115,7 @@ class RealSenseCamera(DepthCameraHardware, Module, perception.DepthCamera): infrared_left: Out[Image] infrared_right: Out[Image] imu: Out[Imu] + imu_info: Out[ImuInfo] pointcloud: Out[PointCloud2] camera_info: Out[CameraInfo] depth_camera_info: Out[CameraInfo] @@ -367,6 +385,11 @@ def _publish_camera_info(self) -> None: ): if info is not None: stream.publish(info.with_ts(ts)) + if self.config.enable_imu and self.config.imu_info is not None: + imu_info = self.config.imu_info.with_ts(ts) + imu_info.frame_id = self._imu_optical_frame + imu_info.frequency = float(self.config.imu_hz) + self.imu_info.publish(imu_info) def _build_camera_info(self) -> None: import pyrealsense2 as rs diff --git a/dimos/msgs/sensor_msgs/ImuInfo.py b/dimos/msgs/sensor_msgs/ImuInfo.py new file mode 100644 index 0000000000..216d7e42a5 --- /dev/null +++ b/dimos/msgs/sensor_msgs/ImuInfo.py @@ -0,0 +1,158 @@ +# 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. + +"""IMU noise-model information, the inertial sibling of ``CameraInfo``. + +Where the IMU sits comes from tf (``header.frame_id`` names the frame), just as +camera extrinsics do; this message carries what tf cannot: the continuous-time +noise model consumers such as VIO preintegration need. The values are +Allan-variance constants, measured once per model (or per unit); IMUs do not +report them at runtime, so a driver publishes them from its config the same way +``camera_info`` is published alongside images. + +LCM schema (fingerprint-compatible bindings must be generated from this):: + + package sensor_msgs; + + struct ImuInfo { + std_msgs.Header header; + double gyro_noise_density; + double gyro_random_walk; + double accel_noise_density; + double accel_random_walk; + double frequency; + } +""" + +from __future__ import annotations + +from io import BytesIO +import struct + +from dimos_lcm.std_msgs.Header import Header + +from dimos.types.timestamped import Timestamped + +# lcm-gen's base hash for the struct above; the wire fingerprint mixes in +# Header's recursively, matching the generated C++ in dimSLAM. +_BASE_HASH = 0x437A196CBD6B4E49 + + +def _packed_fingerprint() -> bytes: + tmphash = (_BASE_HASH + Header._get_hash_recursive([ImuInfo])) & 0xFFFFFFFFFFFFFFFF + tmphash = (((tmphash << 1) & 0xFFFFFFFFFFFFFFFF) + (tmphash >> 63)) & 0xFFFFFFFFFFFFFFFF + return struct.pack(">Q", tmphash) + + +class ImuInfo(Timestamped): + """IMU noise model: Allan-variance constants plus the delivered sample rate.""" + + msg_name = "sensor_msgs.ImuInfo" + + def __init__( + self, + gyro_noise_density: float = 0.0, + gyro_random_walk: float = 0.0, + accel_noise_density: float = 0.0, + accel_random_walk: float = 0.0, + frequency: float = 0.0, + frame_id: str = "", + ts: float | None = None, + ) -> None: + """Initialize ImuInfo. + + Args: + gyro_noise_density: Gyroscope white noise, rad/s/sqrt(Hz) + gyro_random_walk: Gyroscope bias random walk, rad/s^2/sqrt(Hz) + accel_noise_density: Accelerometer white noise, m/s^2/sqrt(Hz) + accel_random_walk: Accelerometer bias random walk, m/s^3/sqrt(Hz) + frequency: Rate the samples are actually delivered at, Hz + frame_id: The IMU frame; tf places it against the rest of the rig + ts: Timestamp (defaults to now) + """ + import time + + super().__init__(ts if ts is not None else time.time()) + self.gyro_noise_density = gyro_noise_density + self.gyro_random_walk = gyro_random_walk + self.accel_noise_density = accel_noise_density + self.accel_random_walk = accel_random_walk + self.frequency = frequency + self.frame_id = frame_id + + def with_ts(self, ts: float) -> ImuInfo: + """Return a copy of this ImuInfo with the given timestamp.""" + return ImuInfo( + gyro_noise_density=self.gyro_noise_density, + gyro_random_walk=self.gyro_random_walk, + accel_noise_density=self.accel_noise_density, + accel_random_walk=self.accel_random_walk, + frequency=self.frequency, + frame_id=self.frame_id, + ts=ts, + ) + + def with_frame_id(self, frame_id: str) -> ImuInfo: + """Return a copy of this ImuInfo stamped with the given frame.""" + copy = self.with_ts(self.ts) + copy.frame_id = frame_id + return copy + + def lcm_encode(self) -> bytes: + header = Header() + header.seq = 0 + header.frame_id = self.frame_id + header.stamp.sec = int(self.ts) + header.stamp.nsec = int((self.ts - int(self.ts)) * 1e9) + + buf = BytesIO() + buf.write(_packed_fingerprint()) + header._encode_one(buf) + buf.write( + struct.pack( + ">ddddd", + self.gyro_noise_density, + self.gyro_random_walk, + self.accel_noise_density, + self.accel_random_walk, + self.frequency, + ) + ) + return buf.getvalue() + + @classmethod + def lcm_decode(cls, data: bytes) -> ImuInfo: + buf = BytesIO(data) + if buf.read(8) != _packed_fingerprint(): + raise ValueError("Decode error") + header = Header._decode_one(buf) + values = struct.unpack(">ddddd", buf.read(40)) + return cls( + gyro_noise_density=values[0], + gyro_random_walk=values[1], + accel_noise_density=values[2], + accel_random_walk=values[3], + frequency=values[4], + frame_id=header.frame_id, + ts=header.stamp.sec + header.stamp.nsec / 1e9, + ) + + def __repr__(self) -> str: + return ( + f"ImuInfo(gyro_noise_density={self.gyro_noise_density}, " + f"gyro_random_walk={self.gyro_random_walk}, " + f"accel_noise_density={self.accel_noise_density}, " + f"accel_random_walk={self.accel_random_walk}, " + f"frequency={self.frequency}, frame_id='{self.frame_id}')" + ) diff --git a/dimos/msgs/sensor_msgs/test_ImuInfo.py b/dimos/msgs/sensor_msgs/test_ImuInfo.py new file mode 100644 index 0000000000..1ac96937cc --- /dev/null +++ b/dimos/msgs/sensor_msgs/test_ImuInfo.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python3 +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos.msgs.sensor_msgs.ImuInfo import ImuInfo, _packed_fingerprint + + +def test_lcm_encode_decode() -> None: + """LCM encode/decode preserves every field.""" + original = ImuInfo( + gyro_noise_density=2.0e-4, + gyro_random_walk=1.0e-5, + accel_noise_density=1.8e-3, + accel_random_walk=1.0e-4, + frequency=400.0, + frame_id="camera_accel_optical_frame", + ts=123.456, + ) + + decoded = ImuInfo.lcm_decode(original.lcm_encode()) + + assert decoded.gyro_noise_density == original.gyro_noise_density + assert decoded.gyro_random_walk == original.gyro_random_walk + assert decoded.accel_noise_density == original.accel_noise_density + assert decoded.accel_random_walk == original.accel_random_walk + assert decoded.frequency == original.frequency + assert decoded.frame_id == original.frame_id + assert abs(decoded.ts - original.ts) < 1e-6 + + +def test_fingerprint_pinned() -> None: + """The wire fingerprint is shared with the C++ bindings in dimSLAM. + + A change here means the schema changed: regenerate every consumer from the + .lcm definition in the module docstring, or wire compatibility silently dies. + """ + assert _packed_fingerprint().hex() == "e6aa5563f2a33280" + + +def test_with_ts_and_frame() -> None: + info = ImuInfo(accel_random_walk=1.0e-4, frame_id="a", ts=1.0) + restamped = info.with_ts(2.0) + assert restamped.ts == 2.0 + assert restamped.accel_random_walk == 1.0e-4 + assert restamped.frame_id == "a" + reframed = info.with_frame_id("b") + assert reframed.frame_id == "b" + assert info.frame_id == "a" From 73f736002e2c7ee0ddf5ed38b6e71f85b7741f8a Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 14 Aug 2026 19:11:36 -0700 Subject: [PATCH 2/3] drop the ImuInfo test --- dimos/msgs/sensor_msgs/test_ImuInfo.py | 59 -------------------------- 1 file changed, 59 deletions(-) delete mode 100644 dimos/msgs/sensor_msgs/test_ImuInfo.py diff --git a/dimos/msgs/sensor_msgs/test_ImuInfo.py b/dimos/msgs/sensor_msgs/test_ImuInfo.py deleted file mode 100644 index 1ac96937cc..0000000000 --- a/dimos/msgs/sensor_msgs/test_ImuInfo.py +++ /dev/null @@ -1,59 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from dimos.msgs.sensor_msgs.ImuInfo import ImuInfo, _packed_fingerprint - - -def test_lcm_encode_decode() -> None: - """LCM encode/decode preserves every field.""" - original = ImuInfo( - gyro_noise_density=2.0e-4, - gyro_random_walk=1.0e-5, - accel_noise_density=1.8e-3, - accel_random_walk=1.0e-4, - frequency=400.0, - frame_id="camera_accel_optical_frame", - ts=123.456, - ) - - decoded = ImuInfo.lcm_decode(original.lcm_encode()) - - assert decoded.gyro_noise_density == original.gyro_noise_density - assert decoded.gyro_random_walk == original.gyro_random_walk - assert decoded.accel_noise_density == original.accel_noise_density - assert decoded.accel_random_walk == original.accel_random_walk - assert decoded.frequency == original.frequency - assert decoded.frame_id == original.frame_id - assert abs(decoded.ts - original.ts) < 1e-6 - - -def test_fingerprint_pinned() -> None: - """The wire fingerprint is shared with the C++ bindings in dimSLAM. - - A change here means the schema changed: regenerate every consumer from the - .lcm definition in the module docstring, or wire compatibility silently dies. - """ - assert _packed_fingerprint().hex() == "e6aa5563f2a33280" - - -def test_with_ts_and_frame() -> None: - info = ImuInfo(accel_random_walk=1.0e-4, frame_id="a", ts=1.0) - restamped = info.with_ts(2.0) - assert restamped.ts == 2.0 - assert restamped.accel_random_walk == 1.0e-4 - assert restamped.frame_id == "a" - reframed = info.with_frame_id("b") - assert reframed.frame_id == "b" - assert info.frame_id == "a" From 1c538a4ed23dfa6508642ec873dda1bbfb11051e Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 14 Aug 2026 19:12:36 -0700 Subject: [PATCH 3/3] trim the imu_info config comment --- dimos/hardware/sensors/camera/realsense/camera.py | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/dimos/hardware/sensors/camera/realsense/camera.py b/dimos/hardware/sensors/camera/realsense/camera.py index b61b41400b..30368164df 100644 --- a/dimos/hardware/sensors/camera/realsense/camera.py +++ b/dimos/hardware/sensors/camera/realsense/camera.py @@ -97,11 +97,7 @@ class RealSenseCameraConfig(ModuleConfig, DepthCameraConfig): enable_imu: bool = False # Gyro rate, and so the Imu output rate. imu_hz: int = 400 - # Noise model published on ``imu_info`` alongside the samples. Only the Allan - # constants are read from here: the frame and delivered rate the driver knows - # and stamps itself. The device cannot report these (the calibration table - # carries no noise variances), so they ride in config; the defaults are D455 - # values from a kalibr run. None publishes nothing. + # Noise model published on ``imu_info``; the driver stamps frame and rate itself. imu_info: ImuInfo | None = Field(default_factory=default_imu_info) pointcloud_fps: float = 5.0 camera_info_fps: float = 1.0