From 0a1774b7aed57cef3e65a547324e917151479d3c Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 22 Jun 2026 23:04:58 +0800 Subject: [PATCH 1/6] feat(recording): go2_mid360 + mid360_realsense_30 recorders Add memory2-based record blueprints for the Go2+Mid-360 and RealSense D435i+Mid-360 rigs: - PointlioPoseRecorder: shared Recorder base stamping each lidar frame with the latest odometry pose. - StaticTfPublisher: republishes a rig's static mount frames onto /tf on an interval (PubSubTF has no latched static tf), so they're captured in the recording's tf stream. - Go2Mid360Recorder / Mid360RealsenseRecorder + their static-transform trees and record blueprints (unitree-go2-mid360-record, mid360-realsense-record). Pygame WASD teleop on the go2 rig. - Raw Livox capture is opt-in via RECORD_PCAP=1 (reuses the existing Mid360PcapRecorder); default off. - Recording doc moved to experimental/docs/nav/map_recording/go2_mid360.md (post-processing stripped). Regenerated all_blueprints.py. --- .../lidar/mid360_realsense_30/__init__.py | 13 ++ .../mid360_realsense_record.py | 102 ++++++++++++++++ .../lidar/mid360_realsense_30/recorder.py | 41 +++++++ .../mid360_realsense_30/static_transforms.py | 101 +++++++++++++++ .../sensors/lidar/pointlio/pose_recorder.py | 60 +++++++++ dimos/protocol/tf/static_tf_publisher.py | 112 +++++++++++++++++ dimos/robot/all_blueprints.py | 8 ++ .../basic/unitree_go2_mid360_record.py | 115 ++++++++++++++++++ .../robot/unitree/go2/go2_mid360_recorder.py | 39 ++++++ .../go2/go2_mid360_static_transforms.py | 57 +++++++++ .../docs/nav/map_recording/go2_mid360.md | 98 +++++++++++++++ 11 files changed, 746 insertions(+) create mode 100644 dimos/hardware/sensors/lidar/mid360_realsense_30/__init__.py create mode 100644 dimos/hardware/sensors/lidar/mid360_realsense_30/mid360_realsense_record.py create mode 100644 dimos/hardware/sensors/lidar/mid360_realsense_30/recorder.py create mode 100644 dimos/hardware/sensors/lidar/mid360_realsense_30/static_transforms.py create mode 100644 dimos/hardware/sensors/lidar/pointlio/pose_recorder.py create mode 100644 dimos/protocol/tf/static_tf_publisher.py create mode 100644 dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py create mode 100644 dimos/robot/unitree/go2/go2_mid360_recorder.py create mode 100644 dimos/robot/unitree/go2/go2_mid360_static_transforms.py create mode 100644 experimental/docs/nav/map_recording/go2_mid360.md diff --git a/dimos/hardware/sensors/lidar/mid360_realsense_30/__init__.py b/dimos/hardware/sensors/lidar/mid360_realsense_30/__init__.py new file mode 100644 index 0000000000..1ed1bd093e --- /dev/null +++ b/dimos/hardware/sensors/lidar/mid360_realsense_30/__init__.py @@ -0,0 +1,13 @@ +# 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. diff --git a/dimos/hardware/sensors/lidar/mid360_realsense_30/mid360_realsense_record.py b/dimos/hardware/sensors/lidar/mid360_realsense_30/mid360_realsense_record.py new file mode 100644 index 0000000000..15d6907783 --- /dev/null +++ b/dimos/hardware/sensors/lidar/mid360_realsense_30/mid360_realsense_record.py @@ -0,0 +1,102 @@ +#!/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. + +"""Record blueprint 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. Raw Livox +capture is opt-in: set ``RECORD_PCAP=1`` to also record a .pcap of the Mid-360 UDP +stream. Mirrors the Go2 record blueprint, minus the dog and teleop. + +Run it for a timestamped ``recordings/`` folder:: + + export LIDAR_IP=192.168.1.107 + uv run python dimos/hardware/sensors/lidar/mid360_realsense_30/mid360_realsense_record.py +""" + +from datetime import datetime +import os +from pathlib import Path + +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.coordination.module_coordinator import ModuleCoordinator +from dimos.core.global_config import global_config +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 +from dimos.utils.logging_config import set_run_log_dir, setup_logger + +logger = setup_logger() + +_LIDAR_IP = os.getenv("LIDAR_IP", "192.168.1.107") +# Opt-in raw-Livox pcap capture (default off). Set RECORD_PCAP=1 to include it. +_RECORD_PCAP = os.getenv("RECORD_PCAP", "").lower() in ("1", "true", "yes", "on") + +_N_WORKERS = 8 + + +def _default_recording_dir() -> Path: + now = datetime.now() + stamp = now.strftime("%Y-%m-%d") + "_" + now.strftime("%I-%M%p").lower() + "-PST" + return Path("recordings") / stamp + + +_modules = [ + 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(lidar_ip=_LIDAR_IP).remappings( + [ + (Mid360, "lidar", "livox_lidar"), + (Mid360, "imu", "livox_imu"), + ] + ), + PointLio.blueprint(frame_id="world", lidar_ip=_LIDAR_IP).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(), +] + +if _RECORD_PCAP: + _modules.append(Mid360PcapRecorder.blueprint(lidar_ip=_LIDAR_IP)) + +mid360_realsense_record = autoconnect(*_modules).global_config(n_workers=_N_WORKERS) + + +if __name__ == "__main__": + recording_dir = _default_recording_dir().resolve() + recording_dir.mkdir(parents=True, exist_ok=True) + set_run_log_dir(recording_dir) + global_config.obstacle_avoidance = False + coordinator = ModuleCoordinator.build( + mid360_realsense_record, + {Mid360RealsenseRecorder.name: {"db_path": str(recording_dir / "mem2.db")}}, + ) + coordinator.loop() diff --git a/dimos/hardware/sensors/lidar/mid360_realsense_30/recorder.py b/dimos/hardware/sensors/lidar/mid360_realsense_30/recorder.py new file mode 100644 index 0000000000..3aa1172a20 --- /dev/null +++ b/dimos/hardware/sensors/lidar/mid360_realsense_30/recorder.py @@ -0,0 +1,41 @@ +# 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.nav_msgs.Odometry import Odometry +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: In[Odometry] + pointlio_lidar: In[PointCloud2] + color_image: In[Image] + realsense_depth_image: In[Image] + realsense_pointcloud: In[PointCloud2] + realsense_camera_info: In[CameraInfo] + realsense_depth_camera_info: In[CameraInfo] diff --git a/dimos/hardware/sensors/lidar/mid360_realsense_30/static_transforms.py b/dimos/hardware/sensors/lidar/mid360_realsense_30/static_transforms.py new file mode 100644 index 0000000000..9a43430224 --- /dev/null +++ b/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) diff --git a/dimos/hardware/sensors/lidar/pointlio/pose_recorder.py b/dimos/hardware/sensors/lidar/pointlio/pose_recorder.py new file mode 100644 index 0000000000..34151cc270 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/pose_recorder.py @@ -0,0 +1,60 @@ +# 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 + +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 + + +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 + + @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 + 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). None before the + # first odometry -> frame stored unposed, map-skipped. + return self._last_odom_pose diff --git a/dimos/protocol/tf/static_tf_publisher.py b/dimos/protocol/tf/static_tf_publisher.py new file mode 100644 index 0000000000..095b36ef39 --- /dev/null +++ b/dimos/protocol/tf/static_tf_publisher.py @@ -0,0 +1,112 @@ +# 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. + +"""Repeatedly publish a fixed set of transforms onto the tf stream. + +``PubSubTF`` has no ``publish_static`` (latched) path, so a one-shot publish would +be missed by anything that subscribed later — including a recorder that wants the +mount geometry captured in its tf stream. This module works around that by +re-publishing the transforms on a fixed interval from a background task, each cycle +re-stamped with the current time. Subclass and override :meth:`transforms` with the +rig's mount frames (see the go2 / realsense recording blueprints). +""" + +from __future__ import annotations + +import asyncio +import time + +from pydantic import Field + +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +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.utils.logging_config import setup_logger + +logger = setup_logger() + +# (name, parent_name, translation_xyz, fixed-axis rpy) — parent None marks the tree root. +FrameSpec = tuple[str, str | None, tuple[float, float, float], tuple[float, float, float]] + + +def frames_to_edge_transforms(frames: list[FrameSpec]) -> list[Transform]: + """Build a ``parent -> child`` Transform for each non-root edge of a frame tree. + + This is the static mount tree (the rigid sensor offsets); a tf buffer composes + these edges to answer any ``world <- frame`` query once odometry supplies the + moving ``world <- root`` edge. + """ + transforms: list[Transform] = [] + for name, parent, translation, rpy in frames: + if parent is None: + continue + transforms.append( + Transform( + translation=Vector3(*translation), + rotation=Quaternion.from_euler(Vector3(*rpy)), + frame_id=parent, + child_frame_id=name, + ) + ) + return transforms + + +class StaticTfPublisherConfig(ModuleConfig): + # How often to re-publish the static transforms onto the tf stream. + publish_hz: float = Field(default=5.0, gt=0.0) + + +class StaticTfPublisher(Module): + config: StaticTfPublisherConfig + + _running: bool = False + _transforms: list[Transform] = [] + + def transforms(self) -> list[Transform]: + """The static transforms to publish. Override in a rig-specific subclass.""" + raise NotImplementedError( + f"{type(self).__name__} must override transforms() with its mount frames" + ) + + @rpc + def start(self) -> None: + super().start() + self._transforms = self.transforms() + if not self._transforms: + logger.warning("%s: no transforms to publish", type(self).__name__) + return + self._running = True + self.spawn(self._publish_loop()) + logger.info( + "%s publishing %d static transform(s) at %.1f Hz", + type(self).__name__, + len(self._transforms), + self.config.publish_hz, + ) + + async def _publish_loop(self) -> None: + period = 1.0 / self.config.publish_hz + while self._running: + now = time.time() + for transform in self._transforms: + transform.ts = now + self.tf.publish(*self._transforms) + await asyncio.sleep(period) + + @rpc + def stop(self) -> None: + self._running = False + super().stop() diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 22bf7d27e1..1e67016f3c 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -73,6 +73,7 @@ "mid360-fastlio-voxels-native": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels_native", "mid360-pointlio": "dimos.hardware.sensors.lidar.pointlio.pointlio_blueprints:mid360_pointlio", "mid360-pointlio-voxels": "dimos.hardware.sensors.lidar.pointlio.pointlio_blueprints:mid360_pointlio_voxels", + "mid360-realsense-record": "dimos.hardware.sensors.lidar.mid360_realsense_30.mid360_realsense_record:mid360_realsense_record", "openarm-mock-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_mock_planner_coordinator", "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_planner_coordinator", "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", @@ -115,6 +116,7 @@ "unitree-go2-keyboard-teleop": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_keyboard_teleop:unitree_go2_keyboard_teleop", "unitree-go2-markers": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_markers", "unitree-go2-memory": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_memory", + "unitree-go2-mid360-record": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_mid360_record:unitree_go2_mid360_record", "unitree-go2-relocalization": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_relocalization", "unitree-go2-ros": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_ros:unitree_go2_ros", "unitree-go2-security": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_security:unitree_go2_security", @@ -169,6 +171,8 @@ "go2-connection": "dimos.robot.unitree.go2.connection.GO2Connection", "go2-fleet-connection": "dimos.robot.unitree.go2.fleet_connection.Go2FleetConnection", "go2-memory": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2.Go2Memory", + "go2-mid360-recorder": "dimos.robot.unitree.go2.go2_mid360_recorder.Go2Mid360Recorder", + "go2-mid360-static-tf": "dimos.robot.unitree.go2.go2_mid360_static_transforms.Go2Mid360StaticTf", "go2-teleop-module": "dimos.teleop.quest.quest_extensions.Go2TeleopModule", "google-maps-skill-container": "dimos.agents.skills.google_maps_skill_container.GoogleMapsSkillContainer", "gps-nav-skill-container": "dimos.agents.skills.gps_nav_skill.GpsNavSkillContainer", @@ -191,6 +195,8 @@ "mcp-server": "dimos.agents.mcp.mcp_server.McpServer", "memory-module": "dimos.memory2.module.MemoryModule", "mid360-pcap-recorder": "dimos.hardware.sensors.lidar.virtual_mid360.recorder.Mid360PcapRecorder", + "mid360-realsense-recorder": "dimos.hardware.sensors.lidar.mid360_realsense_30.recorder.Mid360RealsenseRecorder", + "mid360-realsense-static-tf": "dimos.hardware.sensors.lidar.mid360_realsense_30.static_transforms.Mid360RealsenseStaticTf", "mls-planner-native": "dimos.navigation.nav_3d.mls_planner.mls_planner_native.MLSPlannerNative", "mock-b1-connection-module": "dimos.robot.unitree.b1.connection.MockB1ConnectionModule", "module-a": "dimos.robot.unitree.demo_error_on_name_conflicts.ModuleA", @@ -214,6 +220,7 @@ "phone-teleop-module": "dimos.teleop.phone.phone_teleop_module.PhoneTeleopModule", "pick-and-place-module": "dimos.manipulation.pick_and_place_module.PickAndPlaceModule", "point-lio": "dimos.hardware.sensors.lidar.pointlio.module.PointLio", + "pointlio-pose-recorder": "dimos.hardware.sensors.lidar.pointlio.pose_recorder.PointlioPoseRecorder", "pointlio-recorder": "dimos.hardware.sensors.lidar.pointlio.recorder.PointlioRecorder", "quest-teleop-module": "dimos.teleop.quest.quest_teleop_module.QuestTeleopModule", "ray-tracing-voxel-map": "dimos.mapping.ray_tracing.module.RayTracingVoxelMap", @@ -231,6 +238,7 @@ "simple-planner": "dimos.navigation.cmu_nav.modules.simple_planner.simple_planner.SimplePlanner", "spatial-memory": "dimos.perception.spatial_perception.SpatialMemory", "speak-skill": "dimos.agents.skills.speak_skill.SpeakSkill", + "static-tf-publisher": "dimos.protocol.tf.static_tf_publisher.StaticTfPublisher", "tare-planner": "dimos.navigation.cmu_nav.modules.tare_planner.tare_planner.TarePlanner", "teleop-recorder": "dimos.teleop.utils.recorder.TeleopRecorder", "temporal-memory": "dimos.perception.experimental.temporal_memory.temporal_memory.TemporalMemory", diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py new file mode 100644 index 0000000000..6ade47f317 --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py @@ -0,0 +1,115 @@ +#!/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. + +"""Drive-and-record blueprint for the Go2 + Mid-360 rig. + +Pygame WASD teleop drives the dog while Point-LIO odom+lidar, the Go2's lidar/odom, +and the front camera are recorded into a memory2 db. The Go2/Mid-360 mount frames are +published continuously onto tf so they're captured in the recording. Raw Livox capture +is opt-in: set ``RECORD_PCAP=1`` to also record a .pcap of the Mid-360 UDP stream. + +Run it for a timestamped ``recordings/`` folder:: + + export LIDAR_IP=192.168.1.171 + uv run python dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py +""" + +from datetime import datetime +import os +from pathlib import Path + +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.coordination.module_coordinator import ModuleCoordinator +from dimos.core.global_config import global_config +from dimos.hardware.sensors.lidar.livox.module import Mid360 +from dimos.hardware.sensors.lidar.pointlio.module import PointLio +from dimos.hardware.sensors.lidar.virtual_mid360.recorder import Mid360PcapRecorder +from dimos.navigation.movement_manager.movement_manager import MovementManager +from dimos.robot.unitree.go2.connection import GO2Connection +from dimos.robot.unitree.go2.go2_mid360_recorder import Go2Mid360Recorder +from dimos.robot.unitree.go2.go2_mid360_static_transforms import Go2Mid360StaticTf +from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop +from dimos.utils.logging_config import set_run_log_dir, setup_logger + +logger = setup_logger() + +_LIDAR_IP = os.getenv("LIDAR_IP", "192.168.1.171") +_LIDAR_HOST_IP = os.getenv("LIDAR_HOST_IP", "192.168.1.100") +# Opt-in raw-Livox pcap capture (default off). Set RECORD_PCAP=1 to include it. +_RECORD_PCAP = os.getenv("RECORD_PCAP", "").lower() in ("1", "true", "yes", "on") + +_TELEOP_LINEAR_SPEED = 0.3 +_TELEOP_ANGULAR_SPEED = 0.6 +_N_WORKERS = 12 + + +def _default_recording_dir() -> Path: + now = datetime.now() + stamp = now.strftime("%Y-%m-%d") + "_" + now.strftime("%I-%M%p").lower() + "-PST" + return Path("recordings") / stamp + + +_modules = [ + MovementManager.blueprint(), + GO2Connection.blueprint().remappings( + [ + (GO2Connection, "lidar", "go2_lidar"), + (GO2Connection, "odom", "go2_odom"), + ] + ), + Mid360.blueprint(lidar_ip=_LIDAR_IP, host_ip=_LIDAR_HOST_IP).remappings( + [ + (Mid360, "lidar", "livox_lidar"), + (Mid360, "imu", "livox_imu"), + ] + ), + PointLio.blueprint(frame_id="world", lidar_ip=_LIDAR_IP).remappings( + [ + (PointLio, "lidar", "pointlio_lidar"), + (PointLio, "odometry", "pointlio_odometry"), + ] + ), + Go2Mid360Recorder.blueprint(), + # Continuously republishes the rig's mount frames onto tf (no latched static tf). + Go2Mid360StaticTf.blueprint(), + # Pygame keyboard teleop (WASD drive + Q/E strafe). Its cmd_vel feeds + # MovementManager's tele_cmd_vel. + KeyboardTeleop.blueprint( + linear_speed=_TELEOP_LINEAR_SPEED, angular_speed=_TELEOP_ANGULAR_SPEED + ).remappings( + [ + (KeyboardTeleop, "cmd_vel", "tele_cmd_vel"), + ] + ), +] + +if _RECORD_PCAP: + _modules.append(Mid360PcapRecorder.blueprint(lidar_ip=_LIDAR_IP)) + +unitree_go2_mid360_record = autoconnect(*_modules).global_config( + n_workers=_N_WORKERS, robot_model="unitree_go2" +) + + +if __name__ == "__main__": + recording_dir = _default_recording_dir().resolve() + recording_dir.mkdir(parents=True, exist_ok=True) + set_run_log_dir(recording_dir) + global_config.obstacle_avoidance = False + coordinator = ModuleCoordinator.build( + unitree_go2_mid360_record, + {Go2Mid360Recorder.name: {"db_path": str(recording_dir / "mem2.db")}}, + ) + coordinator.loop() diff --git a/dimos/robot/unitree/go2/go2_mid360_recorder.py b/dimos/robot/unitree/go2/go2_mid360_recorder.py new file mode 100644 index 0000000000..cacb359f60 --- /dev/null +++ b/dimos/robot/unitree/go2/go2_mid360_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 Go2 + 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 Go2's companion 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.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + + +class Go2Mid360Recorder(PointlioPoseRecorder): + pointlio_odometry: In[Odometry] + pointlio_lidar: In[PointCloud2] + go2_lidar: In[PointCloud2] + go2_odom: In[PoseStamped] + color_image: In[Image] diff --git a/dimos/robot/unitree/go2/go2_mid360_static_transforms.py b/dimos/robot/unitree/go2/go2_mid360_static_transforms.py new file mode 100644 index 0000000000..b430a11429 --- /dev/null +++ b/dimos/robot/unitree/go2/go2_mid360_static_transforms.py @@ -0,0 +1,57 @@ +# 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 Go2 + Mid-360 + front-camera rig. + +Published continuously onto tf while recording (see :class:`Go2Mid360StaticTf`) so the +mount geometry lands in the recording's tf stream and companion streams (camera, go2 +lidar) can be anchored to ``base_link``. + +Mount geometry (measured on the physical rig) +--------------------------------------------- +- base_link -> front_camera: 32.7cm forward, ~4.3cm up (URDF front_camera mount). +- front_camera -> mid360_link: lidar is 3.2cm back, 12cm up, pitched 44 deg down. +- front_camera -> camera_optical: the standard ROS optical rotation (x-right, y-down, + z-forward). +""" + +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, +) + +MID360_PITCH_DOWN = math.radians(44.0) + +# 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) + +FRAMES: list[FrameSpec] = [ + ("base_link", None, (0.0, 0.0, 0.0), (0.0, 0.0, 0.0)), + ("front_camera", "base_link", (0.32715, -0.00003, 0.04297), (0.0, 0.0, 0.0)), + ("mid360_link", "front_camera", (-0.032, 0.0, 0.12), (0.0, MID360_PITCH_DOWN, 0.0)), + ("camera_optical", "front_camera", (0.0, 0.0, 0.0), OPTICAL_RPY), +] + + +class Go2Mid360StaticTf(StaticTfPublisher): + """Publishes the Go2/Mid-360 mount tree onto tf on a fixed interval.""" + + def transforms(self) -> list[Transform]: + return frames_to_edge_transforms(FRAMES) diff --git a/experimental/docs/nav/map_recording/go2_mid360.md b/experimental/docs/nav/map_recording/go2_mid360.md new file mode 100644 index 0000000000..baa3abdba1 --- /dev/null +++ b/experimental/docs/nav/map_recording/go2_mid360.md @@ -0,0 +1,98 @@ +# Recording a Map (Go2 + Mid-360) + +This walks you through driving a Go2 around a space and capturing a recording: the +Mid-360 point cloud, Point-LIO odometry, and the front camera. You drive, it records. + +If you're on the RealSense rig instead of a Go2, the steps are the same — use the +`mid360_realsense_30` paths in place of `go2_mid360`. + +## What you need + +- A Unitree Go2 with a Livox Mid-360 mounted on it +- A computer to do the recording (it talks to the dog over wifi and to the lidar over a wired link) +- A phone with a hotspot +- The Mid-360's USB-ethernet adapter and cable + +## 1. Mount the Mid-360 + +Bolt the Mid-360 to the top of the dog, pointing forward, as level as you reasonably can. The recorder doesn't need a perfect mount — Point-LIO figures out the lidar's motion on its own and stamps every frame with a pose — but a level, rigid mount gives you cleaner data. Don't let it wobble. A loose lidar is the fastest way to ruin a recording. + +Run the Mid-360's ethernet to your recording computer. The lidar speaks plain ethernet over a USB adapter, so it's a separate wired link, not part of the wifi. + +## 2. Find the lidar's IP and get on its subnet + +The Mid-360 ships with a static IP. Each unit's address is derived from its serial number: the last octet is the last two digits of the serial. So a lidar whose serial ends in `71` is at `192.168.1.171`. A factory-default unit sits at `192.168.1.155`. Check the sticker. + +If the sticker isn't telling you anything, plug it in, power it on, and watch for its packets: + +```bash +sudo tcpdump -ni udp +``` + +The source IP that starts spamming you is the lidar. + +Your computer's wired interface has to live on the same `/24` as the lidar. Set it to `192.168.1.5`: + +```bash +sudo nmcli con add type ethernet ifname con-name livox-mid360 \ + ipv4.addresses 192.168.1.5/24 ipv4.method manual +sudo nmcli con up livox-mid360 +``` + +This sticks across reboots, so you only do it once per machine. + +## 3. Put the dog and your computer on the same hotspot + +The recorder talks to the dog over wifi, so both the dog and your computer need to be on the same network. A phone hotspot is the easy, portable answer. + +Turn on your phone's hotspot, then point the dog at it over Bluetooth: + +```bash +dimos go2tool connect-wifi --ssid --password +``` + +Power the dog on first — it advertises over Bluetooth right away. The command scans, finds the dog, and hands it the wifi credentials. If more than one robot shows up, it'll ask which one. + +Now connect your computer to the same hotspot. Then find the dog's IP on it: + +```bash +dimos go2tool discover +``` + +That prints a row per robot it sees. Grab the dog's IP and export it: + +```bash +export ROBOT_IP= +``` + +At this point your computer has two links going at once: wifi to the dog, wired ethernet to the lidar. That's expected. + +## 4. Record + +Tell the recorder where the lidar is and start it: + +```bash +export LIDAR_IP=192.168.1.171 # whatever you found in step 2 +uv run python dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py +``` + +A keyboard-teleop window opens. Drive with WASD, turn with Q/E, `Z` to lie down, `X` to stand. Drive the dog through the whole space you want mapped. A few tips: + +- Move at a calm walking pace. Whipping it around blurs scans. +- Close the loop — end where you started, and re-cross your own path a couple times. +- Drive smoothly; sharp jerks make Point-LIO's job harder. + +When you're done, `Ctrl+C` the recorder. It writes everything to a timestamped folder under `recordings/`, e.g. `recordings/2026-06-22_03-15pm-PST/mem2.db`. + +You don't fuss with poses while recording — the Point-LIO recorder stamps each lidar frame with the live odometry pose as it goes, so the trajectory is already baked into the recording. The rig's mount frames are published onto the tf stream continuously, so they're captured too. + +### Optional: capture the raw Livox packets + +By default the raw Mid-360 UDP stream is *not* saved. To also capture a `.pcap` of it alongside the db, set `RECORD_PCAP=1`: + +```bash +RECORD_PCAP=1 LIDAR_IP=192.168.1.171 \ + uv run python dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py +``` + +tcpdump needs capture capability. If it can't capture, the recorder prints the exact `setcap` command to grant it. From af33980958cd02e3bff8fa05807c349c226bc298 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 24 Jun 2026 15:55:49 +0800 Subject: [PATCH 2/6] fix(recording): staleness guard on pose, drop redundant ports, real tz label Address Greptile review on PR #2588: - pose_recorder: guard pointlio_lidar pose-stamping with _POSE_MATCH_TOL (0.1s) on raw odom ts, matching PointlioRecorder. Stale odometry now yields an unposed (map-skipped) frame instead of mis-stamping at the last pose. - go2/realsense recorders: drop redundant pointlio_odometry/pointlio_lidar port re-declarations (inherited from PointlioPoseRecorder). - record blueprints: use datetime.now().astimezone() + %Z for the recordings dir label instead of a hardcoded -PST suffix. --- .../mid360_realsense_record.py | 7 ++++-- .../lidar/mid360_realsense_30/recorder.py | 4 +--- .../sensors/lidar/pointlio/pose_recorder.py | 22 +++++++++++++++++-- .../basic/unitree_go2_mid360_record.py | 7 ++++-- .../robot/unitree/go2/go2_mid360_recorder.py | 4 +--- 5 files changed, 32 insertions(+), 12 deletions(-) diff --git a/dimos/hardware/sensors/lidar/mid360_realsense_30/mid360_realsense_record.py b/dimos/hardware/sensors/lidar/mid360_realsense_30/mid360_realsense_record.py index 15d6907783..f80c8448cb 100644 --- a/dimos/hardware/sensors/lidar/mid360_realsense_30/mid360_realsense_record.py +++ b/dimos/hardware/sensors/lidar/mid360_realsense_30/mid360_realsense_record.py @@ -53,8 +53,11 @@ def _default_recording_dir() -> Path: - now = datetime.now() - stamp = now.strftime("%Y-%m-%d") + "_" + now.strftime("%I-%M%p").lower() + "-PST" + # Local time, with the machine's actual zone abbreviation (not a hardcoded PST). + now = datetime.now().astimezone() + stamp = ( + now.strftime("%Y-%m-%d") + "_" + now.strftime("%I-%M%p").lower() + "-" + now.strftime("%Z") + ) return Path("recordings") / stamp diff --git a/dimos/hardware/sensors/lidar/mid360_realsense_30/recorder.py b/dimos/hardware/sensors/lidar/mid360_realsense_30/recorder.py index 3aa1172a20..204a637308 100644 --- a/dimos/hardware/sensors/lidar/mid360_realsense_30/recorder.py +++ b/dimos/hardware/sensors/lidar/mid360_realsense_30/recorder.py @@ -25,15 +25,13 @@ from dimos.core.stream import In from dimos.hardware.sensors.lidar.pointlio.pose_recorder import PointlioPoseRecorder -from dimos.msgs.nav_msgs.Odometry import Odometry 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: In[Odometry] - pointlio_lidar: In[PointCloud2] + # pointlio_odometry / pointlio_lidar are inherited from PointlioPoseRecorder. color_image: In[Image] realsense_depth_image: In[Image] realsense_pointcloud: In[PointCloud2] diff --git a/dimos/hardware/sensors/lidar/pointlio/pose_recorder.py b/dimos/hardware/sensors/lidar/pointlio/pose_recorder.py index 34151cc270..133254cbce 100644 --- a/dimos/hardware/sensors/lidar/pointlio/pose_recorder.py +++ b/dimos/hardware/sensors/lidar/pointlio/pose_recorder.py @@ -27,12 +27,20 @@ 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. @@ -46,15 +54,25 @@ class PointlioPoseRecorder(Recorder): 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). None before the - # first odometry -> frame stored unposed, map-skipped. + # 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 diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py index 6ade47f317..2ce0516da7 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py @@ -56,8 +56,11 @@ def _default_recording_dir() -> Path: - now = datetime.now() - stamp = now.strftime("%Y-%m-%d") + "_" + now.strftime("%I-%M%p").lower() + "-PST" + # Local time, with the machine's actual zone abbreviation (not a hardcoded PST). + now = datetime.now().astimezone() + stamp = ( + now.strftime("%Y-%m-%d") + "_" + now.strftime("%I-%M%p").lower() + "-" + now.strftime("%Z") + ) return Path("recordings") / stamp diff --git a/dimos/robot/unitree/go2/go2_mid360_recorder.py b/dimos/robot/unitree/go2/go2_mid360_recorder.py index cacb359f60..e3d48878bb 100644 --- a/dimos/robot/unitree/go2/go2_mid360_recorder.py +++ b/dimos/robot/unitree/go2/go2_mid360_recorder.py @@ -26,14 +26,12 @@ from dimos.core.stream import In from dimos.hardware.sensors.lidar.pointlio.pose_recorder import PointlioPoseRecorder from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 class Go2Mid360Recorder(PointlioPoseRecorder): - pointlio_odometry: In[Odometry] - pointlio_lidar: In[PointCloud2] + # pointlio_odometry / pointlio_lidar are inherited from PointlioPoseRecorder. go2_lidar: In[PointCloud2] go2_odom: In[PoseStamped] color_image: In[Image] From 299edb0ca81251b67c8f15afefa99fb448ee1f19 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 24 Jun 2026 16:14:38 +0800 Subject: [PATCH 3/6] fix(recording): remove disallowed __init__.py (test_no_init_files) mid360_realsense_30/__init__.py was license-header only; the repo forbids __init__.py under dimos/. Namespace-package import still works. --- .../sensors/lidar/mid360_realsense_30/__init__.py | 13 ------------- 1 file changed, 13 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/mid360_realsense_30/__init__.py diff --git a/dimos/hardware/sensors/lidar/mid360_realsense_30/__init__.py b/dimos/hardware/sensors/lidar/mid360_realsense_30/__init__.py deleted file mode 100644 index 1ed1bd093e..0000000000 --- a/dimos/hardware/sensors/lidar/mid360_realsense_30/__init__.py +++ /dev/null @@ -1,13 +0,0 @@ -# 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 bfc4aff7a85aa856810db67214cfad9388b4e800 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 24 Jun 2026 17:03:24 +0800 Subject: [PATCH 4/6] refactor(recording): mid360_realsense as blueprints.py with two variants Rename mid360_realsense_record.py -> blueprints.py, drop the __main__ runner, and expose two blueprints instead of the RECORD_PCAP env toggle: - mid360_realsense_record (db only) - mid360_realsense_record_with_pcap (db + raw Mid-360 pcap) Matches the repo's blueprints.py convention (e.g. virtual_mid360). Regenerated all_blueprints.py. --- ...d360_realsense_record.py => blueprints.py} | 54 ++++--------------- dimos/robot/all_blueprints.py | 3 +- 2 files changed, 13 insertions(+), 44 deletions(-) rename dimos/hardware/sensors/lidar/mid360_realsense_30/{mid360_realsense_record.py => blueprints.py} (57%) diff --git a/dimos/hardware/sensors/lidar/mid360_realsense_30/mid360_realsense_record.py b/dimos/hardware/sensors/lidar/mid360_realsense_30/blueprints.py similarity index 57% rename from dimos/hardware/sensors/lidar/mid360_realsense_30/mid360_realsense_record.py rename to dimos/hardware/sensors/lidar/mid360_realsense_30/blueprints.py index f80c8448cb..5f2a8e331a 100644 --- a/dimos/hardware/sensors/lidar/mid360_realsense_30/mid360_realsense_record.py +++ b/dimos/hardware/sensors/lidar/mid360_realsense_30/blueprints.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python3 # Copyright 2026 Dimensional Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -13,26 +12,21 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Record blueprint for the RealSense D435i + Mid-360 rig. +"""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. Raw Livox -capture is opt-in: set ``RECORD_PCAP=1`` to also record a .pcap of the Mid-360 UDP -stream. Mirrors the Go2 record blueprint, minus the dog and teleop. - -Run it for a timestamped ``recordings/`` folder:: +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). export LIDAR_IP=192.168.1.107 - uv run python dimos/hardware/sensors/lidar/mid360_realsense_30/mid360_realsense_record.py + dimos run mid360-realsense-record # db only + dimos run mid360-realsense-record-with-pcap # db + raw pcap """ -from datetime import datetime import os -from pathlib import Path from dimos.core.coordination.blueprints import autoconnect -from dimos.core.coordination.module_coordinator import ModuleCoordinator -from dimos.core.global_config import global_config 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 @@ -41,26 +35,10 @@ ) from dimos.hardware.sensors.lidar.pointlio.module import PointLio from dimos.hardware.sensors.lidar.virtual_mid360.recorder import Mid360PcapRecorder -from dimos.utils.logging_config import set_run_log_dir, setup_logger - -logger = setup_logger() _LIDAR_IP = os.getenv("LIDAR_IP", "192.168.1.107") -# Opt-in raw-Livox pcap capture (default off). Set RECORD_PCAP=1 to include it. -_RECORD_PCAP = os.getenv("RECORD_PCAP", "").lower() in ("1", "true", "yes", "on") - _N_WORKERS = 8 - -def _default_recording_dir() -> Path: - # Local time, with the machine's actual zone abbreviation (not a hardcoded PST). - now = datetime.now().astimezone() - stamp = ( - now.strftime("%Y-%m-%d") + "_" + now.strftime("%I-%M%p").lower() + "-" + now.strftime("%Z") - ) - return Path("recordings") / stamp - - _modules = [ RealSenseCamera.blueprint().remappings( [ @@ -87,19 +65,9 @@ def _default_recording_dir() -> Path: Mid360RealsenseStaticTf.blueprint(), ] -if _RECORD_PCAP: - _modules.append(Mid360PcapRecorder.blueprint(lidar_ip=_LIDAR_IP)) - mid360_realsense_record = autoconnect(*_modules).global_config(n_workers=_N_WORKERS) - -if __name__ == "__main__": - recording_dir = _default_recording_dir().resolve() - recording_dir.mkdir(parents=True, exist_ok=True) - set_run_log_dir(recording_dir) - global_config.obstacle_avoidance = False - coordinator = ModuleCoordinator.build( - mid360_realsense_record, - {Mid360RealsenseRecorder.name: {"db_path": str(recording_dir / "mem2.db")}}, - ) - coordinator.loop() +mid360_realsense_record_with_pcap = autoconnect( + *_modules, + Mid360PcapRecorder.blueprint(lidar_ip=_LIDAR_IP), +).global_config(n_workers=_N_WORKERS) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 1e67016f3c..31eaa42700 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -73,7 +73,8 @@ "mid360-fastlio-voxels-native": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels_native", "mid360-pointlio": "dimos.hardware.sensors.lidar.pointlio.pointlio_blueprints:mid360_pointlio", "mid360-pointlio-voxels": "dimos.hardware.sensors.lidar.pointlio.pointlio_blueprints:mid360_pointlio_voxels", - "mid360-realsense-record": "dimos.hardware.sensors.lidar.mid360_realsense_30.mid360_realsense_record:mid360_realsense_record", + "mid360-realsense-record": "dimos.hardware.sensors.lidar.mid360_realsense_30.blueprints:mid360_realsense_record", + "mid360-realsense-record-with-pcap": "dimos.hardware.sensors.lidar.mid360_realsense_30.blueprints:mid360_realsense_record_with_pcap", "openarm-mock-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_mock_planner_coordinator", "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints.planner:openarm_planner_coordinator", "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", From 0a593541ecabe66b7cf58eeefa059b18a5056624 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 24 Jun 2026 17:45:04 +0800 Subject: [PATCH 5/6] refactor(recording): drop blueprint-level lidar_ip/host_ip + n_workers consts Let each module self-configure its lidar IP from its own env var (DIMOS_MID360_LIDAR_IP for Mid360/pcap, DIMOS_POINTLIO_LIDAR_IP for Point-LIO) instead of a blueprint-level LIDAR_IP env + hardcoded default fanned out to all of them. Inline n_workers. Applies to both the realsense blueprints and the go2 record blueprint. --- .../lidar/mid360_realsense_30/blueprints.py | 20 +++++++++---------- .../basic/unitree_go2_mid360_record.py | 17 ++++++++-------- 2 files changed, 17 insertions(+), 20 deletions(-) diff --git a/dimos/hardware/sensors/lidar/mid360_realsense_30/blueprints.py b/dimos/hardware/sensors/lidar/mid360_realsense_30/blueprints.py index 5f2a8e331a..76c90ff306 100644 --- a/dimos/hardware/sensors/lidar/mid360_realsense_30/blueprints.py +++ b/dimos/hardware/sensors/lidar/mid360_realsense_30/blueprints.py @@ -19,13 +19,14 @@ variants: ``mid360_realsense_record`` (db only) and ``mid360_realsense_record_with_pcap`` (also captures a raw .pcap of the Mid-360 UDP stream). - export LIDAR_IP=192.168.1.107 +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 """ -import os - 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 @@ -36,9 +37,6 @@ from dimos.hardware.sensors.lidar.pointlio.module import PointLio from dimos.hardware.sensors.lidar.virtual_mid360.recorder import Mid360PcapRecorder -_LIDAR_IP = os.getenv("LIDAR_IP", "192.168.1.107") -_N_WORKERS = 8 - _modules = [ RealSenseCamera.blueprint().remappings( [ @@ -48,13 +46,13 @@ (RealSenseCamera, "depth_camera_info", "realsense_depth_camera_info"), ] ), - Mid360.blueprint(lidar_ip=_LIDAR_IP).remappings( + Mid360.blueprint().remappings( [ (Mid360, "lidar", "livox_lidar"), (Mid360, "imu", "livox_imu"), ] ), - PointLio.blueprint(frame_id="world", lidar_ip=_LIDAR_IP).remappings( + PointLio.blueprint(frame_id="world").remappings( [ (PointLio, "lidar", "pointlio_lidar"), (PointLio, "odometry", "pointlio_odometry"), @@ -65,9 +63,9 @@ Mid360RealsenseStaticTf.blueprint(), ] -mid360_realsense_record = autoconnect(*_modules).global_config(n_workers=_N_WORKERS) +mid360_realsense_record = autoconnect(*_modules).global_config(n_workers=8) mid360_realsense_record_with_pcap = autoconnect( *_modules, - Mid360PcapRecorder.blueprint(lidar_ip=_LIDAR_IP), -).global_config(n_workers=_N_WORKERS) + Mid360PcapRecorder.blueprint(), +).global_config(n_workers=8) diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py index 2ce0516da7..805d303cee 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py @@ -20,9 +20,11 @@ published continuously onto tf so they're captured in the recording. Raw Livox capture is opt-in: set ``RECORD_PCAP=1`` to also record a .pcap of the Mid-360 UDP stream. -Run it for a timestamped ``recordings/`` folder:: +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). Run it for a +timestamped ``recordings/`` folder:: - export LIDAR_IP=192.168.1.171 + export DIMOS_MID360_LIDAR_IP=192.168.1.171 DIMOS_POINTLIO_LIDAR_IP=192.168.1.171 uv run python dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py """ @@ -45,14 +47,11 @@ logger = setup_logger() -_LIDAR_IP = os.getenv("LIDAR_IP", "192.168.1.171") -_LIDAR_HOST_IP = os.getenv("LIDAR_HOST_IP", "192.168.1.100") # Opt-in raw-Livox pcap capture (default off). Set RECORD_PCAP=1 to include it. _RECORD_PCAP = os.getenv("RECORD_PCAP", "").lower() in ("1", "true", "yes", "on") _TELEOP_LINEAR_SPEED = 0.3 _TELEOP_ANGULAR_SPEED = 0.6 -_N_WORKERS = 12 def _default_recording_dir() -> Path: @@ -72,13 +71,13 @@ def _default_recording_dir() -> Path: (GO2Connection, "odom", "go2_odom"), ] ), - Mid360.blueprint(lidar_ip=_LIDAR_IP, host_ip=_LIDAR_HOST_IP).remappings( + Mid360.blueprint().remappings( [ (Mid360, "lidar", "livox_lidar"), (Mid360, "imu", "livox_imu"), ] ), - PointLio.blueprint(frame_id="world", lidar_ip=_LIDAR_IP).remappings( + PointLio.blueprint(frame_id="world").remappings( [ (PointLio, "lidar", "pointlio_lidar"), (PointLio, "odometry", "pointlio_odometry"), @@ -99,10 +98,10 @@ def _default_recording_dir() -> Path: ] if _RECORD_PCAP: - _modules.append(Mid360PcapRecorder.blueprint(lidar_ip=_LIDAR_IP)) + _modules.append(Mid360PcapRecorder.blueprint()) unitree_go2_mid360_record = autoconnect(*_modules).global_config( - n_workers=_N_WORKERS, robot_model="unitree_go2" + n_workers=12, robot_model="unitree_go2" ) From 461800fc6ec8794b2b68d70fa6e0ae2d6b8fb385 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 24 Jun 2026 19:06:13 +0800 Subject: [PATCH 6/6] refactor(recording): compose blueprints by nesting, drop _modules list MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Build the with-pcap variant by nesting the base blueprint (autoconnect(base, Mid360PcapRecorder)) instead of spreading a shared *_modules list. autoconnect merges the nested blueprint's modules, remappings, and global_config (n_workers) — verified. Same nesting for the go2 RECORD_PCAP opt-in. --- .../lidar/mid360_realsense_30/blueprints.py | 11 +++++------ .../blueprints/basic/unitree_go2_mid360_record.py | 14 +++++++------- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/dimos/hardware/sensors/lidar/mid360_realsense_30/blueprints.py b/dimos/hardware/sensors/lidar/mid360_realsense_30/blueprints.py index 76c90ff306..50a10beb2b 100644 --- a/dimos/hardware/sensors/lidar/mid360_realsense_30/blueprints.py +++ b/dimos/hardware/sensors/lidar/mid360_realsense_30/blueprints.py @@ -37,7 +37,7 @@ from dimos.hardware.sensors.lidar.pointlio.module import PointLio from dimos.hardware.sensors.lidar.virtual_mid360.recorder import Mid360PcapRecorder -_modules = [ +mid360_realsense_record = autoconnect( RealSenseCamera.blueprint().remappings( [ (RealSenseCamera, "depth_image", "realsense_depth_image"), @@ -61,11 +61,10 @@ Mid360RealsenseRecorder.blueprint(), # Continuously republishes the rig's mount frames onto tf (no latched static tf). Mid360RealsenseStaticTf.blueprint(), -] - -mid360_realsense_record = autoconnect(*_modules).global_config(n_workers=8) +).global_config(n_workers=8) +# Same rig, also capturing a raw .pcap of the Mid-360 UDP stream. mid360_realsense_record_with_pcap = autoconnect( - *_modules, + mid360_realsense_record, Mid360PcapRecorder.blueprint(), -).global_config(n_workers=8) +) diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py index 805d303cee..be041b4788 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py @@ -63,7 +63,7 @@ def _default_recording_dir() -> Path: return Path("recordings") / stamp -_modules = [ +unitree_go2_mid360_record = autoconnect( MovementManager.blueprint(), GO2Connection.blueprint().remappings( [ @@ -95,14 +95,14 @@ def _default_recording_dir() -> Path: (KeyboardTeleop, "cmd_vel", "tele_cmd_vel"), ] ), -] +).global_config(n_workers=12, robot_model="unitree_go2") +# Opt-in: also capture a raw .pcap of the Mid-360 UDP stream (RECORD_PCAP=1). if _RECORD_PCAP: - _modules.append(Mid360PcapRecorder.blueprint()) - -unitree_go2_mid360_record = autoconnect(*_modules).global_config( - n_workers=12, robot_model="unitree_go2" -) + unitree_go2_mid360_record = autoconnect( + unitree_go2_mid360_record, + Mid360PcapRecorder.blueprint(), + ) if __name__ == "__main__":