ImuInfo like CameraInfo

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,8 @@ 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``; 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
     serial_number: str | None = None
@@ -98,6 +111,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 +381,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

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}')"
+        )