feat(recording): go2_mid360 + mid360_realsense_30 recorders

dimos/hardware/sensors/lidar/mid360_realsense_30/blueprints.py

@@ -0,0 +1,70 @@
+# 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.
+
+"""Record blueprints for the RealSense D435i + Mid-360 rig.
+
+Point-LIO odom+lidar and the RealSense color/depth/pointcloud streams are recorded
+into a memory2 db, with the rig's mount frames published continuously onto tf. Two
+variants: ``mid360_realsense_record`` (db only) and ``mid360_realsense_record_with_pcap``
+(also captures a raw .pcap of the Mid-360 UDP stream).
+
+The lidar IPs come from each module's own config (``DIMOS_MID360_LIDAR_IP`` for the
+Mid-360 / pcap capture, ``DIMOS_POINTLIO_LIDAR_IP`` for Point-LIO)::
+
+    export DIMOS_MID360_LIDAR_IP=192.168.1.155 DIMOS_POINTLIO_LIDAR_IP=192.168.1.155
+    dimos run mid360-realsense-record            # db only
+    dimos run mid360-realsense-record-with-pcap  # db + raw pcap
+"""
+
+from dimos.core.coordination.blueprints import autoconnect
+from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera
+from dimos.hardware.sensors.lidar.livox.module import Mid360
+from dimos.hardware.sensors.lidar.mid360_realsense_30.recorder import Mid360RealsenseRecorder
+from dimos.hardware.sensors.lidar.mid360_realsense_30.static_transforms import (
+    Mid360RealsenseStaticTf,
+)
+from dimos.hardware.sensors.lidar.pointlio.module import PointLio
+from dimos.hardware.sensors.lidar.virtual_mid360.recorder import Mid360PcapRecorder
+
+mid360_realsense_record = autoconnect(
+    RealSenseCamera.blueprint().remappings(
+        [
+            (RealSenseCamera, "depth_image", "realsense_depth_image"),
+            (RealSenseCamera, "pointcloud", "realsense_pointcloud"),
+            (RealSenseCamera, "camera_info", "realsense_camera_info"),
+            (RealSenseCamera, "depth_camera_info", "realsense_depth_camera_info"),
+        ]
+    ),
+    Mid360.blueprint().remappings(
+        [
+            (Mid360, "lidar", "livox_lidar"),
+            (Mid360, "imu", "livox_imu"),
+        ]
+    ),
+    PointLio.blueprint(frame_id="world").remappings(
+        [
+            (PointLio, "lidar", "pointlio_lidar"),
+            (PointLio, "odometry", "pointlio_odometry"),
+        ]
+    ),
+    Mid360RealsenseRecorder.blueprint(),
+    # Continuously republishes the rig's mount frames onto tf (no latched static tf).
+    Mid360RealsenseStaticTf.blueprint(),
+).global_config(n_workers=8)
+
+# Same rig, also capturing a raw .pcap of the Mid-360 UDP stream.
+mid360_realsense_record_with_pcap = autoconnect(
+    mid360_realsense_record,
+    Mid360PcapRecorder.blueprint(),
+)

dimos/hardware/sensors/lidar/mid360_realsense_30/recorder.py

@@ -0,0 +1,39 @@
+# 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.
+
+"""Records the RealSense D435i + Mid-360 rig into a memory2 SQLite db.
+
+Captures Point-LIO odom + lidar (trajectory baked into ``pointlio_lidar`` via the
+inherited ``@pose_setter_for``) plus the RealSense color/depth/pointcloud streams. The
+raw Livox stream is NOT recorded here — enable the pcap recorder in the record blueprint
+to capture it. Companion streams are recorded as-is and anchored via the static mount
+frames published on tf.
+"""
+
+from __future__ import annotations
+
+from dimos.core.stream import In
+from dimos.hardware.sensors.lidar.pointlio.pose_recorder import PointlioPoseRecorder
+from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo
+from dimos.msgs.sensor_msgs.Image import Image
+from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
+
+
+class Mid360RealsenseRecorder(PointlioPoseRecorder):
+    # pointlio_odometry / pointlio_lidar are inherited from PointlioPoseRecorder.
+    color_image: In[Image]
+    realsense_depth_image: In[Image]
+    realsense_pointcloud: In[PointCloud2]
+    realsense_camera_info: In[CameraInfo]
+    realsense_depth_camera_info: In[CameraInfo]

dimos/hardware/sensors/lidar/mid360_realsense_30/static_transforms.py

