diff --git a/dimos/hardware/sensors/lidar/mid360_realsense_30/blueprints.py b/dimos/hardware/sensors/lidar/mid360_realsense_30/blueprints.py new file mode 100644 index 0000000000..50a10beb2b --- /dev/null +++ b/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(), +) 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..204a637308 --- /dev/null +++ b/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] 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..133254cbce --- /dev/null +++ b/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 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..31eaa42700 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -73,6 +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.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", @@ -115,6 +117,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 +172,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 +196,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 +221,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 +239,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..be041b4788 --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py @@ -0,0 +1,117 @@ +#!/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. + +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 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 +""" + +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() + +# 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 + + +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 + + +unitree_go2_mid360_record = autoconnect( + MovementManager.blueprint(), + GO2Connection.blueprint().remappings( + [ + (GO2Connection, "lidar", "go2_lidar"), + (GO2Connection, "odom", "go2_odom"), + ] + ), + 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"), + ] + ), + 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"), + ] + ), +).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: + unitree_go2_mid360_record = autoconnect( + unitree_go2_mid360_record, + Mid360PcapRecorder.blueprint(), + ) + + +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..e3d48878bb --- /dev/null +++ b/dimos/robot/unitree/go2/go2_mid360_recorder.py @@ -0,0 +1,37 @@ +# 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.sensor_msgs.Image import Image +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + + +class Go2Mid360Recorder(PointlioPoseRecorder): + # pointlio_odometry / pointlio_lidar are inherited from PointlioPoseRecorder. + 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.