diff --git a/dimos/hardware/sensors/camera/realsense/camera.py b/dimos/hardware/sensors/camera/realsense/camera.py index 228c0b63f8..30368164df 100644 --- a/dimos/hardware/sensors/camera/realsense/camera.py +++ b/dimos/hardware/sensors/camera/realsense/camera.py @@ -15,6 +15,7 @@ from __future__ import annotations import atexit +from collections import deque import threading import time from typing import TYPE_CHECKING @@ -40,11 +41,21 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 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 +from dimos.utils.logging_config import setup_logger from dimos.utils.reactive import backpressure +logger = setup_logger() + + +def ms_to_s(milliseconds: float) -> float: + return milliseconds / 1000.0 + + if TYPE_CHECKING: import pyrealsense2 as rs # type: ignore[import-not-found,import-untyped] @@ -57,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 @@ -66,7 +87,18 @@ class RealSenseCameraConfig(ModuleConfig, DepthCameraConfig): base_transform: Transform | None = Field(default_factory=default_base_transform) align_depth_to_color: bool = True enable_depth: bool = True + enable_color: bool = True + # On, auto-exposure halves the color frame rate to lengthen exposure. + color_auto_exposure_priority: bool = False enable_pointcloud: bool = False + enable_infrared: bool = False + # Dots make depth much better and the IR pair useless for feature tracking. + emitter_enabled: bool = True + 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 @@ -76,9 +108,15 @@ class RealSenseCamera(DepthCameraHardware, Module, perception.DepthCamera): config: RealSenseCameraConfig color_image: Out[Image] depth_image: Out[Image] + 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] + infrared_left_camera_info: Out[CameraInfo] + infrared_right_camera_info: Out[CameraInfo] tf: Out[TFMessage] @property @@ -101,17 +139,51 @@ def _depth_frame(self) -> str: def _depth_optical_frame(self) -> str: return f"{self.config.camera_name}_depth_optical_frame" + @property + def _infra1_frame(self) -> str: + return f"{self.config.camera_name}_infra1_frame" + + @property + def _infra1_optical_frame(self) -> str: + return f"{self.config.camera_name}_infra1_optical_frame" + + @property + def _infra2_frame(self) -> str: + return f"{self.config.camera_name}_infra2_frame" + + @property + def _infra2_optical_frame(self) -> str: + return f"{self.config.camera_name}_infra2_optical_frame" + + @property + def _imu_frame(self) -> str: + return f"{self.config.camera_name}_accel_frame" + + @property + def _imu_optical_frame(self) -> str: + # accel and gyro are co-located on the motion module. + return f"{self.config.camera_name}_accel_optical_frame" + def __init__(self, *args, **kwargs) -> None: # type: ignore[no-untyped-def] super().__init__(*args, **kwargs) self._pipeline: rs.pipeline | None = None self._profile: rs.pipeline_profile | None = None + self._imu_pipeline: rs.pipeline | None = None + self._accel_history: deque[tuple[float, tuple[float, float, float]]] = deque(maxlen=2) + self._pending_gyro: deque[tuple[float, tuple[float, float, float]]] = deque(maxlen=16) self._align: rs.align | None = None + self._mount_edges: list[tuple[str, str, Vector3, Quaternion]] = [] self._running = False self._thread: threading.Thread | None = None self._color_camera_info: CameraInfo | None = None self._depth_camera_info: CameraInfo | None = None + self._infra1_camera_info: CameraInfo | None = None + self._infra2_camera_info: CameraInfo | None = None self._depth_scale: float = 0.001 - self._color_to_depth_extrinsics: rs.extrinsics | None = None + self._last_frame_numbers: dict[str, int] = {} + self._last_hardware_ts: float | None = None + self._dropped_frames: dict[str, int] = {} + self._repeated_frames: dict[str, int] = {} # Pointcloud generation state self._latest_color_img: Image | None = None self._latest_depth_img: Image | None = None @@ -127,13 +199,14 @@ def start(self) -> None: if self.config.serial_number: config.enable_device(self.config.serial_number) - config.enable_stream( - rs.stream.color, - self.config.width, - self.config.height, - rs.format.bgr8, - self.config.fps, - ) + if self.config.enable_color: + config.enable_stream( + rs.stream.color, + self.config.width, + self.config.height, + rs.format.bgr8, + self.config.fps, + ) if self.config.enable_depth: config.enable_stream( @@ -144,28 +217,60 @@ def start(self) -> None: self.config.fps, ) + if self.config.enable_infrared: + # index 1 = left imager, index 2 = right + for ir_index in (1, 2): + config.enable_stream( + rs.stream.infrared, + ir_index, + self.config.width, + self.config.height, + rs.format.y8, + self.config.fps, + ) + self._profile = self._pipeline.start(config) + self._require_global_time() - if self.config.enable_depth: + # The IR imagers are the depth sensor, and it owns the emitter option. + if self.config.enable_depth or self.config.enable_infrared: depth_sensor = self._profile.get_device().first_depth_sensor() self._depth_scale = depth_sensor.get_depth_scale() + if depth_sensor.supports(rs.option.emitter_enabled): + depth_sensor.set_option( + rs.option.emitter_enabled, 1.0 if self.config.emitter_enabled else 0.0 + ) + + if self.config.enable_color: + color_sensor = self._profile.get_device().first_color_sensor() + if color_sensor.supports(rs.option.auto_exposure_priority): + color_sensor.set_option( + rs.option.auto_exposure_priority, + 1.0 if self.config.color_auto_exposure_priority else 0.0, + ) if self.config.align_depth_to_color and self.config.enable_depth: - self._align = rs.align(rs.stream.color) + if self.config.enable_color: + self._align = rs.align(rs.stream.color) + else: + logger.info("align_depth_to_color ignored: color stream is disabled") self._build_camera_info() - self._get_extrinsics() + self._build_mount_edges() self._running = True self._thread = threading.Thread(target=self._capture_loop, daemon=True) self._thread.start() + if self.config.enable_imu: + self._start_imu() + if self.config.enable_pointcloud and self.config.enable_depth: interval_sec = 1.0 / self.config.pointcloud_fps self.register_disposable( backpressure(rx.interval(interval_sec)).subscribe( on_next=lambda _: self._generate_pointcloud(), - on_error=lambda e: print(f"Pointcloud error: {e}"), + on_error=lambda error: logger.error("RealSense pointcloud: %s", error), ) ) @@ -173,18 +278,114 @@ def start(self) -> None: self.register_disposable( rx.interval(interval_sec).subscribe( on_next=lambda _: self._publish_camera_info(), - on_error=lambda e: print(f"CameraInfo error: {e}"), + on_error=lambda error: logger.error("RealSense camera_info: %s", error), ) ) + def _start_imu(self) -> None: + """Stream the motion module on its own pipeline and callback.""" + import pyrealsense2 as rs + + offered = self._stream_rates(rs.stream.gyro) + if self.config.imu_hz not in offered: + raise ValueError( + f"imu_hz={self.config.imu_hz} is not offered by this camera; it has {offered}" + ) + accel_hz = max(self._stream_rates(rs.stream.accel)) + + imu_config = rs.config() + if self.config.serial_number: + imu_config.enable_device(self.config.serial_number) + imu_config.enable_stream(rs.stream.accel, rs.format.motion_xyz32f, accel_hz) + imu_config.enable_stream(rs.stream.gyro, rs.format.motion_xyz32f, self.config.imu_hz) + + self._imu_pipeline = rs.pipeline() + self._imu_pipeline.start(imu_config, self._on_motion_frame) + + def _stream_rates(self, stream_type: rs.stream) -> list[int]: + """Every rate the device offers for a stream.""" + if self._profile is None: + return [] + rates = { + profile.fps() + for sensor in self._profile.get_device().query_sensors() + for profile in sensor.get_stream_profiles() + if profile.stream_type() == stream_type + } + return sorted(rates) + + def _require_global_time(self) -> None: + """Put every sensor on the host clock rather than its own boot clock.""" + import pyrealsense2 as rs + + if self._profile is None: + return + for sensor in self._profile.get_device().query_sensors(): + if not sensor.supports(rs.option.global_time_enabled): + logger.warning( + "RealSense %s has no global timestamps, so its stream is on the " + "device's own clock and cannot be fused with the others", + sensor.get_info(rs.camera_info.name), + ) + continue + sensor.set_option(rs.option.global_time_enabled, 1.0) + + def _on_motion_frame(self, frame: rs.frame) -> None: + import pyrealsense2 as rs + + motion = frame.as_motion_frame() + if not motion: + return + data = motion.get_motion_data() + # Hardware capture time, not host time. + ts = ms_to_s(motion.get_timestamp()) + stream = motion.get_profile().stream_type() + if stream == rs.stream.accel: + self._accel_history.append((ts, (data.x, data.y, data.z))) + elif stream == rs.stream.gyro: + self._pending_gyro.append((ts, (data.x, data.y, data.z))) + self._publish_paired_imu() + + def _publish_paired_imu(self) -> None: + """Emit one Imu per gyro sample, with the accelerometer read at that instant. + + The two are sampled independently and never share a timestamp. + """ + while self._pending_gyro and len(self._accel_history) == 2: + (start_ts, start), (end_ts, end) = self._accel_history + ts, angular = self._pending_gyro[0] + if ts > end_ts: + return # no accelerometer sample past it yet + self._pending_gyro.popleft() + span = end_ts - start_ts + ratio = 0.0 if span <= 0.0 else max(0.0, min(1.0, (ts - start_ts) / span)) + linear = tuple(a + (b - a) * ratio for a, b in zip(start, end, strict=True)) + self.imu.publish( + Imu( + angular_velocity=Vector3(*angular), + linear_acceleration=Vector3(*linear), + frame_id=self._imu_optical_frame, + ts=ts, + ) + ) + def _publish_camera_info(self) -> None: - ts = time.time() - if self._color_camera_info: - self._color_camera_info.ts = ts - self.camera_info.publish(self._color_camera_info) - if self._depth_camera_info: - self._depth_camera_info.ts = ts - self.depth_camera_info.publish(self._depth_camera_info) + ts = self._last_hardware_ts + if ts is None or not self._running: + return + for info, stream in ( + (self._color_camera_info, self.camera_info), + (self._depth_camera_info, self.depth_camera_info), + (self._infra1_camera_info, self.infrared_left_camera_info), + (self._infra2_camera_info, self.infrared_right_camera_info), + ): + 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 @@ -193,15 +394,17 @@ def _build_camera_info(self) -> None: return # Color camera info - color_stream = self._profile.get_stream(rs.stream.color).as_video_stream_profile() - color_intrinsics = color_stream.get_intrinsics() - self._color_camera_info = self._intrinsics_to_camera_info( - color_intrinsics, self._color_optical_frame - ) + color_intrinsics = None + if self.config.enable_color: + color_stream = self._profile.get_stream(rs.stream.color).as_video_stream_profile() + color_intrinsics = color_stream.get_intrinsics() + self._color_camera_info = self._intrinsics_to_camera_info( + color_intrinsics, self._color_optical_frame + ) # Depth camera info if self.config.enable_depth: - if self.config.align_depth_to_color: + if self.config.align_depth_to_color and color_intrinsics is not None: # When aligned to color, depth uses color intrinsics and frame self._depth_camera_info = self._intrinsics_to_camera_info( color_intrinsics, self._color_optical_frame @@ -213,17 +416,41 @@ def _build_camera_info(self) -> None: depth_intrinsics, self._depth_optical_frame ) + # Infrared stereo pair camera info + if self.config.enable_infrared: + infra1_stream = self._profile.get_stream( + rs.stream.infrared, 1 + ).as_video_stream_profile() + self._infra1_camera_info = self._intrinsics_to_camera_info( + infra1_stream.get_intrinsics(), self._infra1_optical_frame + ) + infra2_stream = self._profile.get_stream( + rs.stream.infrared, 2 + ).as_video_stream_profile() + self._infra2_camera_info = self._intrinsics_to_camera_info( + infra2_stream.get_intrinsics(), self._infra2_optical_frame + ) + # P[3] is -fx * baseline; left at 0 a stereo consumer sees infinite depth. + # The pair is rectified, so the whole of their offset is along x. + baseline = abs(float(infra2_stream.get_extrinsics_to(infra1_stream).translation[0])) + projection = list(self._infra2_camera_info.P) + projection[3] = -projection[0] * baseline + self._infra2_camera_info.P = projection + def _intrinsics_to_camera_info(self, intrinsics: rs.intrinsics, frame_id: str) -> CameraInfo: import pyrealsense2 as rs - fx, fy = intrinsics.fx, intrinsics.fy - cx, cy = intrinsics.ppx, intrinsics.ppy - - K = [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0] - P = [fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0] - D = list(intrinsics.coeffs) if intrinsics.coeffs else [] - - distortion_model = { + info = CameraInfo.from_intrinsics( + intrinsics.fx, + intrinsics.fy, + intrinsics.ppx, + intrinsics.ppy, + intrinsics.width, + intrinsics.height, + frame_id, + ) + info.D = list(intrinsics.coeffs) if intrinsics.coeffs else [] + info.distortion_model = { rs.distortion.none: "", rs.distortion.modified_brown_conrady: "plumb_bob", rs.distortion.inverse_brown_conrady: "plumb_bob", @@ -231,43 +458,102 @@ def _intrinsics_to_camera_info(self, intrinsics: rs.intrinsics, frame_id: str) - rs.distortion.brown_conrady: "plumb_bob", rs.distortion.kannala_brandt4: "equidistant", }.get(intrinsics.model, "") + return info - return CameraInfo( - height=intrinsics.height, - width=intrinsics.width, - distortion_model=distortion_model, - D=D, - K=K, - P=P, - frame_id=frame_id, - ) - - def _get_extrinsics(self) -> None: + def _device_stream_profile( + self, stream_type: rs.stream, index: int | None = None + ) -> rs.stream_profile | None: + """A profile for one of the device's streams, running or not.""" + if self._profile is None: + return None + for sensor in self._profile.get_device().query_sensors(): + for profile in sensor.get_stream_profiles(): + if profile.stream_type() == stream_type and ( + index is None or profile.stream_index() == index + ): + return profile + return None + + def _build_mount_edges(self) -> None: + """Place every imager below camera_link, which sits on the depth imager.""" import pyrealsense2 as rs - if self._profile is None or not self.config.enable_depth: + streams: list[tuple[str, rs.stream, int | None]] = [ + (self._depth_frame, rs.stream.depth, None) + ] + if self.config.enable_color: + streams.append((self._color_frame, rs.stream.color, None)) + if self.config.enable_infrared: + streams.append((self._infra1_frame, rs.stream.infrared, 1)) + streams.append((self._infra2_frame, rs.stream.infrared, 2)) + if self.config.enable_imu: + streams.append((self._imu_frame, rs.stream.accel, None)) + + origin = self._device_stream_profile(rs.stream.depth, None) + if origin is None: + logger.warning("RealSense has no depth stream; publishing no camera frames on tf") return - depth_stream = self._profile.get_stream(rs.stream.depth) - color_stream = self._profile.get_stream(rs.stream.color) - self._color_to_depth_extrinsics = color_stream.get_extrinsics_to(depth_stream) - - def _extrinsics_to_transform( - self, - extrinsics: rs.extrinsics, - frame_id: str, - child_frame_id: str, - ts: float, - ) -> Transform: - rotation_matrix = np.array(extrinsics.rotation).reshape(3, 3) - quat = Rotation.from_matrix(rotation_matrix).as_quat() # [x, y, z, w] - return Transform( - translation=Vector3(*extrinsics.translation), - rotation=Quaternion(quat[0], quat[1], quat[2], quat[3]), - frame_id=frame_id, - child_frame_id=child_frame_id, - ts=ts, - ) + self._mount_edges = [] + for frame_id, stream_type, index in streams: + profile = self._device_stream_profile(stream_type, index) + if profile is None: + logger.warning("RealSense has no stream for %s; not published on tf", frame_id) + continue + translation, rotation = self._extrinsics_to_body(profile.get_extrinsics_to(origin)) + self._mount_edges.append((self._camera_link, frame_id, translation, rotation)) + self._mount_edges.append( + ( + frame_id, + f"{frame_id.removesuffix('_frame')}_optical_frame", + Vector3(0.0, 0.0, 0.0), + OPTICAL_ROTATION, + ) + ) + + @staticmethod + def _extrinsics_to_body(extrinsics: rs.extrinsics) -> tuple[Vector3, Quaternion]: + """Convert a librealsense extrinsic (optical axes) into body (REP-103) axes.""" + body_from_optical = np.eye(4) + body_from_optical[:3, :3] = Rotation.from_quat( + [OPTICAL_ROTATION.x, OPTICAL_ROTATION.y, OPTICAL_ROTATION.z, OPTICAL_ROTATION.w] + ).as_matrix() + + optical = np.eye(4) + optical[:3, :3] = np.array(extrinsics.rotation).reshape(3, 3) + optical[:3, 3] = np.array(extrinsics.translation) + + body = body_from_optical @ optical @ body_from_optical.T + rotation = Rotation.from_matrix(body[:3, :3]).as_quat() # [x, y, z, w] + return Vector3(*body[:3, 3]), Quaternion(*rotation) + + def _fresh(self, key: str, frame: rs.frame | None) -> float | None: + """This frame's own timestamp, or None if the stream has not advanced. + + wait_for_frames() returns the latest frame of each stream, and under load they + desync, so each stream is gated on its own frame number. + """ + if frame is None: + return None + number = frame.get_frame_number() + previous = self._last_frame_numbers.get(key) + if number == previous: + self._repeated_frames[key] = self._repeated_frames.get(key, 0) + 1 + return None + if previous is not None and number > previous + 1: + missed = number - previous - 1 + self._dropped_frames[key] = self._dropped_frames.get(key, 0) + missed + logger.warning( + "RealSense %s dropped %d frame(s) at %d (%d total)", + key, + missed, + number, + self._dropped_frames[key], + ) + self._last_frame_numbers[key] = number + stamp = ms_to_s(float(frame.get_timestamp())) + self._last_hardware_ts = stamp + return stamp def _capture_loop(self) -> None: import cv2 @@ -279,123 +565,106 @@ def _capture_loop(self) -> None: # Pipeline stopped or None - exit loop break - ts = time.time() + # Grab the infrared stereo pair from the raw frameset before align() + # (align rebuilds the frameset around depth+color and drops IR). + infra1_frame = frames.get_infrared_frame(1) if self.config.enable_infrared else None + infra2_frame = frames.get_infrared_frame(2) if self.config.enable_infrared else None if self._align is not None: frames = self._align.process(frames) - color_frame = frames.get_color_frame() + color_frame = frames.get_color_frame() if self.config.enable_color else None depth_frame = frames.get_depth_frame() if self.config.enable_depth else None + color_ts = self._fresh("color", color_frame) + depth_ts = self._fresh("depth", depth_frame) + infra1_ts = self._fresh("infra1", infra1_frame) + infra2_ts = self._fresh("infra2", infra2_frame) + # Process color color_img = None - if color_frame: + if color_frame and color_ts is not None: color_data = np.asanyarray(color_frame.get_data()) color_data = cv2.cvtColor(color_data, cv2.COLOR_BGR2RGB) color_img = Image( data=color_data, format=ImageFormat.RGB, frame_id=self._color_optical_frame, - ts=ts, + ts=color_ts, ) self.color_image.publish(color_img) # Process depth depth_img = None - if depth_frame: + if depth_frame and depth_ts is not None: depth_data = np.asanyarray(depth_frame.get_data()) - # When aligned, depth is in color optical frame depth_frame_id = ( self._color_optical_frame - if self.config.align_depth_to_color + if self._align is not None else self._depth_optical_frame ) depth_img = Image( data=depth_data, format=ImageFormat.DEPTH16, frame_id=depth_frame_id, - ts=ts, + ts=depth_ts, ) self.depth_image.publish(depth_img) + if infra1_frame and infra1_ts is not None: + self.infrared_left.publish( + Image( + data=np.asanyarray(infra1_frame.get_data()), + format=ImageFormat.GRAY, + frame_id=self._infra1_optical_frame, + ts=infra1_ts, + ) + ) + if infra2_frame and infra2_ts is not None: + self.infrared_right.publish( + Image( + data=np.asanyarray(infra2_frame.get_data()), + format=ImageFormat.GRAY, + frame_id=self._infra2_optical_frame, + ts=infra2_ts, + ) + ) + # Store latest images for pointcloud generation if self.config.enable_pointcloud and color_img is not None and depth_img is not None: with self._pointcloud_lock: self._latest_color_img = color_img self._latest_depth_img = depth_img - # Publish TF - self._publish_tf(ts) + latest = [ + stamp for stamp in (color_ts, depth_ts, infra1_ts, infra2_ts) if stamp is not None + ] + if latest: + self._publish_tf(max(latest)) def _publish_tf(self, ts: float) -> None: transforms = [] - # base_link -> camera_link (user-provided mounting transform) if self.config.base_transform is not None: - base_to_camera = Transform( - translation=self.config.base_transform.translation, - rotation=self.config.base_transform.rotation, - frame_id=self.config.base_frame_id, - child_frame_id=self._camera_link, - ts=ts, - ) - transforms.append(base_to_camera) - - # camera_link -> camera_depth_frame (identity, depth is at camera_link origin) - camera_link_to_depth = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id=self._camera_link, - child_frame_id=self._depth_frame, - ts=ts, - ) - transforms.append(camera_link_to_depth) - - # camera_depth_frame -> camera_depth_optical_frame - depth_to_depth_optical = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=OPTICAL_ROTATION, - frame_id=self._depth_frame, - child_frame_id=self._depth_optical_frame, - ts=ts, - ) - transforms.append(depth_to_depth_optical) - - # camera_link -> camera_color_frame. With depth disabled there are no - # color->depth extrinsics, so fall back to identity (color at the - # camera_link origin) instead of dereferencing None. - if self._color_to_depth_extrinsics is not None: - color_tf = self._extrinsics_to_transform( - self._color_to_depth_extrinsics, - self._camera_link, - self._color_frame, - ts, + transforms.append( + Transform( + translation=self.config.base_transform.translation, + rotation=self.config.base_transform.rotation, + frame_id=self.config.base_frame_id, + child_frame_id=self._camera_link, + ts=ts, + ) ) - # Invert the transform since extrinsics are color->depth - color_tf = color_tf.inverse() - color_tf.frame_id = self._camera_link - color_tf.child_frame_id = self._color_frame - color_tf.ts = ts - else: - color_tf = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id=self._camera_link, - child_frame_id=self._color_frame, - ts=ts, + for parent, child, translation, rotation in self._mount_edges: + transforms.append( + Transform( + translation=translation, + rotation=rotation, + frame_id=parent, + child_frame_id=child, + ts=ts, + ) ) - transforms.append(color_tf) - - # camera_color_frame -> camera_color_optical_frame - color_to_color_optical = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=OPTICAL_ROTATION, - frame_id=self._color_frame, - child_frame_id=self._color_optical_frame, - ts=ts, - ) - transforms.append(color_to_color_optical) - self.tf.publish(TFMessage(*transforms)) def _generate_pointcloud(self) -> None: @@ -416,13 +685,20 @@ def _generate_pointcloud(self) -> None: ) pcd = pcd.voxel_downsample(0.005) self.pointcloud.publish(pcd) - except Exception as e: - print(f"Pointcloud generation error: {e}") + except Exception as error: + logger.error("RealSense pointcloud generation failed: %s", error) @rpc def stop(self) -> None: self._running = False + if self._imu_pipeline: + try: + self._imu_pipeline.stop() + except Exception: + pass # Pipeline might already be stopped + self._imu_pipeline = None + # Stop pipeline first to unblock wait_for_frames() if self._pipeline: try: @@ -440,7 +716,13 @@ def stop(self) -> None: self._profile = None self._align = None - self._color_to_depth_extrinsics = None + if self._dropped_frames or self._repeated_frames: + logger.info( + "RealSense capture ended: dropped %s, repeats suppressed %s", + dict(self._dropped_frames), + dict(self._repeated_frames), + ) + self._last_frame_numbers = {} self._latest_color_img = None self._latest_depth_img = None super().stop() 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/robot/assembly/mid360_realsense_30.py b/dimos/robot/assembly/mid360_realsense_30.py index 3a22c6e397..50956d1318 100644 --- a/dimos/robot/assembly/mid360_realsense_30.py +++ b/dimos/robot/assembly/mid360_realsense_30.py @@ -33,9 +33,8 @@ Frame sources ------------- -RealSense D435i frame transforms are transcribed from the official -realsense2_description xacro (urdf/_d435.urdf.xacro + urdf/_d435i_imu_modules.urdf.xacro, -use_nominal_extrinsics=true). +The RealSense's own frames come from RealSenseCamera, which reads them off the device; +this file places the lidar side. Mid-360 geometry (manual): body is 65 x 65 x 60 mm; the point-cloud origin O lies on the central vertical axis, ~47 mm above the base. The IMU chip is *not* on that axis. The @@ -64,29 +63,14 @@ frames_to_edge_transforms, ) +BASE_LINK = "base_link" + CAMERA_ANGLE_UP = math.radians(10) -# Mid-360 box: pitched down from bottom_screw_frame, then offset back/up in that frame +# Mid-360 box: pitched down from camera_link, then offset back/up in that frame BOX_PITCH_DOWN = math.radians(26) + CAMERA_ANGLE_UP BOX_BACK = 0.085 -BOX_UP = 0.037 # ~4cm up - -# Physical constants from _d435.urdf.xacro (meters) -CAM_HEIGHT = 0.025 -DEPTH_PY = 0.0175 -DEPTH_PZ = CAM_HEIGHT / 2 -MOUNT_FROM_CENTER_OFFSET = 0.0149 -GLASS_TO_FRONT = 0.1e-3 -ZERO_DEPTH_TO_GLASS = 4.2e-3 -MESH_X_OFFSET = MOUNT_FROM_CENTER_OFFSET - GLASS_TO_FRONT - ZERO_DEPTH_TO_GLASS - -DEPTH_TO_INFRA1_OFFSET = 0.0 -DEPTH_TO_INFRA2_OFFSET = -0.050 -DEPTH_TO_COLOR_OFFSET = 0.015 -IMU_XYZ = (-0.01174, -0.00552, 0.0051) - -# rpy that maps a sensor frame to its optical frame (z-forward, x-right, y-down) -OPTICAL_RPY = (-math.pi / 2, 0.0, -math.pi / 2) +BOX_UP = 0.037 # Mid-360 internal frames (manual: point-cloud origin O ~47mm above base, on central axis). # Box center is 30mm above base, so O sits +17mm along box +z. @@ -94,24 +78,11 @@ # IMU position in point-cloud (lidar) coordinates, from Livox Mid-360 extrinsics. IMU_IN_LIDAR = (0.011, 0.02329, -0.04412) -# The physical mount tree (parent -> child). The gravity-flat "world" helper frame from -# the offline tooling is omitted here — during recording, world comes from odometry. +# The lidar side of the rig. The camera hangs its own frames off base_link, named for +# whichever model is plugged in, and reads their geometry off the device. FRAMES: list[FrameSpec] = [ - ("bottom_screw_frame", None, (0.0, 0.0, 0.0), (0.0, 0.0, 0.0)), - ("link", "bottom_screw_frame", (MESH_X_OFFSET, DEPTH_PY, DEPTH_PZ), (0.0, 0.0, 0.0)), - ("depth_frame", "link", (0.0, 0.0, 0.0), (0.0, 0.0, 0.0)), - ("depth_optical_frame", "depth_frame", (0.0, 0.0, 0.0), OPTICAL_RPY), - ("infra1_frame", "link", (0.0, DEPTH_TO_INFRA1_OFFSET, 0.0), (0.0, 0.0, 0.0)), - ("infra1_optical_frame", "infra1_frame", (0.0, 0.0, 0.0), OPTICAL_RPY), - ("infra2_frame", "link", (0.0, DEPTH_TO_INFRA2_OFFSET, 0.0), (0.0, 0.0, 0.0)), - ("infra2_optical_frame", "infra2_frame", (0.0, 0.0, 0.0), OPTICAL_RPY), - ("color_frame", "link", (0.0, DEPTH_TO_COLOR_OFFSET, 0.0), (0.0, 0.0, 0.0)), - ("color_optical_frame", "color_frame", (0.0, 0.0, 0.0), OPTICAL_RPY), - ("accel_frame", "link", IMU_XYZ, (0.0, 0.0, 0.0)), - ("accel_optical_frame", "accel_frame", (0.0, 0.0, 0.0), OPTICAL_RPY), - ("gyro_frame", "link", IMU_XYZ, (0.0, 0.0, 0.0)), - ("gyro_optical_frame", "gyro_frame", (0.0, 0.0, 0.0), OPTICAL_RPY), - ("box_pitch_frame", "bottom_screw_frame", (0.0, 0.0, 0.0), (0.0, BOX_PITCH_DOWN, 0.0)), + (BASE_LINK, None, (0.0, 0.0, 0.0), (0.0, 0.0, 0.0)), + ("box_pitch_frame", BASE_LINK, (0.0, 0.0, 0.0), (0.0, BOX_PITCH_DOWN, 0.0)), ("box_center", "box_pitch_frame", (-BOX_BACK, 0.0, BOX_UP), (0.0, 0.0, 0.0)), ("lidar_frame", "box_center", (0.0, 0.0, LIDAR_ABOVE_BOX_CENTER), (0.0, 0.0, 0.0)), ("imu_frame", "lidar_frame", IMU_IN_LIDAR, (0.0, 0.0, 0.0)),