@@ -0,0 +1,101 @@
+# 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.
+
+"""Static mount frames for the RealSense D435i + Mid-360 rig.
+
+Published continuously onto tf while recording (see :class:`Mid360RealsenseStaticTf`)
+so the mount geometry lands in the recording's tf stream.
+
+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).
+
+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
+lidar-to-IMU extrinsic comes from the official Mid-360 config (extrinsic_T flipped gives
+the IMU position in lidar coords).
+"""
+
+from __future__ import annotations
+
+import math
+
+from dimos.msgs.geometry_msgs.Transform import Transform
+from dimos.protocol.tf.static_tf_publisher import (
+    FrameSpec,
+    StaticTfPublisher,
+    frames_to_edge_transforms,
+)
+
+CAMERA_ANGLE_UP = math.radians(10)
+
+# Mid-360 box: pitched down from bottom_screw_frame, 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)
+
+# 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.
+LIDAR_ABOVE_BOX_CENTER = 0.017
+# 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.
+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)),
+    ("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)),
+]
+
+
+class Mid360RealsenseStaticTf(StaticTfPublisher):
+    """Publishes the RealSense/Mid-360 mount tree onto tf on a fixed interval."""
+
+    def transforms(self) -> list[Transform]:
+        return frames_to_edge_transforms(FRAMES)

dimos/hardware/sensors/lidar/pointlio/pose_recorder.py

@@ -0,0 +1,78 @@
+# 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.
+
+"""Memory2 recorder base that anchors Point-LIO frames with the live odometry pose.
+
+Subclass with whatever companion ``In`` ports a given rig wants recorded (camera,
+robot odom/lidar, etc.). Point-LIO's ``odometry`` / ``lidar`` outputs are wired to
+``pointlio_odometry`` / ``pointlio_lidar`` (via ``.remappings()``), and each lidar
+frame is stamped with the latest odometry pose (``@pose_setter_for``) so
+``pointlio_lidar`` carries the trajectory and ``dimos map global`` can register the
+body-frame cloud directly — no separate ``dimos map pose-fill`` pass.
+
+This is distinct from :mod:`dimos.hardware.sensors.lidar.pointlio.recorder`, the
+standalone time-aligning recorder used by the pcap-replay tooling.
+"""
+
+from __future__ import annotations
+
+import time
+
+from dimos.core.stream import In
+from dimos.memory2.module import OnExisting, Recorder, RecorderConfig, pose_setter_for
+from dimos.msgs.geometry_msgs.Pose import Pose
+from dimos.msgs.nav_msgs.Odometry import Odometry
+from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2
+
+# Max sensor-ts gap to stamp a lidar frame with the latest odometry pose. Past
+# this the odometry is considered stale (Point-LIO dropout/lag) and the frame is
+# left unposed -> map-skipped rather than registered at a wrong location. Matches
+# PointlioRecorder._POSE_MATCH_TOL / pose_fill's nearest-match window.
+_POSE_MATCH_TOL = 0.1
+
+
+class PointlioPoseRecorderConfig(RecorderConfig):
+    # Append into a populated db (keep other streams); replace only our own.
+    on_existing: OnExisting = OnExisting.APPEND
+
+
+class PointlioPoseRecorder(Recorder):
+    config: PointlioPoseRecorderConfig
+
+    pointlio_odometry: In[Odometry]
+    pointlio_lidar: In[PointCloud2]
+
+    _last_odom_pose: Pose | None = None
+    _last_odom_raw_ts: float = 0.0
+
+    @pose_setter_for("pointlio_odometry")
+    def _odom_pose(self, msg: Odometry) -> Pose | None:
+        pose = getattr(msg, "pose", None)
+        self._last_odom_pose = getattr(pose, "pose", None) if pose is not None else None
+        raw_ts = getattr(msg, "ts", None)
+        self._last_odom_raw_ts = raw_ts if raw_ts is not None else time.time()
+        return self._last_odom_pose
+
+    @pose_setter_for("pointlio_lidar")
+    def _lidar_pose(self, msg: PointCloud2) -> Pose | None:
+        # Most-recent odometry pose, stamped directly (no tf) — but only if it's
+        # fresh. Stale odometry (older than _POSE_MATCH_TOL) or no odometry yet
+        # returns None -> frame stored unposed, map-skipped.
+        if self._last_odom_pose is None:
+            return None
+        raw_ts = getattr(msg, "ts", None)
+        raw_ts = raw_ts if raw_ts is not None else time.time()
+        if abs(raw_ts - self._last_odom_raw_ts) > _POSE_MATCH_TOL:
+            return None
+        return self._last_odom_pose