From 0f50b687b6f1d054f989dfa79772f515c038c16b Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 1 Jun 2026 15:32:58 -0700 Subject: [PATCH 001/185] add recorder --- dimos/robot/all_blueprints.py | 6 + .../blueprints/smart/unitree_go2_record.py | 303 ++++++++++++++++++ 2 files changed, 309 insertions(+) create mode 100644 dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index c3e0f9fff0..2cf1774ba5 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -104,6 +104,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-record": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_record:unitree_go2_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", @@ -125,9 +126,11 @@ all_modules = { "alfred-high-level": "dimos.robot.diy.alfred.effector_high_level.AlfredHighLevel", + "arise-sim-adapter": "dimos.navigation.smart_nav.arise_sim_adapter.AriseSimAdapter", "arm-teleop-module": "dimos.teleop.quest.quest_extensions.ArmTeleopModule", "b-box-navigation-module": "dimos.navigation.bbox_navigation.BBoxNavigationModule", "b1-connection-module": "dimos.robot.unitree.b1.connection.B1ConnectionModule", + "better-fast-lio2": "dimos.navigation.smart_nav.modules.better_fastlio2.better_fastlio2.BetterFastLio2", "camera-module": "dimos.hardware.sensors.camera.module.CameraModule", "cartesian-motion-controller": "dimos.manipulation.control.servo_control.cartesian_motion_controller.CartesianMotionController", "control-coordinator": "dimos.control.coordinator.ControlCoordinator", @@ -153,6 +156,7 @@ "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-memory": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_record.Go2Mid360Memory", "google-maps-skill-container": "dimos.agents.skills.google_maps_skill_container.GoogleMapsSkillContainer", "gps-nav-skill-container": "dimos.agents.skills.gps_nav_skill.GpsNavSkillContainer", "grasp-gen-module": "dimos.manipulation.grasping.graspgen_module.GraspGenModule", @@ -205,8 +209,10 @@ "semantic-search": "dimos.memory2.module.SemanticSearch", "simple-phone-teleop": "dimos.teleop.phone.phone_extensions.SimplePhoneTeleop", "simple-planner": "dimos.navigation.nav_stack.modules.simple_planner.simple_planner.SimplePlanner", + "slam-sim-adapter": "dimos.navigation.smart_nav.slam_sim_adapter.SlamSimAdapter", "spatial-memory": "dimos.perception.spatial_perception.SpatialMemory", "speak-skill": "dimos.agents.skills.speak_skill.SpeakSkill", + "speed-warner": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_record.SpeedWarner", "tare-planner": "dimos.navigation.nav_stack.modules.tare_planner.tare_planner.TarePlanner", "temporal-memory": "dimos.perception.experimental.temporal_memory.temporal_memory.TemporalMemory", "terrain-analysis": "dimos.navigation.nav_stack.modules.terrain_analysis.terrain_analysis.TerrainAnalysis", diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py new file mode 100644 index 0000000000..1a3c985a5d --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py @@ -0,0 +1,303 @@ +#!/usr/bin/env python3 +# Copyright 2025-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 datetime import datetime +import math +import os +from pathlib import Path +import shutil +import signal +import subprocess +import time +from typing import Any + +from pydantic import Field +from reactivex.disposable import Disposable + +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.core import rpc +from dimos.core.module import Module +from dimos.core.stream import In +from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 +from dimos.hardware.sensors.lidar.livox.module import Mid360 +from dimos.memory2.module import Recorder, RecorderConfig +from dimos.memory2.stream import Stream +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.Imu import Imu +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.navigation.movement_manager.movement_manager import MovementManager +from dimos.robot.unitree.go2.connection import GO2Connection +from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +# tcpdump fails fast (EPERM, bad iface) within a few ms; pause briefly so poll() catches that. +_TCPDUMP_STARTUP_PROBE_SEC = 0.3 + + +def _stamp() -> str: + now = datetime.now() + return now.strftime("%Y-%m-%d") + "_" + now.strftime("%-I-%M%p").lower() + "-PST" + + +def _default_recording_dir() -> Path: + return Path(f"recording_go2_mid360_{_stamp()}") + + +class Go2Mid360MemoryConfig(RecorderConfig): + """One recording dir per session: /data.db plus /mid360.pcap.""" + + recording_dir: Path = Field(default_factory=_default_recording_dir) + # Filled in by model_post_init below if left at the default. + db_path: str | Path = "" + pcap_path: str | Path = "" + + default_frame_id: str = "base_link" + + # tcpdump configuration. Capture is filtered to UDP from the lidar IP. + record_pcap: bool = True + record_pcap_iface: str = "enp2s0" + record_pcap_snaplen: int = 2048 + lidar_ip: str = "192.168.1.107" + + def model_post_init(self, __context: object) -> None: + super().model_post_init(__context) + # Resolve db/pcap paths from recording_dir if the caller didn't set them. + if not self.db_path: + self.db_path = self.recording_dir / "data.db" + if not self.pcap_path: + self.pcap_path = self.recording_dir / "mid360.pcap" + + +class Go2Mid360Memory(Recorder): + """Records Go2 camera, native Go2 lidar, Mid-360 (lidar + IMU), FastLio2 + odometry, and Go2 leg odometry. + + Also owns the tcpdump process that captures raw UDP packets from the + Mid-360. Single session = single timestamped dir holding both the + sqlite memory store and the pcap. + """ + + config: Go2Mid360MemoryConfig + + color_image: In[Image] + lidar: In[PointCloud2] + odom: In[PoseStamped] + fastlio_lidar: In[PointCloud2] + fastlio_odometry: In[Odometry] + livox_lidar: In[PointCloud2] + livox_imu: In[Imu] + + _pcap_proc: subprocess.Popen[bytes] | None = None + + @rpc + def start(self) -> None: + Path(self.config.recording_dir).mkdir(parents=True, exist_ok=True) + if self.config.record_pcap: + self._start_pcap() + super().start() + + @rpc + def stop(self) -> None: + super().stop() + self._stop_pcap() + + def _port_to_stream(self, name: str, input_topic: In[Any], stream: Stream[Any]) -> None: + """Append each message from *input_topic* to *stream*, attaching world pose via tf. + + Stamped messages use their own ``.frame_id`` and ``.ts``; unstamped + messages (or ones whose frame isn't in the tf graph, e.g. a payload + already in world coords) fall back to ``config.default_frame_id`` — + so every observation gets a robot-pose anchor when tf is publishing. + + Registers the subscription as a disposable on this module. + """ + + default_frame_id = self.config.default_frame_id + tf_tolerance = self.config.tf_tolerance + + def on_msg(msg: Any) -> None: + # Force system time for all messages + ts = time.time() + frame_id = ( + getattr(msg, "child_frame_id", None) + or getattr(msg, "frame_id", None) + or default_frame_id + ) + transform = self.tf.get("world", frame_id, time_point=ts, time_tolerance=tf_tolerance) + pose = transform.to_pose() if transform is not None else None + + stream.append(msg, ts=ts, pose=pose) + + self.register_disposable(Disposable(input_topic.subscribe(on_msg))) + + def _start_pcap(self) -> None: + cfg = self.config + path = Path(cfg.pcap_path).expanduser() + path.parent.mkdir(parents=True, exist_ok=True) + + # Capture every UDP packet originating from the lidar. + packet_filter_expression = f"src host {cfg.lidar_ip} and udp" + tcpdump = shutil.which("tcpdump") or "tcpdump" + cmd = [ + tcpdump, + "-i", + cfg.record_pcap_iface, + "-w", + str(path), + "-s", + str(cfg.record_pcap_snaplen), + "-U", + "-n", + packet_filter_expression, + ] + + proc = subprocess.Popen( + cmd, + stdout=subprocess.DEVNULL, + stderr=subprocess.PIPE, + start_new_session=True, + ) + # tcpdump exits within a few ms on EPERM; wait briefly so we can detect that. + time.sleep(_TCPDUMP_STARTUP_PROBE_SEC) + if proc.poll() is not None: + stderr = proc.stderr.read().decode(errors="replace") if proc.stderr else "" + self._pcap_proc = None + logger.error( + f"Go2Mid360Memory pcap recording failed to start — tcpdump exited" + f" rc={proc.returncode} stderr={stderr.strip()}" + ) + print( + "[go2_record] pcap recording is enabled but tcpdump cannot capture.\n" + " Grant capture capability once with:\n" + f" sudo setcap cap_net_raw,cap_net_admin=eip {tcpdump}\n" + " then restart. (tcpdump stderr above.)", + flush=True, + ) + return + + logger.info( + f"Go2Mid360Memory pcap recording enabled path={path} " + f"iface={cfg.record_pcap_iface} filter={packet_filter_expression!r}" + ) + self._pcap_proc = proc + + def _stop_pcap(self) -> None: + proc = self._pcap_proc + if proc is None: + return + self._pcap_proc = None + if proc.poll() is not None: + return + # SIGINT is tcpdump's documented "stop cleanly" signal — it prints + # packet counts and flushes the pcap header. + proc.send_signal(signal.SIGINT) + try: + proc.wait(timeout=self.config.shutdown_timeout) + except subprocess.TimeoutExpired: + try: + os.killpg(os.getpgid(proc.pid), signal.SIGTERM) + except ProcessLookupError: + pass + try: + proc.wait(timeout=self.config.shutdown_timeout) + except subprocess.TimeoutExpired: + proc.kill() + proc.wait() + logger.info(f"Go2Mid360Memory pcap recording stopped path={self.config.pcap_path}") + + +MPH_PER_MPS = 2.23694 +SPEED_LIMIT_MPH = 30.0 +_SPEED_STATUS_PRINT_INTERVAL_SEC = 1.0 + + +class SpeedWarner(Module): + """Watches fastlio_odometry; once speed ever exceeds the limit (impossible for the Go2, + so it indicates the FastLio2 estimate has diverged / sensor is about to crash), + latches and spams an error on every subsequent odom message until restart. + + FastLio2's C++ publisher hardcodes twist to zero (cpp/main.cpp), so msg.vx/vy/vz + are always 0. Speed is derived from pose deltas instead. + """ + + fastlio_odometry: In[Odometry] + + _tripped: bool = False + _max_mph: float = 0.0 + _last_pos: tuple[float, float, float] | None = None + _last_ts: float | None = None + _last_print_ts: float = 0.0 + + async def handle_fastlio_odometry(self, msg: Odometry) -> None: + ts = msg.ts or time.time() + pos = (msg.pose.x, msg.pose.y, msg.pose.z) + last_pos, last_ts = self._last_pos, self._last_ts + self._last_pos, self._last_ts = pos, ts + if last_pos is None or last_ts is None: + return + dt = ts - last_ts + if dt <= 0: + return + dx, dy, dz = pos[0] - last_pos[0], pos[1] - last_pos[1], pos[2] - last_pos[2] + speed_mph = math.sqrt(dx * dx + dy * dy + dz * dz) / dt * MPH_PER_MPS + if speed_mph > self._max_mph: + self._max_mph = speed_mph + if ts - self._last_print_ts >= _SPEED_STATUS_PRINT_INTERVAL_SEC: + self._last_print_ts = ts + print( + f"\rspeed: {speed_mph:6.2f} mph max: {self._max_mph:6.2f} mph ", + end="", + flush=True, + ) + if not self._tripped and speed_mph > SPEED_LIMIT_MPH: + self._tripped = True + logger.error( + f"!!! FASTLIO ODOMETRY DIVERGED !!! reported {speed_mph:.1f} mph " + f"(limit {SPEED_LIMIT_MPH:.1f} mph). Latching warnings." + ) + + +_LIDAR_IP = os.getenv("LIDAR_IP", "192.168.1.107") + + +unitree_go2_record = autoconnect( + GO2Connection.blueprint(), + KeyboardTeleop.blueprint(), + MovementManager.blueprint(), + Mid360.blueprint( + lidar_ip=_LIDAR_IP, + ).remappings( + [ + (Mid360, "lidar", "livox_lidar"), + (Mid360, "imu", "livox_imu"), + ] + ), + FastLio2.blueprint( + frame_id="world", + map_freq=-1, + lidar_ip=_LIDAR_IP, + ).remappings( + [ + (FastLio2, "lidar", "fastlio_lidar"), + (FastLio2, "odometry", "fastlio_odometry"), + ] + ), + Go2Mid360Memory.blueprint(lidar_ip=_LIDAR_IP), + SpeedWarner.blueprint(), +).global_config(n_workers=10, robot_model="unitree_go2") From f172f5a67b09181c2fb7aed6fa3cd657ccc974be Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 1 Jun 2026 15:34:00 -0700 Subject: [PATCH 002/185] frame fix --- dimos/hardware/sensors/lidar/fastlio2/module.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 2c8ab22dfc..7c7ff59ff8 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -171,8 +171,8 @@ def start(self) -> None: def _on_odom_for_tf(self, msg: Odometry) -> None: self.tf.publish( Transform( - frame_id=FRAME_ODOM, - child_frame_id=FRAME_BODY, + frame_id=self.frame_id, + child_frame_id=self.config.child_frame_id, translation=Vector3( msg.pose.position.x, msg.pose.position.y, From 0a74a33c0766918d66540a1a3df31d6e7e64b8a8 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 1 Jun 2026 15:50:16 -0700 Subject: [PATCH 003/185] add max_velocity --- .../sensors/lidar/fastlio2/cpp/flake.lock | 8 +++---- .../sensors/lidar/fastlio2/cpp/flake.nix | 2 +- .../sensors/lidar/fastlio2/cpp/main.cpp | 8 +++++++ .../hardware/sensors/lidar/fastlio2/module.py | 7 +++++++ .../blueprints/smart/unitree_go2_record.py | 21 ++++++++++--------- 5 files changed, 31 insertions(+), 15 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock index ed10ba8629..615efd3891 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock @@ -37,16 +37,16 @@ "fast-lio": { "flake": false, "locked": { - "lastModified": 1778784133, - "narHash": "sha256-bZEmIJlrzcqrHLxCiXXXf7fgi2yjVRuad8z08HN1eMU=", + "lastModified": 1780353421, + "narHash": "sha256-HL64XVYsbiEbXqADj07BwX1Eij2Xm6vj+a2o0RQhSrs=", "owner": "dimensionalOS", "repo": "dimos-module-fastlio2", - "rev": "d93bc6795babe9e63cc77d2bf2b72294d9afa839", + "rev": "fd28a0995384352962558c4bf843d7bdfc17c1ed", "type": "github" }, "original": { "owner": "dimensionalOS", - "ref": "v0.3.0-quiet-logs", + "ref": "jeff/feat/velocity-cap", "repo": "dimos-module-fastlio2", "type": "github" } diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix index d73c4fd631..9ddaa468a7 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix @@ -13,7 +13,7 @@ flake = false; }; fast-lio = { - url = "github:dimensionalOS/dimos-module-fastlio2/v0.3.0-quiet-logs"; + url = "github:dimensionalOS/dimos-module-fastlio2/jeff/feat/velocity-cap"; flake = false; }; lcm-extended = { diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index 5c53381aa3..114ceabc60 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -411,6 +411,11 @@ int main(int argc, char** argv) { float map_max_range = mod.arg_float("map_max_range", 100.0f); float map_freq = mod.arg_float("map_freq", 0.0f); + // Optional override of the FAST-LIO post-IESKF-update velocity cap (m/s). + // Zero (default) means use the upstream default; any positive value + // overrides. Size to the platform's physical envelope. + double max_velocity_norm_ms = mod.arg_float("max_velocity_norm_ms", 0.0f); + // Verbose logging — propagates to the FAST-LIO C++ core via the // `fastlio_debug` global. Default false → only real errors print. bool debug = mod.arg_bool("debug", false); @@ -487,6 +492,9 @@ int main(int argc, char** argv) { // Init FAST-LIO with config if (debug) printf("[fastlio2] Initializing FAST-LIO...\n"); FastLio fast_lio(config_path, msr_freq, main_freq); + if (max_velocity_norm_ms > 0.0) { + fast_lio.set_max_velocity_norm_ms(max_velocity_norm_ms); + } g_fastlio = &fast_lio; if (debug) printf("[fastlio2] FAST-LIO initialized.\n"); diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 7c7ff59ff8..259a726987 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -107,6 +107,13 @@ class FastLio2Config(NativeModuleConfig): map_voxel_size: float = 0.1 map_max_range: float = 100.0 + # VERY IMPORTANT + # this is used to prevent catestrophic divergence + # go2 dog should set this to 3.1 m/s + # it needs some buffer room (dog can't actually move that fast) + # but other than that buffer room, tigher=less chance of catestrophic divergence + max_velocity_norm_ms: float = 0.0 + # FAST-LIO YAML config (relative to config/ dir, or absolute path) # C++ binary reads YAML directly via yaml-cpp config: Annotated[ diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py index 1a3c985a5d..4beb59070d 100644 --- a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py +++ b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py @@ -46,9 +46,6 @@ logger = setup_logger() -# tcpdump fails fast (EPERM, bad iface) within a few ms; pause briefly so poll() catches that. -_TCPDUMP_STARTUP_PROBE_SEC = 0.3 - def _stamp() -> str: now = datetime.now() @@ -65,22 +62,22 @@ class Go2Mid360MemoryConfig(RecorderConfig): recording_dir: Path = Field(default_factory=_default_recording_dir) # Filled in by model_post_init below if left at the default. db_path: str | Path = "" - pcap_path: str | Path = "" default_frame_id: str = "base_link" - # tcpdump configuration. Capture is filtered to UDP from the lidar IP. - record_pcap: bool = True + # tcpdump configuration. Pcap recording is opt-in: set record_pcap=True to + # enable. pcap_path defaults to /mid360.pcap when unset. + record_pcap: bool = False + pcap_path: Path | None = None record_pcap_iface: str = "enp2s0" record_pcap_snaplen: int = 2048 lidar_ip: str = "192.168.1.107" def model_post_init(self, __context: object) -> None: super().model_post_init(__context) - # Resolve db/pcap paths from recording_dir if the caller didn't set them. if not self.db_path: self.db_path = self.recording_dir / "data.db" - if not self.pcap_path: + if self.record_pcap and self.pcap_path is None: self.pcap_path = self.recording_dir / "mid360.pcap" @@ -103,6 +100,9 @@ class Go2Mid360Memory(Recorder): livox_lidar: In[PointCloud2] livox_imu: In[Imu] + # tcpdump fails fast (EPERM, bad iface) within a few ms; pause briefly so poll() catches that. + _TCPDUMP_STARTUP_PROBE_SEC: float = 0.3 + _pcap_proc: subprocess.Popen[bytes] | None = None @rpc @@ -174,7 +174,7 @@ def _start_pcap(self) -> None: start_new_session=True, ) # tcpdump exits within a few ms on EPERM; wait briefly so we can detect that. - time.sleep(_TCPDUMP_STARTUP_PROBE_SEC) + time.sleep(self._TCPDUMP_STARTUP_PROBE_SEC) if proc.poll() is not None: stderr = proc.stderr.read().decode(errors="replace") if proc.stderr else "" self._pcap_proc = None @@ -292,12 +292,13 @@ async def handle_fastlio_odometry(self, msg: Odometry) -> None: frame_id="world", map_freq=-1, lidar_ip=_LIDAR_IP, + max_velocity_norm_ms=3.1, ).remappings( [ (FastLio2, "lidar", "fastlio_lidar"), (FastLio2, "odometry", "fastlio_odometry"), ] ), - Go2Mid360Memory.blueprint(lidar_ip=_LIDAR_IP), + Go2Mid360Memory.blueprint(lidar_ip=_LIDAR_IP, record_pcap=True), SpeedWarner.blueprint(), ).global_config(n_workers=10, robot_model="unitree_go2") From 63070e99e98308e8e2aa5c35a9edfb5e3d49b1c9 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 1 Jun 2026 15:51:30 -0700 Subject: [PATCH 004/185] refactor: extract FastLio2Recorder into its own module Move the recorder + tcpdump pcap logic out of the go2_record blueprint into dimos/hardware/sensors/lidar/fastlio2/recorder.py. Pcap recording is now opt-in (record_pcap defaults to False), and the default paths land under ./go2_recordings//{mem2.db,raw_mid360.pcap}. --- .../sensors/lidar/fastlio2/recorder.py | 216 ++++++++++++++++++ dimos/robot/all_blueprints.py | 2 +- .../blueprints/smart/unitree_go2_record.py | 194 +--------------- 3 files changed, 219 insertions(+), 193 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/fastlio2/recorder.py diff --git a/dimos/hardware/sensors/lidar/fastlio2/recorder.py b/dimos/hardware/sensors/lidar/fastlio2/recorder.py new file mode 100644 index 0000000000..737860d8f0 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/recorder.py @@ -0,0 +1,216 @@ +# Copyright 2025-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 datetime import datetime +import os +from pathlib import Path +import shutil +import signal +import subprocess +import time +from typing import Any + +from pydantic import Field +from reactivex.disposable import Disposable + +from dimos.core.core import rpc +from dimos.core.stream import In +from dimos.memory2.module import Recorder, RecorderConfig +from dimos.memory2.stream import Stream +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.Imu import Imu +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +def _stamp() -> str: + now = datetime.now() + return now.strftime("%Y-%m-%d") + "_" + now.strftime("%-I-%M%p").lower() + "-PST" + + +def _default_recording_dir() -> Path: + return Path("go2_recordings") / _stamp() + + +class FastLio2RecorderConfig(RecorderConfig): + """One recording dir per session: /mem2.db plus /raw_mid360.pcap.""" + + recording_dir: Path = Field(default_factory=_default_recording_dir) + # Filled in by model_post_init below if left at the default. + db_path: str | Path = "" + + default_frame_id: str = "base_link" + + # tcpdump configuration. Pcap recording is opt-in: set record_pcap=True to + # enable. pcap_path defaults to /mid360.pcap when unset. + record_pcap: bool = False + pcap_path: Path | None = None + record_pcap_iface: str = "enp2s0" + record_pcap_snaplen: int = 2048 + lidar_ip: str = "192.168.1.107" + + def model_post_init(self, __context: object) -> None: + super().model_post_init(__context) + if not self.db_path: + self.db_path = self.recording_dir / "mem2.db" + if self.record_pcap and self.pcap_path is None: + self.pcap_path = self.recording_dir / "raw_mid360.pcap" + + +class FastLio2Recorder(Recorder): + """Records FastLio2 inputs and outputs for offline replay: raw Livox + Mid-360 lidar + IMU into the SDK, FastLio2's registered lidar and + odometry out, plus any companion streams (e.g. Go2 camera/leg odom) + that help interpret the run. + + Also owns the tcpdump process that captures the raw Mid-360 UDP + packets — this is the ground-truth input the FastLio2 binary can be + replayed against bit-for-bit. Single session = single timestamped dir + holding both the sqlite memory store and the pcap. + """ + + config: FastLio2RecorderConfig + + livox_lidar: In[PointCloud2] + livox_imu: In[Imu] + fastlio_lidar: In[PointCloud2] + fastlio_odometry: In[Odometry] + color_image: In[Image] + lidar: In[PointCloud2] + odom: In[PoseStamped] + + # tcpdump fails fast (EPERM, bad iface) within a few ms; pause briefly so poll() catches that. + _TCPDUMP_STARTUP_PROBE_SEC: float = 0.3 + + _pcap_proc: subprocess.Popen[bytes] | None = None + + @rpc + def start(self) -> None: + Path(self.config.recording_dir).mkdir(parents=True, exist_ok=True) + if self.config.record_pcap: + self._start_pcap() + super().start() + + @rpc + def stop(self) -> None: + super().stop() + self._stop_pcap() + + def _port_to_stream(self, name: str, input_topic: In[Any], stream: Stream[Any]) -> None: + """Append each message from *input_topic* to *stream*, attaching world pose via tf. + + Stamped messages use their own ``.frame_id`` and ``.ts``; unstamped + messages (or ones whose frame isn't in the tf graph, e.g. a payload + already in world coords) fall back to ``config.default_frame_id`` — + so every observation gets a robot-pose anchor when tf is publishing. + + Registers the subscription as a disposable on this module. + """ + + default_frame_id = self.config.default_frame_id + tf_tolerance = self.config.tf_tolerance + + def on_msg(msg: Any) -> None: + # Force system time for all messages + ts = time.time() + frame_id = ( + getattr(msg, "child_frame_id", None) + or getattr(msg, "frame_id", None) + or default_frame_id + ) + transform = self.tf.get("world", frame_id, time_point=ts, time_tolerance=tf_tolerance) + pose = transform.to_pose() if transform is not None else None + + stream.append(msg, ts=ts, pose=pose) + + self.register_disposable(Disposable(input_topic.subscribe(on_msg))) + + def _start_pcap(self) -> None: + cfg = self.config + path = Path(cfg.pcap_path).expanduser() + path.parent.mkdir(parents=True, exist_ok=True) + + # Capture every UDP packet originating from the lidar. + packet_filter_expression = f"src host {cfg.lidar_ip} and udp" + tcpdump = shutil.which("tcpdump") or "tcpdump" + cmd = [ + tcpdump, + "-i", + cfg.record_pcap_iface, + "-w", + str(path), + "-s", + str(cfg.record_pcap_snaplen), + "-U", + "-n", + packet_filter_expression, + ] + + proc = subprocess.Popen( + cmd, + stdout=subprocess.DEVNULL, + stderr=subprocess.PIPE, + start_new_session=True, + ) + # tcpdump exits within a few ms on EPERM; wait briefly so we can detect that. + time.sleep(self._TCPDUMP_STARTUP_PROBE_SEC) + if proc.poll() is not None: + stderr = proc.stderr.read().decode(errors="replace") if proc.stderr else "" + self._pcap_proc = None + logger.error( + f"FastLio2Recorder pcap recording failed to start — tcpdump exited" + f" rc={proc.returncode} stderr={stderr.strip()}" + ) + print( + "[go2_record] pcap recording is enabled but tcpdump cannot capture.\n" + " Grant capture capability once with:\n" + f" sudo setcap cap_net_raw,cap_net_admin=eip {tcpdump}\n" + " then restart. (tcpdump stderr above.)", + flush=True, + ) + return + + logger.info( + f"FastLio2Recorder pcap recording enabled path={path} " + f"iface={cfg.record_pcap_iface} filter={packet_filter_expression!r}" + ) + self._pcap_proc = proc + + def _stop_pcap(self) -> None: + proc = self._pcap_proc + if proc is None: + return + self._pcap_proc = None + if proc.poll() is not None: + return + # SIGINT is tcpdump's documented "stop cleanly" signal — it prints + # packet counts and flushes the pcap header. + proc.send_signal(signal.SIGINT) + try: + proc.wait(timeout=self.config.shutdown_timeout) + except subprocess.TimeoutExpired: + try: + os.killpg(os.getpgid(proc.pid), signal.SIGTERM) + except ProcessLookupError: + pass + try: + proc.wait(timeout=self.config.shutdown_timeout) + except subprocess.TimeoutExpired: + proc.kill() + proc.wait() + logger.info(f"FastLio2Recorder pcap recording stopped path={self.config.pcap_path}") diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 2cf1774ba5..c2b8fa1632 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -147,6 +147,7 @@ "emitter-module": "dimos.utils.demo_image_encoding.EmitterModule", "far-planner": "dimos.navigation.nav_stack.modules.far_planner.far_planner.FarPlanner", "fast-lio2": "dimos.hardware.sensors.lidar.fastlio2.module.FastLio2", + "fast-lio2-recorder": "dimos.hardware.sensors.lidar.fastlio2.recorder.FastLio2Recorder", "g1-connection": "dimos.robot.unitree.g1.connection.G1Connection", "g1-connection-base": "dimos.robot.unitree.g1.connection.G1ConnectionBase", "g1-high-level-dds-sdk": "dimos.robot.unitree.g1.effectors.high_level.dds_sdk.G1HighLevelDdsSdk", @@ -156,7 +157,6 @@ "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-memory": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_record.Go2Mid360Memory", "google-maps-skill-container": "dimos.agents.skills.google_maps_skill_container.GoogleMapsSkillContainer", "gps-nav-skill-container": "dimos.agents.skills.gps_nav_skill.GpsNavSkillContainer", "grasp-gen-module": "dimos.manipulation.grasping.graspgen_module.GraspGenModule", diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py index 4beb59070d..ba625d6d34 100644 --- a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py +++ b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py @@ -13,32 +13,17 @@ # See the License for the specific language governing permissions and # limitations under the License. -from datetime import datetime import math import os -from pathlib import Path -import shutil -import signal -import subprocess import time -from typing import Any - -from pydantic import Field -from reactivex.disposable import Disposable from dimos.core.coordination.blueprints import autoconnect -from dimos.core.core import rpc from dimos.core.module import Module from dimos.core.stream import In from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 +from dimos.hardware.sensors.lidar.fastlio2.recorder import FastLio2Recorder from dimos.hardware.sensors.lidar.livox.module import Mid360 -from dimos.memory2.module import Recorder, RecorderConfig -from dimos.memory2.stream import Stream -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.Imu import Imu -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.robot.unitree.go2.connection import GO2Connection from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop @@ -47,181 +32,6 @@ logger = setup_logger() -def _stamp() -> str: - now = datetime.now() - return now.strftime("%Y-%m-%d") + "_" + now.strftime("%-I-%M%p").lower() + "-PST" - - -def _default_recording_dir() -> Path: - return Path(f"recording_go2_mid360_{_stamp()}") - - -class Go2Mid360MemoryConfig(RecorderConfig): - """One recording dir per session: /data.db plus /mid360.pcap.""" - - recording_dir: Path = Field(default_factory=_default_recording_dir) - # Filled in by model_post_init below if left at the default. - db_path: str | Path = "" - - default_frame_id: str = "base_link" - - # tcpdump configuration. Pcap recording is opt-in: set record_pcap=True to - # enable. pcap_path defaults to /mid360.pcap when unset. - record_pcap: bool = False - pcap_path: Path | None = None - record_pcap_iface: str = "enp2s0" - record_pcap_snaplen: int = 2048 - lidar_ip: str = "192.168.1.107" - - def model_post_init(self, __context: object) -> None: - super().model_post_init(__context) - if not self.db_path: - self.db_path = self.recording_dir / "data.db" - if self.record_pcap and self.pcap_path is None: - self.pcap_path = self.recording_dir / "mid360.pcap" - - -class Go2Mid360Memory(Recorder): - """Records Go2 camera, native Go2 lidar, Mid-360 (lidar + IMU), FastLio2 - odometry, and Go2 leg odometry. - - Also owns the tcpdump process that captures raw UDP packets from the - Mid-360. Single session = single timestamped dir holding both the - sqlite memory store and the pcap. - """ - - config: Go2Mid360MemoryConfig - - color_image: In[Image] - lidar: In[PointCloud2] - odom: In[PoseStamped] - fastlio_lidar: In[PointCloud2] - fastlio_odometry: In[Odometry] - livox_lidar: In[PointCloud2] - livox_imu: In[Imu] - - # tcpdump fails fast (EPERM, bad iface) within a few ms; pause briefly so poll() catches that. - _TCPDUMP_STARTUP_PROBE_SEC: float = 0.3 - - _pcap_proc: subprocess.Popen[bytes] | None = None - - @rpc - def start(self) -> None: - Path(self.config.recording_dir).mkdir(parents=True, exist_ok=True) - if self.config.record_pcap: - self._start_pcap() - super().start() - - @rpc - def stop(self) -> None: - super().stop() - self._stop_pcap() - - def _port_to_stream(self, name: str, input_topic: In[Any], stream: Stream[Any]) -> None: - """Append each message from *input_topic* to *stream*, attaching world pose via tf. - - Stamped messages use their own ``.frame_id`` and ``.ts``; unstamped - messages (or ones whose frame isn't in the tf graph, e.g. a payload - already in world coords) fall back to ``config.default_frame_id`` — - so every observation gets a robot-pose anchor when tf is publishing. - - Registers the subscription as a disposable on this module. - """ - - default_frame_id = self.config.default_frame_id - tf_tolerance = self.config.tf_tolerance - - def on_msg(msg: Any) -> None: - # Force system time for all messages - ts = time.time() - frame_id = ( - getattr(msg, "child_frame_id", None) - or getattr(msg, "frame_id", None) - or default_frame_id - ) - transform = self.tf.get("world", frame_id, time_point=ts, time_tolerance=tf_tolerance) - pose = transform.to_pose() if transform is not None else None - - stream.append(msg, ts=ts, pose=pose) - - self.register_disposable(Disposable(input_topic.subscribe(on_msg))) - - def _start_pcap(self) -> None: - cfg = self.config - path = Path(cfg.pcap_path).expanduser() - path.parent.mkdir(parents=True, exist_ok=True) - - # Capture every UDP packet originating from the lidar. - packet_filter_expression = f"src host {cfg.lidar_ip} and udp" - tcpdump = shutil.which("tcpdump") or "tcpdump" - cmd = [ - tcpdump, - "-i", - cfg.record_pcap_iface, - "-w", - str(path), - "-s", - str(cfg.record_pcap_snaplen), - "-U", - "-n", - packet_filter_expression, - ] - - proc = subprocess.Popen( - cmd, - stdout=subprocess.DEVNULL, - stderr=subprocess.PIPE, - start_new_session=True, - ) - # tcpdump exits within a few ms on EPERM; wait briefly so we can detect that. - time.sleep(self._TCPDUMP_STARTUP_PROBE_SEC) - if proc.poll() is not None: - stderr = proc.stderr.read().decode(errors="replace") if proc.stderr else "" - self._pcap_proc = None - logger.error( - f"Go2Mid360Memory pcap recording failed to start — tcpdump exited" - f" rc={proc.returncode} stderr={stderr.strip()}" - ) - print( - "[go2_record] pcap recording is enabled but tcpdump cannot capture.\n" - " Grant capture capability once with:\n" - f" sudo setcap cap_net_raw,cap_net_admin=eip {tcpdump}\n" - " then restart. (tcpdump stderr above.)", - flush=True, - ) - return - - logger.info( - f"Go2Mid360Memory pcap recording enabled path={path} " - f"iface={cfg.record_pcap_iface} filter={packet_filter_expression!r}" - ) - self._pcap_proc = proc - - def _stop_pcap(self) -> None: - proc = self._pcap_proc - if proc is None: - return - self._pcap_proc = None - if proc.poll() is not None: - return - # SIGINT is tcpdump's documented "stop cleanly" signal — it prints - # packet counts and flushes the pcap header. - proc.send_signal(signal.SIGINT) - try: - proc.wait(timeout=self.config.shutdown_timeout) - except subprocess.TimeoutExpired: - try: - os.killpg(os.getpgid(proc.pid), signal.SIGTERM) - except ProcessLookupError: - pass - try: - proc.wait(timeout=self.config.shutdown_timeout) - except subprocess.TimeoutExpired: - proc.kill() - proc.wait() - logger.info(f"Go2Mid360Memory pcap recording stopped path={self.config.pcap_path}") - - MPH_PER_MPS = 2.23694 SPEED_LIMIT_MPH = 30.0 _SPEED_STATUS_PRINT_INTERVAL_SEC = 1.0 @@ -299,6 +109,6 @@ async def handle_fastlio_odometry(self, msg: Odometry) -> None: (FastLio2, "odometry", "fastlio_odometry"), ] ), - Go2Mid360Memory.blueprint(lidar_ip=_LIDAR_IP, record_pcap=True), + FastLio2Recorder.blueprint(lidar_ip=_LIDAR_IP, record_pcap=True), SpeedWarner.blueprint(), ).global_config(n_workers=10, robot_model="unitree_go2") From 9afdeb4cc3e106949dfe99c25eec02adc4103475 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 1 Jun 2026 17:55:10 -0700 Subject: [PATCH 005/185] temp go2 helper --- bin/go2_rec | 10 ++++++++++ 1 file changed, 10 insertions(+) create mode 100755 bin/go2_rec diff --git a/bin/go2_rec b/bin/go2_rec new file mode 100755 index 0000000000..8c8a5fe69e --- /dev/null +++ b/bin/go2_rec @@ -0,0 +1,10 @@ +#!/usr/bin/env bash + +# sudo ifconfig en7 inet 192.168.1.5 netmask 255.255.255.0 +source .venv/bin/activate +kd + +log_path="./go2_recordings/log.txt" +mkdir -p "$(dirname "$log_path")" +echo "NEW RECORDING: $(date)" >> "$log_path" +dimos --no-obstacle-avoidance --robot-ip 192.168.124.177 run unitree-go2-record | tee -a "$log_path" From 20a75a8588218a48d491144fdad401b292a4bfd5 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Tue, 2 Jun 2026 13:15:22 -0700 Subject: [PATCH 006/185] better tcpdump cleanup --- .../sensors/lidar/fastlio2/recorder.py | 33 +++++++++++++++++-- 1 file changed, 31 insertions(+), 2 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/recorder.py b/dimos/hardware/sensors/lidar/fastlio2/recorder.py index 737860d8f0..995082ce46 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/recorder.py +++ b/dimos/hardware/sensors/lidar/fastlio2/recorder.py @@ -15,9 +15,11 @@ from datetime import datetime import os from pathlib import Path +import shlex import shutil import signal import subprocess +import textwrap import time from typing import Any @@ -43,6 +45,30 @@ def _stamp() -> str: return now.strftime("%Y-%m-%d") + "_" + now.strftime("%-I-%M%p").lower() + "-PST" +def _stop_when_parent_dies(cmd: list[str]) -> list[str]: + parent_pid = os.getpid() + quoted = " ".join(shlex.quote(arg) for arg in cmd) + # script to work on macos and linux (theres a cleaner just-linux solution) + script = textwrap.dedent(f""" + {quoted} & + child=$! + trap 'kill -INT "$child" 2>/dev/null' INT TERM + ( + while kill -0 {parent_pid} 2>/dev/null; do + sleep 0.5 + done + kill -INT "$child" 2>/dev/null + ) & + watcher=$! + wait "$child" + code=$? + kill "$watcher" 2>/dev/null + wait "$watcher" 2>/dev/null + exit $code + """).strip() + return ["bash", "-c", script] + + def _default_recording_dir() -> Path: return Path("go2_recordings") / _stamp() @@ -162,7 +188,7 @@ def _start_pcap(self) -> None: ] proc = subprocess.Popen( - cmd, + _stop_when_parent_dies(cmd), stdout=subprocess.DEVNULL, stderr=subprocess.PIPE, start_new_session=True, @@ -211,6 +237,9 @@ def _stop_pcap(self) -> None: try: proc.wait(timeout=self.config.shutdown_timeout) except subprocess.TimeoutExpired: - proc.kill() + try: + os.killpg(os.getpgid(proc.pid), signal.SIGKILL) + except ProcessLookupError: + pass proc.wait() logger.info(f"FastLio2Recorder pcap recording stopped path={self.config.pcap_path}") From c6f2813fd601df60fdb86b27d1522d1fc5b01bd7 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Tue, 2 Jun 2026 18:44:44 -0700 Subject: [PATCH 007/185] record with transform frames --- .../sensors/lidar/fastlio2/speed_warner.py | 73 ++++++++ .../blueprints/smart/unitree_go2_record.py | 164 ++++++++++++------ 2 files changed, 187 insertions(+), 50 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/fastlio2/speed_warner.py diff --git a/dimos/hardware/sensors/lidar/fastlio2/speed_warner.py b/dimos/hardware/sensors/lidar/fastlio2/speed_warner.py new file mode 100644 index 0000000000..3532e41614 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/speed_warner.py @@ -0,0 +1,73 @@ +# Copyright 2025-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. + +import math +import time + +from dimos.core.module import Module +from dimos.core.stream import In +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +MPH_PER_MPS = 2.23694 +SPEED_LIMIT_MPH_WARNING = 30.0 +_SPEED_STATUS_PRINT_INTERVAL_SEC = 1.0 + + +class SpeedWarner(Module): + """Watches fastlio_odometry; once speed ever exceeds the limit (impossible for the Go2, + so it indicates the FastLio2 estimate has diverged / sensor is about to crash), + latches and spams an error on every subsequent odom message until restart. + + FastLio2's C++ publisher hardcodes twist to zero (cpp/main.cpp), so msg.vx/vy/vz + are always 0. Speed is derived from pose deltas instead. + """ + + fastlio_odometry: In[Odometry] + + _tripped: bool = False + _max_mph: float = 0.0 + _last_pos: tuple[float, float, float] | None = None + _last_ts: float | None = None + _last_print_ts: float = 0.0 + + async def handle_fastlio_odometry(self, msg: Odometry) -> None: + ts = msg.ts or time.time() + pos = (msg.pose.x, msg.pose.y, msg.pose.z) + last_pos, last_ts = self._last_pos, self._last_ts + self._last_pos, self._last_ts = pos, ts + if last_pos is None or last_ts is None: + return + dt = ts - last_ts + if dt <= 0: + return + dx, dy, dz = pos[0] - last_pos[0], pos[1] - last_pos[1], pos[2] - last_pos[2] + speed_mph = math.sqrt(dx * dx + dy * dy + dz * dz) / dt * MPH_PER_MPS + if speed_mph > self._max_mph: + self._max_mph = speed_mph + if ts - self._last_print_ts >= _SPEED_STATUS_PRINT_INTERVAL_SEC: + self._last_print_ts = ts + print( + f"\rspeed: {speed_mph:6.2f} mph max: {self._max_mph:6.2f} mph ", + end="", + flush=True, + ) + if not self._tripped and speed_mph > SPEED_LIMIT_MPH_WARNING: + self._tripped = True + logger.error( + f"!!! FASTLIO ODOMETRY DIVERGED !!! reported {speed_mph:.1f} mph " + f"(limit {SPEED_LIMIT_MPH_WARNING:.1f} mph). Latching warnings." + ) diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py index ba625d6d34..970d4f02b5 100644 --- a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py +++ b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py @@ -16,80 +16,128 @@ import math import os import time +from typing import Any + +from reactivex.disposable import Disposable from dimos.core.coordination.blueprints import autoconnect -from dimos.core.module import Module +from dimos.core.coordination.module_coordinator import ModuleCoordinator +from dimos.core.global_config import global_config from dimos.core.stream import In from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 -from dimos.hardware.sensors.lidar.fastlio2.recorder import FastLio2Recorder +from dimos.hardware.sensors.lidar.fastlio2.recorder import FastLio2Recorder, _default_recording_dir +from dimos.hardware.sensors.lidar.fastlio2.speed_warner import SpeedWarner from dimos.hardware.sensors.lidar.livox.module import Mid360 +from dimos.memory2.stream import Stream +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.msgs.nav_msgs.Odometry import Odometry from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.robot.unitree.go2.connection import GO2Connection from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop -from dimos.utils.logging_config import setup_logger +from dimos.utils.logging_config import set_run_log_dir, setup_logger logger = setup_logger() +# mid360_link is physically measured relative to camera (easier than measuring to base_link): +# relative to camera the lidar is 3.2cm back, 12cm up, pitched 44deg down. +# base_link -> front_camera -> mid360_link +BASE_TO_FRONT_CAMERA = Transform( + translation=Vector3(0.32715, -0.00003, 0.04297), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + frame_id="base_link", + child_frame_id="front_camera", +) + +_MID360_PITCH_HALF = math.radians(44.0) / 2.0 +BASE_TO_MID360 = BASE_TO_FRONT_CAMERA + Transform( + translation=Vector3(-0.032, 0.0, 0.12), + rotation=Quaternion(0.0, math.sin(_MID360_PITCH_HALF), 0.0, math.cos(_MID360_PITCH_HALF)), + frame_id="front_camera", + child_frame_id="mid360_link", +) +MID360_TO_BASE = BASE_TO_MID360.inverse() + +# base_link -> camera_optical using the URDF front_camera mount plus the +# standard ROS optical-frame rotation (x-right, y-down, z-forward). +BASE_TO_CAMERA_OPTICAL = BASE_TO_FRONT_CAMERA + Transform( + translation=Vector3(0.0, 0.0, 0.0), + rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), + frame_id="front_camera", + child_frame_id="camera_optical", +) -MPH_PER_MPS = 2.23694 -SPEED_LIMIT_MPH = 30.0 -_SPEED_STATUS_PRINT_INTERVAL_SEC = 1.0 + +_LIDAR_IP = os.getenv("LIDAR_IP", "192.168.1.107") -class SpeedWarner(Module): - """Watches fastlio_odometry; once speed ever exceeds the limit (impossible for the Go2, - so it indicates the FastLio2 estimate has diverged / sensor is about to crash), - latches and spams an error on every subsequent odom message until restart. +class Go2TfHackRecorder(FastLio2Recorder): + """Records with statically-applied transforms instead of querying tf. - FastLio2's C++ publisher hardcodes twist to zero (cpp/main.cpp), so msg.vx/vy/vz - are always 0. Speed is derived from pose deltas instead. + FastLio2 tracks the Mid-360 (``mid360_link``) and reports its pose in the + ``world`` frame as ``fastlio_odometry``; its registered cloud is likewise + already in that world frame. We anchor recorded observations to the robot + body, building every pose from the latest fastlio odom and fixed mounts: + + - ``fastlio_lidar`` -> ``base_link`` pose in world (odom, then mid360_link -> base_link) + - ``color_image`` -> ``camera_optical`` pose in world (odom, mid360_link -> base_link, + then base_link -> camera_optical) + - everything else (odom streams included) -> no pose """ - fastlio_odometry: In[Odometry] - - _tripped: bool = False - _max_mph: float = 0.0 - _last_pos: tuple[float, float, float] | None = None - _last_ts: float | None = None - _last_print_ts: float = 0.0 - - async def handle_fastlio_odometry(self, msg: Odometry) -> None: - ts = msg.ts or time.time() - pos = (msg.pose.x, msg.pose.y, msg.pose.z) - last_pos, last_ts = self._last_pos, self._last_ts - self._last_pos, self._last_ts = pos, ts - if last_pos is None or last_ts is None: - return - dt = ts - last_ts - if dt <= 0: - return - dx, dy, dz = pos[0] - last_pos[0], pos[1] - last_pos[1], pos[2] - last_pos[2] - speed_mph = math.sqrt(dx * dx + dy * dy + dz * dz) / dt * MPH_PER_MPS - if speed_mph > self._max_mph: - self._max_mph = speed_mph - if ts - self._last_print_ts >= _SPEED_STATUS_PRINT_INTERVAL_SEC: - self._last_print_ts = ts - print( - f"\rspeed: {speed_mph:6.2f} mph max: {self._max_mph:6.2f} mph ", - end="", - flush=True, - ) - if not self._tripped and speed_mph > SPEED_LIMIT_MPH: - self._tripped = True - logger.error( - f"!!! FASTLIO ODOMETRY DIVERGED !!! reported {speed_mph:.1f} mph " - f"(limit {SPEED_LIMIT_MPH:.1f} mph). Latching warnings." - ) + _latest_fastlio_odom: Odometry | None = None + _warning_names: set[str] = set() + def _port_to_stream(self, name: str, input_topic: In[Any], stream: Stream[Any]) -> None: + def on_msg(msg: Any) -> None: + ts = time.time() + pose = None + if name == "fastlio_odometry": + self._latest_fastlio_odom = msg + elif name == "fastlio_lidar": + world_to_base = self._world_to_base_from_fastlio() + if world_to_base is not None: + pose = world_to_base.to_pose() + elif name == "color_image": + # anchor images to world frame as defined by fastlio odom + world_to_base = self._world_to_base_from_fastlio() + if world_to_base is not None: + pose = (world_to_base + BASE_TO_CAMERA_OPTICAL).to_pose() + elif "odom" in name: + pass + else: + if name not in self._warning_names: + self._warning_names.add(name) + logger.warning(f"cannot compute pose for {name}; recording without pose") -_LIDAR_IP = os.getenv("LIDAR_IP", "192.168.1.107") + stream.append(msg, ts=ts, pose=pose) + + self.register_disposable(Disposable(input_topic.subscribe(on_msg))) + + def _world_to_base_from_fastlio(self) -> Transform | None: + odom = self._latest_fastlio_odom + if odom is None: + return None + world_to_mid360 = Transform( + translation=odom.position, + rotation=odom.orientation, + frame_id="world", + child_frame_id="mid360_link", + ts=odom.ts, + ) + return world_to_mid360 + MID360_TO_BASE unitree_go2_record = autoconnect( - GO2Connection.blueprint(), KeyboardTeleop.blueprint(), MovementManager.blueprint(), + GO2Connection.blueprint().remappings( + [ + (GO2Connection, "lidar", "go2_lidar"), + (GO2Connection, "odom", "go2_odom"), + ] + ), Mid360.blueprint( lidar_ip=_LIDAR_IP, ).remappings( @@ -109,6 +157,22 @@ async def handle_fastlio_odometry(self, msg: Odometry) -> None: (FastLio2, "odometry", "fastlio_odometry"), ] ), - FastLio2Recorder.blueprint(lidar_ip=_LIDAR_IP, record_pcap=True), + Go2TfHackRecorder.blueprint(lidar_ip=_LIDAR_IP, record_pcap=True).remappings( + [ + (Go2TfHackRecorder, "lidar", "go2_lidar"), + (Go2TfHackRecorder, "odom", "go2_odom"), + ] + ), SpeedWarner.blueprint(), ).global_config(n_workers=10, robot_model="unitree_go2") + + +if __name__ == "__main__": + recording_dir = _default_recording_dir() + set_run_log_dir(recording_dir) + global_config.obstacle_avoidance = False + coordinator = ModuleCoordinator.build( + unitree_go2_record, + {FastLio2Recorder.name: {"recording_dir": recording_dir}}, + ) + coordinator.loop() From 6049881b5eb92af74d3bb0fab617d4b72dbe080d Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Tue, 2 Jun 2026 19:24:30 -0700 Subject: [PATCH 008/185] feat: go2 offline recording post-process + apriltag quality filtering Add the offline post-processing pipeline for Go2 + Livox recordings: - recording/{apriltags,gtsam_gt,lidar_reanchor,build_rrd,camera,rec_check} - scripts/go2_mid360_post_process.py orchestrator AprilTag detection now drops distant/oblique glimpses (keep <=1m, head-on within 45deg), clusters same-id detections within 5s, and emits one medoid representative per cluster (most spatially/rotationally central). --- .../robot/unitree/go2/recording/apriltags.py | 201 ++++++++ .../robot/unitree/go2/recording/build_rrd.py | 429 ++++++++++++++++++ dimos/robot/unitree/go2/recording/camera.py | 32 ++ dimos/robot/unitree/go2/recording/gtsam_gt.py | 184 ++++++++ .../unitree/go2/recording/lidar_reanchor.py | 114 +++++ .../robot/unitree/go2/recording/rec_check.py | 232 ++++++++++ .../go2/scripts/go2_mid360_post_process.py | 218 +++++++++ 7 files changed, 1410 insertions(+) create mode 100644 dimos/robot/unitree/go2/recording/apriltags.py create mode 100644 dimos/robot/unitree/go2/recording/build_rrd.py create mode 100644 dimos/robot/unitree/go2/recording/camera.py create mode 100644 dimos/robot/unitree/go2/recording/gtsam_gt.py create mode 100644 dimos/robot/unitree/go2/recording/lidar_reanchor.py create mode 100644 dimos/robot/unitree/go2/recording/rec_check.py create mode 100755 dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py diff --git a/dimos/robot/unitree/go2/recording/apriltags.py b/dimos/robot/unitree/go2/recording/apriltags.py new file mode 100644 index 0000000000..79f92987cd --- /dev/null +++ b/dimos/robot/unitree/go2/recording/apriltags.py @@ -0,0 +1,201 @@ +# 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. + +"""AprilTag detection over a mem2.db color stream. + +Detects tags in the `color_image` stream, keeps only close head-on views, and +clusters same-id detections in time into a single medoid representative each. +(Re)writes the `april_tags` PoseStamped stream of tag-in-camera relative poses +(solvePnP, marker_id in each observation's tags) and returns those +representatives for the GTSAM solve. +""" + +from __future__ import annotations + +from collections import defaultdict +import math + +import cv2 +import numpy as np +from scipy.spatial.transform import Rotation + +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.Image import Image +from dimos.robot.unitree.go2.recording.camera import CAMERA_DISTORTION, CAMERA_INTRINSICS + + +def make_detector(dictionary_name: str): + d = cv2.aruco.getPredefinedDictionary(getattr(cv2.aruco, dictionary_name)) + return cv2.aruco.ArucoDetector(d, cv2.aruco.DetectorParameters()) + + +def _object_points(marker_length_m: float) -> np.ndarray: + h = marker_length_m / 2.0 + return np.array([[-h, h, 0.0], [h, h, 0.0], [h, -h, 0.0], [-h, -h, 0.0]], dtype=np.float32) + + +def estimate_marker_pose(corners_pixels, marker_length_m): + """solvePnP a single tag -> (rotation_vector, translation_vector) in the + camera_optical frame, or None if it failed.""" + image_corners = corners_pixels.reshape(4, 1, 2).astype(np.float32) + found, rotation_vector, translation_vector = cv2.solvePnP( + _object_points(marker_length_m), + image_corners, + CAMERA_INTRINSICS, + CAMERA_DISTORTION, + flags=cv2.SOLVEPNP_IPPE_SQUARE, + ) + return (rotation_vector, translation_vector) if found else None + + +def view_quality(t_cam_marker: list[float]) -> tuple[float, float]: + """(distance_m, view_angle_deg) for a tag pose in the camera optical frame. + + distance is the camera->tag range; view_angle is the angle between the line + of sight and the tag's surface normal (0 = perfectly head-on).""" + translation = np.array(t_cam_marker[:3], dtype=np.float64) + distance = float(np.linalg.norm(translation)) + normal = Rotation.from_quat(t_cam_marker[3:7]).as_matrix()[:, 2] + line_of_sight = translation / (distance + 1e-9) + cos_angle = abs(float(np.dot(line_of_sight, normal))) + view_angle = math.degrees(math.acos(min(1.0, cos_angle))) + return distance, view_angle + + +def cluster_by_time(detections: list[dict], gap_sec: float) -> list[list[dict]]: + """Group same-marker detections into clusters. A new cluster begins whenever + the time gap to the previous same-marker detection exceeds gap_sec.""" + clusters: list[list[dict]] = [] + by_marker: dict[int, list[dict]] = defaultdict(list) + for detection in detections: + by_marker[detection["marker_id"]].append(detection) + for marker_detections in by_marker.values(): + marker_detections.sort(key=lambda detection: detection["ts"]) + current = [marker_detections[0]] + for detection in marker_detections[1:]: + if detection["ts"] - current[-1]["ts"] > gap_sec: + clusters.append(current) + current = [detection] + else: + current.append(detection) + clusters.append(current) + return clusters + + +def _pose_distance(a: list[float], b: list[float], rotation_weight_m_per_rad: float) -> float: + translation = float(np.linalg.norm(np.array(a[:3]) - np.array(b[:3]))) + rotation = 2.0 * math.acos(min(1.0, abs(float(np.dot(a[3:7], b[3:7]))))) + return translation + rotation_weight_m_per_rad * rotation + + +def cluster_medoid(cluster: list[dict], rotation_weight_m_per_rad: float) -> dict: + """The detection whose pose is most central (min total spatial+rotational + distance to the rest) — a robust representative of the cluster.""" + poses = [detection["t_cam_marker"] for detection in cluster] + best_index, best_cost = 0, float("inf") + for i in range(len(poses)): + cost = sum( + _pose_distance(poses[i], poses[j], rotation_weight_m_per_rad) + for j in range(len(poses)) + if j != i + ) + if cost < best_cost: + best_cost, best_index = cost, i + return cluster[best_index] + + +def detect_apriltags( + store, + image_stream="color_image", + stream_name="april_tags", + marker_length=0.10, + dictionary="DICT_APRILTAG_36h11", + *, + max_distance_m=1.0, + max_view_angle_deg=45.0, + cluster_gap_sec=5.0, + rotation_weight_m_per_rad=0.5, +): + """Detect tags in `image_stream`, keep only close, head-on views, cluster + same-id detections by time, and (re)write the `april_tags` stream from one + medoid representative per cluster. Returns that list of representatives.""" + detector = make_detector(dictionary) + raw_detections: list[dict] = [] + images = store.stream(image_stream, Image).to_list() + for image_obs in images: + image = image_obs.data + bgr = image.numpy() if hasattr(image, "numpy") else np.asarray(image.data) + all_corners, marker_ids, _ = detector.detectMarkers(bgr) + if marker_ids is None: + continue + for corners, marker_id in zip(all_corners, marker_ids.flatten(), strict=False): + pose = estimate_marker_pose(corners, marker_length) + if pose is None: + continue + rotation_vector, translation_vector = pose + quaternion = Rotation.from_rotvec(rotation_vector.reshape(3)).as_quat() # x,y,z,w + translation = translation_vector.reshape(3) + tag_in_camera = [ + float(translation[0]), + float(translation[1]), + float(translation[2]), + float(quaternion[0]), + float(quaternion[1]), + float(quaternion[2]), + float(quaternion[3]), + ] + raw_detections.append( + { + "ts": float(image_obs.ts), + "marker_id": int(marker_id), + "t_cam_marker": tag_in_camera, + } + ) + + # Quality gate: drop distant or oblique glimpses, keep close head-on views. + kept = [] + for detection in raw_detections: + distance, view_angle = view_quality(detection["t_cam_marker"]) + if distance <= max_distance_m and view_angle <= max_view_angle_deg: + kept.append(detection) + + # One representative (medoid) per time-clustered group of same-id detections. + detections = [] + for cluster in cluster_by_time(kept, cluster_gap_sec): + detections.append( + {**cluster_medoid(cluster, rotation_weight_m_per_rad), "n_observations": len(cluster)} + ) + detections.sort(key=lambda detection: detection["ts"]) + + if stream_name in store.list_streams(): + store.delete_stream(stream_name) + april_tag_stream = store.stream(stream_name, PoseStamped) + for detection in detections: + tag_in_camera = detection["t_cam_marker"] + pose_stamped = PoseStamped( + ts=detection["ts"], position=tag_in_camera[:3], orientation=tag_in_camera[3:] + ) + april_tag_stream.append( + pose_stamped, + ts=detection["ts"], + pose=tuple(tag_in_camera), + tags={"marker_id": detection["marker_id"]}, + ) + + found_ids = sorted({detection["marker_id"] for detection in detections}) + print( + f" april_tags: {len(raw_detections)} raw -> {len(kept)} in-spec " + f"-> {len(detections)} clusters, markers {found_ids} (over {len(images)} images)" + ) + return detections diff --git a/dimos/robot/unitree/go2/recording/build_rrd.py b/dimos/robot/unitree/go2/recording/build_rrd.py new file mode 100644 index 0000000000..13e9cb10cf --- /dev/null +++ b/dimos/robot/unitree/go2/recording/build_rrd.py @@ -0,0 +1,429 @@ +# 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. + +"""Dump a go2 mem2.db to a Rerun .rrd (adapted from dimos +mapping/loop_closure/utils/map_rrd.py), focused on the post-processed result. + +For each lidar stream it logs both the per-frame clouds and a single aggregated, +voxel-downsampled "map" (corrected fastlio re-anchored on `gtsam_odom`; the Go2 +`lidar` stays in its own `odom` frame). Clouds get a slight height-color +gradient; trajectories get a start->end gradient. Each AprilTag is placed in 3D +(via the corrected trajectory) with its detections, a labeled marker, basis- +vector axes at the perceived pose, and the robot's-eye camera image at the +recognition moment. +""" + +from __future__ import annotations + +import math +import sqlite3 + +import numpy as np +import rerun as rr +import rerun.blueprint as rrb +from scipy.spatial.transform import Rotation + +from dimos.memory2.store.sqlite import SqliteStore +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.robot.unitree.go2.recording.camera import CAMERA_INTRINSICS + +TIMELINE = "ts" +# one distinct base color per point cloud (height modulates brightness within each) +_CLOUD_PALETTE = [(0, 180, 170), (240, 160, 40), (220, 90, 180), (150, 200, 60), (90, 150, 235)] +_TAG_PALETTE = [ + (255, 80, 80), + (80, 200, 255), + (255, 220, 60), + (160, 100, 255), + (90, 230, 130), + (255, 150, 60), +] + + +def _mat(trans, quat) -> np.ndarray: + M = np.eye(4) + M[:3, :3] = Rotation.from_quat(quat).as_matrix() + M[:3, 3] = trans + return M + + +def _mat7(p) -> np.ndarray: + return _mat(p[:3], p[3:7]) + + +# fastlio reports the mid360 (lidar) frame, which is mounted pitched 44 deg down +# relative to the front camera; place tags/camera by chaining mid360 -> camera. +# (base_link cancels out, so we only need the camera<->mid360 relationship.) +_PITCH_HALF = math.radians(44.0) / 2.0 +_M_FC_MID360 = _mat([-0.032, 0.0, 0.12], [0.0, math.sin(_PITCH_HALF), 0.0, math.cos(_PITCH_HALF)]) +_M_FC_OPTICAL = _mat([0.0, 0.0, 0.0], [-0.5, 0.5, -0.5, 0.5]) # REP-103 optical in camera body +MID360_TO_OPTICAL = np.linalg.inv(_M_FC_MID360) @ _M_FC_OPTICAL # legacy: optical pts -> mid360 pts +# default (correct-transform data): odom frame already aligned with the camera body, +# so just the camera mount offset + optical convention, no mid360 pitch hack. +BASE_TO_OPTICAL = _mat([0.32715, 0.0, 0.04297], [-0.5, 0.5, -0.5, 0.5]) + + +def _pose7(p): + if hasattr(p, "orientation"): + o = p.orientation + return [p.x, p.y, p.z, o.x, o.y, o.z, o.w] + return list(p) + + +def _down(pts: np.ndarray, voxel: float) -> np.ndarray: + if voxel <= 0 or len(pts) == 0: + return pts + k = np.floor(pts / voxel).astype(np.int64) + (1 << 20) + key = k[:, 0] | (k[:, 1] << 21) | (k[:, 2] << 42) + _, idx = np.unique(key, return_index=True) + return pts[idx] + + +def _shaded(pts: np.ndarray, base) -> np.ndarray: + """Per-cloud color: the cloud's base color, brightness modulated by height + (darker low -> brighter high). Distinct hue per cloud, subtle within-cloud.""" + z = pts[:, 2] + lo, hi = np.percentile(z, 5), np.percentile(z, 95) + t = np.clip((z - lo) / (hi - lo + 1e-9), 0, 1)[:, None] + return (np.array(base, np.float32) * (0.5 + 0.5 * t)).astype(np.uint8) + + +def _log_frames(store, stream, entity, stride, voxel, base): + if stream not in store.list_streams(): + return + n = 0 + for k, obs in enumerate(store.stream(stream, PointCloud2)): + if k % stride: + continue + pts = _down(obs.data.points_f32(), voxel) + if len(pts) == 0: + continue + rr.set_time(TIMELINE, timestamp=obs.ts) + rr.log(entity, rr.Points3D(pts, colors=_shaded(pts, base))) + n += 1 + print(f" rrd: {entity} <- {stream} ({n} frames, stride {stride}, voxel {voxel}m)") + + +def _log_map(store, stream, entity, voxel, base): + if stream not in store.list_streams(): + return + chunks = [ + c for c in (obs.data.points_f32() for obs in store.stream(stream, PointCloud2)) if len(c) + ] + if not chunks: + return + pts = _down(np.concatenate(chunks), voxel) + rr.log(entity, rr.Points3D(pts, colors=_shaded(pts, base)), static=True) + print(f" rrd: {entity} <- {stream} ({len(pts):,} pts, voxel {voxel}m)") + + +def _path(db_path, stream): + try: + conn = sqlite3.connect(db_path) + rows = conn.execute( + f'SELECT pose_x,pose_y,pose_z FROM "{stream}" WHERE pose_x IS NOT NULL ORDER BY ts' + ).fetchall() + conn.close() + except sqlite3.OperationalError: + return None + return np.array(rows, float) if rows else None + + +def _log_odom_frames(db_path, stride=5): + """A moving XYZ basis-vector triad per odom stream (Transform3D over time + + a static axis triad). Doubles as the eye's tracking target.""" + for stream, name in [ + ("gtsam_odom", "gtsam"), + ("go2_odom", "go2"), + ("fastlio_odometry", "fastlio"), + ("odom", "odom"), + ]: + try: + conn = sqlite3.connect(db_path) + rows = conn.execute( + f"SELECT ts,pose_x,pose_y,pose_z,pose_qx,pose_qy,pose_qz,pose_qw " + f'FROM "{stream}" WHERE pose_qw IS NOT NULL ORDER BY ts' + ).fetchall() + conn.close() + except sqlite3.OperationalError: + continue + if not rows: + continue + ent = f"world/{name}_frame" + rr.log( + ent, + rr.Arrows3D( + vectors=[[0.5, 0, 0], [0, 0.5, 0], [0, 0, 0.5]], + colors=[[255, 0, 0], [0, 255, 0], [0, 0, 255]], + ), + static=True, + ) + # green box marking the odom source (child -> inherits the moving transform) + rr.log( + f"{ent}/box", + rr.Boxes3D(half_sizes=[[0.35, 0.2, 0.15]], colors=[[0, 220, 0]]), + static=True, + ) + for r in rows[::stride]: + rr.set_time(TIMELINE, timestamp=r[0]) + rr.log( + ent, rr.Transform3D(translation=r[1:4], quaternion=rr.Quaternion(xyzw=list(r[4:8]))) + ) + print(f" rrd: {ent} <- {stream} ({len(rows[::stride])} frames, basis axes)") + + +def _log_path_gradient(db_path, stream, entity, base): + """Log a trajectory as per-segment line strips with a start->end brightness + gradient (dim at start, full color at end) so direction/time is visible.""" + pts = _path(db_path, stream) + if pts is None or len(pts) < 2: + return + segs = [pts[i : i + 2] for i in range(len(pts) - 1)] + t = np.linspace(0.0, 1.0, len(segs))[:, None] + base = np.array(base, np.float32) + colors = ((0.25 + 0.75 * t) * base).astype(np.uint8) + rr.log(entity, rr.LineStrips3D(segs, colors=colors), static=True) + print(f" rrd: {entity} <- {stream} ({len(pts)} poses, gradient)") + + +def _has_rows(conn, stream): + try: + return ( + conn.execute(f'SELECT 1 FROM "{stream}" WHERE pose_qw IS NOT NULL LIMIT 1').fetchone() + is not None + ) + except sqlite3.OperationalError: + return False + + +def _log_apriltags(store, db_path, cam_xform, max_views_per_tag=40): + """Place every AprilTag recognition in 3D via the corrected trajectory: + T_world_tag = T_world_base(t) . T_base_optical . T_cam_tag. Per marker logs + the detection cloud + a labeled marker, and for a sample of recognitions: + XYZ basis axes at the perceived tag pose and the robot's-eye camera image on + a pinhole frustum at the camera pose (3D only — see the blueprint).""" + if "april_tags" not in store.list_streams(): + return + connection = sqlite3.connect(db_path) + traj_stream = "gtsam_odom" if _has_rows(connection, "gtsam_odom") else "fastlio_odometry" + pose_rows = connection.execute( + f"SELECT ts,pose_x,pose_y,pose_z,pose_qx,pose_qy,pose_qz,pose_qw " + f'FROM "{traj_stream}" WHERE pose_qw IS NOT NULL ORDER BY ts' + ).fetchall() + connection.close() + if not pose_rows: + return + traj_timestamps = np.array([row[0] for row in pose_rows]) + traj_poses = np.array([row[1:8] for row in pose_rows], float) + + def optical_in_world(timestamp): + """T_world_optical = T_world_traj . (traj-frame -> optical).""" + index = int(np.searchsorted(traj_timestamps, timestamp)) + return _mat7(traj_poses[min(max(index, 0), len(traj_timestamps) - 1)]) @ cam_xform + + detections_by_marker: dict[int, dict] = {} + for detection_obs in store.stream("april_tags", PoseStamped): + marker_id = (detection_obs.tags or {}).get("marker_id") + if marker_id is None or detection_obs.pose is None: + continue + tag_world = optical_in_world(detection_obs.ts) @ _mat7(_pose7(detection_obs.pose)) + entry = detections_by_marker.setdefault(int(marker_id), {"poses": [], "ts": []}) + entry["poses"].append(tag_world) + entry["ts"].append(detection_obs.ts) + + camera_targets = [] # (entity, T_world_optical, ts) for image logging + for palette_index, (marker_id, entry) in enumerate(sorted(detections_by_marker.items())): + tag_poses = np.array(entry["poses"]) + timestamps = np.array(entry["ts"]) + positions = tag_poses[:, :3, 3] + color = _TAG_PALETTE[palette_index % len(_TAG_PALETTE)] + center = np.median(positions, 0) + rr.log( + f"world/april_tags/marker_{marker_id}/detections", + rr.Points3D(positions, colors=color, radii=0.1), + static=True, + ) + rr.log( + f"world/april_tags/marker_{marker_id}", + rr.Points3D( + [center], colors=color, radii=0.5, labels=[f"tag {marker_id}"], show_labels=True + ), + static=True, + ) + # sample recognitions: axes at the perceived tag pose + a camera frustum + sample_stride = max(1, len(tag_poses) // max_views_per_tag) + sampled = range(0, len(tag_poses), sample_stride) + for sample_index in sampled: + tag_pose = tag_poses[sample_index] + tag_entity = f"world/april_tags/marker_{marker_id}/recognitions/{sample_index:04d}/tag" + rr.log( + tag_entity, + rr.Transform3D(translation=tag_pose[:3, 3], mat3x3=tag_pose[:3, :3]), + static=True, + ) + rr.log( + tag_entity, + rr.Arrows3D( + vectors=[[0.3, 0, 0], [0, 0.3, 0], [0, 0, 0.3]], + colors=[[255, 0, 0], [0, 255, 0], [0, 0, 255]], + ), + static=True, + ) + cam_entity = f"world/april_tags/marker_{marker_id}/recognitions/{sample_index:04d}/cam" + camera_targets.append( + (cam_entity, optical_in_world(timestamps[sample_index]), timestamps[sample_index]) + ) + print( + f" rrd: world/april_tags/marker_{marker_id} @ {center.round(1).tolist()} " + f"({len(positions)} detections, {len(sampled)} views, via {traj_stream})" + ) + + _log_cam_frustums(store, camera_targets) + + +def _log_cam_frustums(store, camera_targets): + """Place the robot's-eye color image on a pinhole frustum at the camera pose + for each (entity, T_world_optical, ts) target — rendered only in 3D.""" + if not camera_targets or "color_image" not in store.list_streams(): + return + nearest = [(1e18, None) for _ in camera_targets] # (time delta, image obs) per target + for image_obs in store.stream("color_image", Image): + for target_index, (_entity, _pose, target_ts) in enumerate(camera_targets): + delta = abs(image_obs.ts - target_ts) + if delta < nearest[target_index][0]: + nearest[target_index] = (delta, image_obs) + logged = 0 + for (entity, optical_pose, _target_ts), (_delta, image_obs) in zip( + camera_targets, nearest, strict=False + ): + if image_obs is None: + continue + rr.log( + entity, + rr.Transform3D(translation=optical_pose[:3, 3], mat3x3=optical_pose[:3, :3]), + static=True, + ) + rr.log( + entity, + rr.Pinhole( + image_from_camera=CAMERA_INTRINSICS, + resolution=[1280, 720], + camera_xyz=rr.ViewCoordinates.RDF, + image_plane_distance=0.6, + ), + static=True, + ) + try: + rr.log(f"{entity}/rgb", image_obs.data.to_rerun(), static=True) + logged += 1 + except Exception: + pass + print(f" rrd: {logged} recognition camera frustums (robot's-eye images, 3D only)") + + +def build_rrd( + db_path: str, + out_path: str, + *, + map_voxel: float = 0.1, + cloud_stride: int = 3, + camera_stride: int = 30, + mid360_pitch: bool = False, +): + rr.init("go2_post_process", recording_id=str(out_path)) + rr.save(str(out_path)) + cam_xform = MID360_TO_OPTICAL if mid360_pitch else BASE_TO_OPTICAL + + with SqliteStore(path=db_path) as store: + streams = store.list_streams() + # Explicit blueprint: a 3D view (incl. the recognition camera frustums) + + # a 2D panel for the live camera, so rerun doesn't auto-make a panel per + # pinhole. Heavy aggregated maps default to hidden (toggle in the entity + # panel); the eye tracks the primary odom frame; left/bottom panels open. + track = ( + "/world/go2_frame" + if "go2_odom" in streams + else "/world/fastlio_frame" + if "fastlio_odometry" in streams + else "/world/gtsam_frame" + ) + hide = { + f"/world/{m}": rrb.EntityBehavior(visible=False) + for m in ( + "go2_map", + "corrected_go2_map", + "fastlio_map", + "corrected_fastlio_map", + "onboard_map", + ) + } + rr.send_blueprint( + rrb.Blueprint( + rrb.Horizontal( + rrb.Spatial3DView( + origin="/world", + name="3D", + overrides=hide, + eye_controls=rrb.EyeControls3D( + kind=rrb.Eye3DKind.Orbital, tracking_entity=track + ), + ), + rrb.Spatial2DView(origin="/world/camera", name="camera"), + column_shares=[3, 1], + ), + rrb.BlueprintPanel(state=rrb.PanelState.Expanded), + rrb.TimePanel(state=rrb.PanelState.Expanded), + rrb.SelectionPanel(state=rrb.PanelState.Collapsed), + ) + ) + + ci = 0 # rotate a distinct base color through each point cloud + for raw, corr, name in [ + ("go2_lidar", "go2_lidar_corrected", "go2"), # new: correct transforms + ("fastlio_lidar", "fastlio_lidar_corrected", "fastlio"), # legacy mid360 + ("lidar", None, "onboard"), # legacy Go2 onboard L1, own frame + ]: + for stream, ent in [(raw, name), (corr, f"corrected_{name}")]: + if stream and stream in streams: + base = _CLOUD_PALETTE[ci % len(_CLOUD_PALETTE)] + ci += 1 + _log_frames(store, stream, f"world/{ent}_lidar", cloud_stride, map_voxel, base) + _log_map(store, stream, f"world/{ent}_map", map_voxel, base) + + _log_apriltags(store, db_path, cam_xform) + + for stream, entity, base in [ + ("gtsam_odom", "world/gtsam_path", (0, 220, 0)), # corrected GT -> green + ("go2_odom", "world/go2_path", (220, 200, 0)), # go2 odom -> yellow + ("fastlio_odometry", "world/fastlio_path", (0, 200, 220)), # cyan + ("odom", "world/odom_path", (255, 165, 0)), # Go2 onboard odom -> orange + ]: + _log_path_gradient(db_path, stream, entity, base) + _log_odom_frames(db_path) # moving XYZ basis triads (+ eye tracking target) + + if "color_image" in streams: + n = 0 + for k, obs in enumerate(store.stream("color_image", Image)): + if k % camera_stride: + continue + rr.set_time(TIMELINE, timestamp=obs.ts) + try: + rr.log("world/camera/image", obs.data.to_rerun()) + n += 1 + except Exception: + break + print(f" rrd: world/camera/image <- color_image ({n} frames, stride {camera_stride})") + print(f" wrote {out_path}") diff --git a/dimos/robot/unitree/go2/recording/camera.py b/dimos/robot/unitree/go2/recording/camera.py new file mode 100644 index 0000000000..255164eacf --- /dev/null +++ b/dimos/robot/unitree/go2/recording/camera.py @@ -0,0 +1,32 @@ +# 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. + +"""Go2 front RGB camera rig (1280x720) — shared intrinsics/extrinsic. + +Same values as front_camera_720.yaml. Edit here for a different rig. +""" + +from __future__ import annotations + +import numpy as np + +# Pinhole intrinsic matrix K (fx, fy, cx, cy). +CAMERA_INTRINSICS = np.array( + [[797.47561649, 0.0, 643.53521678], [0.0, 796.48721128, 349.27836053], [0.0, 0.0, 1.0]], + dtype=np.float64, +) +# Lens distortion coefficients (k1, k2, p1, p2). +CAMERA_DISTORTION = np.array([-0.07309429, -0.02341141, -0.00693059, 0.00923868], dtype=np.float64) +# Pose of the camera_optical frame expressed in base_link: [x, y, z, qx, qy, qz, qw]. +CAMERA_OPTICAL_IN_BASE = [0.3, 0.0, 0.0, -0.5, 0.5, -0.5, 0.5] diff --git a/dimos/robot/unitree/go2/recording/gtsam_gt.py b/dimos/robot/unitree/go2/recording/gtsam_gt.py new file mode 100644 index 0000000000..97b0c03423 --- /dev/null +++ b/dimos/robot/unitree/go2/recording/gtsam_gt.py @@ -0,0 +1,184 @@ +# 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. + +"""GTSAM landmark-SLAM groundtruth from AprilTag observations. + +Treats the FAST-LIO / odom pose chain as locally correct and the AprilTags as +static landmarks; a tag seen at several times pins the chain and removes +accumulated odometry drift. Trusts tag POSITION (solvePnP is metric) but +distrusts tag ORIENTATION (a small planar tag is yaw/pitch ambiguous), and wraps +tag factors in a Huber kernel so a bad detection can't dominate. +""" + +from __future__ import annotations + +import sqlite3 + +import numpy as np + +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.robot.unitree.go2.recording.camera import CAMERA_OPTICAL_IN_BASE + + +def _pose_from7(pose7): + """[x y z qx qy qz qw] -> gtsam.Pose3.""" + import gtsam + + return gtsam.Pose3( + gtsam.Rot3.Quaternion(pose7[6], pose7[3], pose7[4], pose7[5]), + gtsam.Point3(pose7[0], pose7[1], pose7[2]), + ) + + +def _pose_to7(pose3): + """gtsam.Pose3 -> [x y z qx qy qz qw].""" + quaternion = pose3.rotation().toQuaternion() + translation = pose3.translation() + return [ + translation[0], + translation[1], + translation[2], + quaternion.x(), + quaternion.y(), + quaternion.z(), + quaternion.w(), + ] + + +def pick_pose_stream(connection) -> str: + """The odom stream to use as the pose chain (go2_odom / fastlio_odometry preferred).""" + stream_names = [row[0] for row in connection.execute("SELECT name FROM _streams").fetchall()] + candidates = [name for name in ["go2_odom", "fastlio_odometry"] if name in stream_names] + candidates += [ + name for name in stream_names if "odom" in name.lower() and name not in candidates + ] + for name in candidates: + try: + populated = connection.execute( + f'SELECT count(*) FROM "{name}" WHERE pose_qw IS NOT NULL' + ).fetchone()[0] + except sqlite3.OperationalError: + continue + if populated > 0: + return name + raise SystemExit(f"no odom stream with populated pose columns among {candidates}") + + +def build_gtsam_gt( + db_path, + markers, + *, + node_stride=3, + odom_rot_sig=0.004, + odom_trans_sig=0.02, + tag_rot_sig=1.0, + tag_trans_sig=0.1, + tag_huber=0.5, +): + """Landmark-SLAM the odom chain + AprilTag landmarks. Returns [(ts, pose7), ...].""" + import gtsam + from gtsam import BetweenFactorPose3, PriorFactorPose3 + from gtsam.symbol_shorthand import L, X + + connection = sqlite3.connect(db_path) + pose_stream = pick_pose_stream(connection) + pose_rows = connection.execute( + f"SELECT ts,pose_x,pose_y,pose_z,pose_qx,pose_qy,pose_qz,pose_qw " + f'FROM "{pose_stream}" WHERE pose_qw IS NOT NULL ORDER BY ts' + ).fetchall() + connection.close() + pose_rows = pose_rows[::node_stride] + node_timestamps = np.array([row[0] for row in pose_rows]) + node_poses = [_pose_from7(row[1:8]) for row in pose_rows] + num_nodes = len(pose_rows) + print( + f" gtsam: pose stream '{pose_stream}', {num_nodes} nodes (stride {node_stride}), " + f"{len(markers)} tag obs" + ) + + base_to_optical = _pose_from7(CAMERA_OPTICAL_IN_BASE) + + def nearest_node(timestamp): + node_index = int(np.searchsorted(node_timestamps, timestamp)) + node_index = min(max(node_index, 0), num_nodes - 1) + if node_index > 0 and abs(node_timestamps[node_index - 1] - timestamp) < abs( + node_timestamps[node_index] - timestamp + ): + node_index -= 1 + return node_index + + graph = gtsam.NonlinearFactorGraph() + initial = gtsam.Values() + prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.full(6, 1e-4)) + odom_noise = gtsam.noiseModel.Diagonal.Sigmas( + np.array([odom_rot_sig] * 3 + [odom_trans_sig] * 3) + ) + tag_noise_base = gtsam.noiseModel.Diagonal.Sigmas( + np.array([tag_rot_sig] * 3 + [tag_trans_sig] * 3) + ) + tag_noise = gtsam.noiseModel.Robust.Create( + gtsam.noiseModel.mEstimator.Huber.Create(tag_huber), tag_noise_base + ) + + for node_index in range(num_nodes): + initial.insert(X(node_index), node_poses[node_index]) + graph.add(PriorFactorPose3(X(0), node_poses[0], prior_noise)) + for node_index in range(num_nodes - 1): + relative = node_poses[node_index].between(node_poses[node_index + 1]) + graph.add(BetweenFactorPose3(X(node_index), X(node_index + 1), relative, odom_noise)) + + landmark_ids = set() + for detection in markers: + marker_id = int(detection["marker_id"]) + node_index = nearest_node(detection["ts"]) + tag_in_body = base_to_optical.compose(_pose_from7(detection["t_cam_marker"])) + if marker_id not in landmark_ids: + initial.insert(L(marker_id), node_poses[node_index].compose(tag_in_body)) + landmark_ids.add(marker_id) + graph.add(BetweenFactorPose3(X(node_index), L(marker_id), tag_in_body, tag_noise)) + + params = gtsam.LevenbergMarquardtParams() + params.setMaxIterations(100) + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) + result = optimizer.optimize() + corrections = [ + np.linalg.norm( + result.atPose3(X(node_index)).translation() - node_poses[node_index].translation() + ) + for node_index in range(num_nodes) + ] + print( + f" gtsam: landmarks {sorted(landmark_ids)} | correction max {max(corrections):.2f} m, " + f"mean {np.mean(corrections):.2f} m ({optimizer.iterations()} iters)" + ) + return [ + (float(node_timestamps[node_index]), _pose_to7(result.atPose3(X(node_index)))) + for node_index in range(num_nodes) + ] + + +def write_gtsam_odom(store, trajectory, stream_name, tum_path): + """Write the corrected trajectory as a PoseStamped stream + a .tum file.""" + if stream_name in store.list_streams(): + store.delete_stream(stream_name) + odom_stream = store.stream(stream_name, PoseStamped) + with open(tum_path, "w") as tum_file: + for timestamp, pose in trajectory: + odom_stream.append( + PoseStamped(ts=timestamp, position=pose[:3], orientation=pose[3:7]), + ts=timestamp, + pose=tuple(pose), + ) + tum_file.write(f"{timestamp:.9f} " + " ".join(f"{value:.9f}" for value in pose) + "\n") + print(f" wrote '{stream_name}' stream ({len(trajectory)} poses) + {tum_path}") diff --git a/dimos/robot/unitree/go2/recording/lidar_reanchor.py b/dimos/robot/unitree/go2/recording/lidar_reanchor.py new file mode 100644 index 0000000000..1d9cc559c2 --- /dev/null +++ b/dimos/robot/unitree/go2/recording/lidar_reanchor.py @@ -0,0 +1,114 @@ +# 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. + +"""Re-anchor a world-frame lidar stream onto a corrected trajectory. + +A lidar stream is stored in world frame *relative to some odometry* (e.g. +`fastlio_lidar` is relative to `fastlio_odometry`, `lidar` is relative to the +Go2 onboard `odom`). When a drift-corrected trajectory exists (`gtsam_odom`, +from AprilTag landmark SLAM), each cloud can be re-anchored onto it: + + P_corrected = T_gtsam(t) · T_odom(t)^-1 · P_world + +i.e. subtract the cloud's own odometry (back to body) and re-apply the corrected +pose at the nearest timestamp. The actual point transform runs through +open3d via ``PointCloud2.transform``. +""" + +from __future__ import annotations + +import sqlite3 + +import numpy as np +from scipy.spatial.transform import Rotation + +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.msgs.sensor_msgs.PointCloud2 import PointCloud2 + + +def _load_poses(db_path: str, stream: str): + """(ts (N,), pose (N,7) x y z qx qy qz qw) for a pose-bearing stream, via SQL.""" + conn = sqlite3.connect(db_path) + rows = conn.execute( + f"SELECT ts,pose_x,pose_y,pose_z,pose_qx,pose_qy,pose_qz,pose_qw " + f'FROM "{stream}" WHERE pose_qw IS NOT NULL ORDER BY ts' + ).fetchall() + conn.close() + if not rows: + raise SystemExit(f"no populated poses in stream '{stream}'") + return np.array([r[0] for r in rows]), np.array([r[1:8] for r in rows], dtype=np.float64) + + +def _mat(p): + M = np.eye(4) + M[:3, :3] = Rotation.from_quat(p[3:7]).as_matrix() + M[:3, 3] = p[:3] + return M + + +def _to_transform(M): + q = Rotation.from_matrix(M[:3, :3]).as_quat() + return Transform( + translation=Vector3(float(M[0, 3]), float(M[1, 3]), float(M[2, 3])), + rotation=Quaternion(float(q[0]), float(q[1]), float(q[2]), float(q[3])), + ) + + +def _nearest(ts_arr: np.ndarray, t: float) -> int: + j = int(np.searchsorted(ts_arr, t)) + j = min(max(j, 0), len(ts_arr) - 1) + if j > 0 and abs(ts_arr[j - 1] - t) < abs(ts_arr[j] - t): + j -= 1 + return j + + +def reanchor_stream( + store, db_path: str, *, lidar_stream: str, odom_stream: str, gtsam_stream: str, out_stream: str +) -> int: + """Write `out_stream`: every cloud of `lidar_stream` re-anchored from + `odom_stream` onto `gtsam_stream`. Returns the number of clouds written.""" + odom_ts, odom_pose = _load_poses(db_path, odom_stream) + gt_ts, gt_pose = _load_poses(db_path, gtsam_stream) + # precompute per-odom-node the corrected relative transform isn't possible + # (it's per-cloud by timestamp), but caching odom/gtsam matrices is cheap. + odom_M = [None] * len(odom_ts) + gt_M = [None] * len(gt_ts) + + src = store.stream(lidar_stream, PointCloud2).to_list() + if out_stream in store.list_streams(): + store.delete_stream(out_stream) + out = store.stream(out_stream, PointCloud2) + n = 0 + for obs in src: + t = obs.ts + i = _nearest(odom_ts, t) + j = _nearest(gt_ts, t) + if odom_M[i] is None: + odom_M[i] = _mat(odom_pose[i]) + if gt_M[j] is None: + gt_M[j] = _mat(gt_pose[j]) + rel = gt_M[j] @ np.linalg.inv(odom_M[i]) + cloud = obs.data + corrected = cloud.transform(_to_transform(rel)) # open3d under the hood + nc = PointCloud2.from_numpy( + corrected.points_f32(), timestamp=t, intensities=cloud.intensities_f32() + ) + out.append(nc, ts=t, pose=tuple(gt_pose[j])) + n += 1 + print( + f" {out_stream}: {n} clouds re-anchored ({lidar_stream} via {odom_stream} -> {gtsam_stream})" + ) + return n diff --git a/dimos/robot/unitree/go2/recording/rec_check.py b/dimos/robot/unitree/go2/recording/rec_check.py new file mode 100644 index 0000000000..5780b1b213 --- /dev/null +++ b/dimos/robot/unitree/go2/recording/rec_check.py @@ -0,0 +1,232 @@ +#!/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. + +"""Sanity-check a go2 recording dir: pcap + mem2.db sizes, stream rates, pose travel.""" + +from __future__ import annotations + +from datetime import datetime +import math +from pathlib import Path +import sqlite3 +import subprocess +import sys + +STREAMS = ( + "livox_imu", + "livox_lidar", + "lidar", + "fastlio_lidar", + "fastlio_odometry", + "odom", + "color_image", +) +RECORDINGS_DIR = Path("go2_recordings") + + +def find_dir(argv: list[str]) -> Path: + if len(argv) > 1: + directory = Path(argv[1]) + if not directory.exists(): + sys.exit(f"not found: {directory}") + return directory + candidates = sorted( + (p for p in RECORDINGS_DIR.glob("2*") if p.is_dir()), + key=lambda p: p.stat().st_mtime, + ) + if not candidates: + sys.exit(f"no recordings under {RECORDINGS_DIR}/") + return candidates[-1] + + +def human_size(n: int) -> str: + for unit in ("B", "KB", "MB", "GB", "TB"): + if n < 1024: + return f"{n:.1f}{unit}" + n /= 1024 # type: ignore[assignment] + return f"{n:.1f}PB" + + +def pcap_stats(pcap: Path) -> tuple[int, float, float] | None: + try: + result = subprocess.run( + ["capinfos", "-Mra", str(pcap)], + capture_output=True, + text=True, + check=True, + ) + except (FileNotFoundError, subprocess.CalledProcessError): + return _pcap_stats_via_tcpdump(pcap) + packets = first = last = None + for line in result.stdout.splitlines(): + if "Number of packets" in line: + packets = int(line.split(":", 1)[1].strip().replace(",", "")) + elif "First packet time" in line: + first = _parse_capinfos_time(line.split(":", 1)[1].strip()) + elif "Last packet time" in line: + last = _parse_capinfos_time(line.split(":", 1)[1].strip()) + if packets is None or first is None or last is None: + return None + return packets, first, last + + +def _parse_capinfos_time(value: str) -> float | None: + for fmt in ("%Y-%m-%d %H:%M:%S.%f", "%Y-%m-%d %H:%M:%S"): + try: + return datetime.strptime(value.split(" UTC")[0], fmt).timestamp() + except ValueError: + continue + return None + + +def _pcap_stats_via_tcpdump(pcap: Path) -> tuple[int, float, float] | None: + try: + result = subprocess.run( + ["tcpdump", "-r", str(pcap), "-tt", "-nn"], + capture_output=True, + text=True, + check=True, + ) + except (FileNotFoundError, subprocess.CalledProcessError): + return None + timestamps = [] + for line in result.stdout.splitlines(): + head = line.split(" ", 1)[0] + try: + timestamps.append(float(head)) + except ValueError: + continue + if not timestamps: + return None + return len(timestamps), timestamps[0], timestamps[-1] + + +def stream_rows(cur: sqlite3.Cursor, name: str) -> tuple[int, float | None, float | None, int]: + tables = {row[0] for row in cur.execute("SELECT name FROM sqlite_master WHERE type='table'")} + if name not in tables: + return 0, None, None, 0 + n, t0, t1 = cur.execute(f'SELECT COUNT(*), MIN(ts), MAX(ts) FROM "{name}"').fetchone() + pose_non_null = cur.execute(f'SELECT COUNT(pose_x) FROM "{name}"').fetchone()[0] + return n, t0, t1, pose_non_null + + +def odometry_travel(cur: sqlite3.Cursor) -> dict | None: + rows = cur.execute( + "SELECT pose_x, pose_y, pose_z FROM fastlio_odometry WHERE pose_x IS NOT NULL ORDER BY ts" + ).fetchall() + if not rows: + return None + xs, ys, zs = zip(*rows, strict=False) + path_length = sum(math.dist(rows[i - 1], rows[i]) for i in range(1, len(rows))) + return { + "rows": len(rows), + "start": rows[0], + "end": rows[-1], + "path_length": path_length, + "straight_line": math.dist(rows[0], rows[-1]), + "bbox_x": (min(xs), max(xs)), + "bbox_y": (min(ys), max(ys)), + "bbox_z": (min(zs), max(zs)), + } + + +def format_clock(seconds: float | None) -> str: + if seconds is None: + return "-" + return datetime.fromtimestamp(seconds).strftime("%H:%M:%S") + + +def main() -> int: + return report(find_dir(sys.argv)) + + +def report(directory: Path) -> int: + print(f"=== {directory} ===") + print() + + pcap = directory / "raw_mid360.pcap" + db = directory / "mem2.db" + print("files:") + for path in (pcap, db, directory / "mem2.db-wal", directory / "mem2.db-shm"): + if path.exists(): + print(f" {path.name:<20} {human_size(path.stat().st_size):>10}") + else: + print(f" {path.name:<20} (missing)") + print() + + if pcap.exists() and pcap.stat().st_size > 24: + stats = pcap_stats(pcap) + if stats is None: + print("pcap: present (capinfos/tcpdump unavailable to inspect)") + else: + packets, first, last = stats + span = last - first + rate = packets / span if span > 0 else 0 + print( + f"pcap: {packets:,} packets {format_clock(first)} -> {format_clock(last)} " + f"span={span:.1f}s rate={rate:.0f}pkt/s" + ) + elif pcap.exists(): + print(f"pcap: empty (only {pcap.stat().st_size}B — header only)") + else: + print("pcap: missing") + print() + + if not db.exists(): + print("mem2.db missing — cannot check streams.") + return 1 + + connection = sqlite3.connect(db) + cur = connection.cursor() + header = f"{'stream':<18} {'rows':>9} {'span_s':>8} {'hz':>7} {'pose%':>7} blob" + print(header) + print("-" * len(header)) + for name in STREAMS: + n, t0, t1, pose_n = stream_rows(cur, name) + if n == 0: + print(f" {name:<16} {'-':>9} (no rows)") + continue + span = (t1 - t0) if (t0 and t1) else 0 + rate = (n - 1) / span if span > 0 else 0 + pose_pct = 100 * pose_n / n if n else 0 + blob = cur.execute( + f'SELECT LENGTH(b.data) FROM "{name}" t JOIN "{name}_blob" b ON t.id=b.id LIMIT 1' + ).fetchone() + blob_label = human_size(blob[0]) if blob else "-" + print(f" {name:<16} {n:>9,} {span:>8.1f} {rate:>7.1f} {pose_pct:>6.0f}% {blob_label}") + + travel = odometry_travel(cur) + print() + if travel: + sx, sy, sz = travel["start"] + ex, ey, ez = travel["end"] + bx, by, bz = travel["bbox_x"], travel["bbox_y"], travel["bbox_z"] + print("fastlio_odometry travel:") + print(f" start x={sx:.2f} y={sy:.2f} z={sz:.2f}") + print(f" end x={ex:.2f} y={ey:.2f} z={ez:.2f}") + print(f" path_length {travel['path_length']:.2f} m") + print(f" straight_line {travel['straight_line']:.2f} m") + print( + f" bbox x=[{bx[0]:.1f},{bx[1]:.1f}] " + f"y=[{by[0]:.1f},{by[1]:.1f}] z=[{bz[0]:.1f},{bz[1]:.1f}]" + ) + else: + print("fastlio_odometry travel: no pose-stamped rows") + + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py b/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py new file mode 100755 index 0000000000..5a2b46954b --- /dev/null +++ b/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py @@ -0,0 +1,218 @@ +#!/usr/bin/env python +# 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. + +"""Post-process Go2 + Livox recordings: add AprilTag-corrected groundtruth + .rrd. + +Thin orchestrator over dimos/robot/unitree/go2/recording/*. For every `mem2.db` +under a recordings directory it: + 1. prints a recording sanity check (rec_check), + 2. detects AprilTags -> `april_tags` stream (recording.apriltags), + 3. solves a drift-corrected trajectory -> `gtsam_odom` (recording.gtsam_gt), + 4. re-anchors the lidar onto it -> `_corrected` (recording.lidar_reanchor), + 5. writes a Rerun `.rrd` visualization (recording.build_rrd). + +A tag seen at several times pins the odometry chain and removes accumulated +drift. Also writes `gtsam_odom.tum` next to each db (relocalization groundtruth). + +Run in a python env with dimos + cv2 + gtsam + scipy (from the dimos repo): + + uv run --no-sync python \ + dimos/robot/unitree/go2/scripts/go2_post_process.py [--recordings-dir DIR] [--force] + +Default recordings dir: ~/datasets/go2_recordings (each recording in its own +subdir with mem2.db). Already-processed dbs are skipped unless --force. +""" + +from __future__ import annotations + +import argparse +from pathlib import Path + +from dimos.memory2.store.sqlite import SqliteStore +from dimos.robot.unitree.go2.recording import rec_check +from dimos.robot.unitree.go2.recording.apriltags import detect_apriltags +from dimos.robot.unitree.go2.recording.build_rrd import build_rrd +from dimos.robot.unitree.go2.recording.gtsam_gt import build_gtsam_gt, write_gtsam_odom +from dimos.robot.unitree.go2.recording.lidar_reanchor import reanchor_stream + +# Lidar/odom pairs that may be re-anchored onto gtsam_odom — only when their odom +# is the same frame family gtsam was built from (so the re-anchor composes). The +# legacy Go2 onboard `lidar`/`odom` is a different estimator frame -> left as-is. +REANCHOR_PAIRS = [("go2_lidar", "go2_odom"), ("fastlio_lidar", "fastlio_odometry")] + + +def correct_db(db: Path, *, image_stream, apriltag_stream, gtsam_stream, marker_length, dictionary): + """AprilTag detection -> GTSAM trajectory -> re-anchor lidar. Returns True if + a corrected trajectory was written.""" + with SqliteStore(path=str(db)) as store: + detections = detect_apriltags( + store, image_stream, apriltag_stream, marker_length, dictionary + ) + if not detections: + print(" no AprilTags detected — skipping gtsam_odom (no landmark constraints)") + return False + trajectory = build_gtsam_gt(str(db), detections) + with SqliteStore(path=str(db)) as store: + write_gtsam_odom(store, trajectory, gtsam_stream, db.parent / "gtsam_odom.tum") + stream_names = store.list_streams() + for lidar_stream, odom_stream in REANCHOR_PAIRS: + if lidar_stream in stream_names and odom_stream in stream_names: + try: + reanchor_stream( + store, + str(db), + lidar_stream=lidar_stream, + odom_stream=odom_stream, + gtsam_stream=gtsam_stream, + out_stream=f"{lidar_stream}_corrected", + ) + except Exception as error: + print(f" re-anchor {lidar_stream}_corrected failed: {error}") + return True + + +def process_db( + db: Path, + *, + image_stream, + apriltag_stream, + gtsam_stream, + marker_length, + dictionary, + force, + no_gtsam=False, + make_rrd=True, + camera_freq=30, + map_voxel=0.1, + cloud_stride=3, + mid360_pitch=False, +): + print(f">> {db}") + try: + rec_check.report(db.parent) + except Exception as error: + print(f" rec_check skipped: {error}") + + with SqliteStore(path=str(db)) as store: + already_corrected = gtsam_stream in store.list_streams() + + if no_gtsam: + print(" --no-gtsam: skipping AprilTag/GTSAM/re-anchor") + elif already_corrected and not force: + print(f" already has '{gtsam_stream}' — skipping AprilTag/GTSAM/re-anchor (use --force)") + else: + correct_db( + db, + image_stream=image_stream, + apriltag_stream=apriltag_stream, + gtsam_stream=gtsam_stream, + marker_length=marker_length, + dictionary=dictionary, + ) + + if make_rrd: + try: + build_rrd( + str(db), + str(db.parent / f"{db.parent.name}.rrd"), + camera_stride=camera_freq, + map_voxel=map_voxel, + cloud_stride=cloud_stride, + mid360_pitch=mid360_pitch, + ) + except Exception as error: + print(f" rrd failed: {error}") + + +def main(): + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument( + "--recordings-dir", + default=str(Path.home() / "datasets" / "go2_recordings"), + help="dir containing recording subdirs with mem2.db", + ) + parser.add_argument("--db", default="", help="process a single mem2.db instead of scanning") + parser.add_argument("--image-stream", default="color_image") + parser.add_argument("--apriltag-stream", default="april_tags") + parser.add_argument("--gtsam-stream", default="gtsam_odom") + parser.add_argument("--marker-length", type=float, default=0.10) + parser.add_argument("--dictionary", default="DICT_APRILTAG_36h11") + parser.add_argument("--force", action="store_true", help="reprocess even if gtsam_odom exists") + parser.add_argument( + "--no-gtsam", + action="store_true", + help="skip AprilTag/GTSAM/re-anchor (e.g. rebuild only the .rrd)", + ) + parser.add_argument("--no-rrd", action="store_true", help="skip the .rrd visualization step") + parser.add_argument( + "--camera-freq", + type=int, + default=30, + help="keep 1 of every N color frames in the .rrd (usually the biggest part)", + ) + parser.add_argument( + "--map-voxel", + type=float, + default=0.1, + help="voxel size (m) for the .rrd clouds/maps; larger = smaller .rrd", + ) + parser.add_argument( + "--cloud-stride", + type=int, + default=3, + help="keep 1 of every N per-frame lidar clouds in the .rrd", + ) + parser.add_argument( + "--mid360-pitch", + action="store_true", + help="apply the legacy mid360->camera 44deg pitch correction (old fastlio " + "recordings; new go2_* data has correct transforms, leave off)", + ) + args = parser.parse_args() + + if args.db: + databases = [Path(args.db)] + else: + root = Path(args.recordings_dir) + databases = sorted(path for path in root.rglob("mem2.db") if "-wal" not in path.name) + if not databases: + raise SystemExit(f"no mem2.db found under {root}") + print(f"found {len(databases)} recording(s)") + for db in databases: + try: + process_db( + db, + image_stream=args.image_stream, + apriltag_stream=args.apriltag_stream, + gtsam_stream=args.gtsam_stream, + marker_length=args.marker_length, + dictionary=args.dictionary, + force=args.force, + no_gtsam=args.no_gtsam, + make_rrd=not args.no_rrd, + camera_freq=args.camera_freq, + map_voxel=args.map_voxel, + cloud_stride=args.cloud_stride, + mid360_pitch=args.mid360_pitch, + ) + except Exception as error: + print(f" !! failed: {error}") + print("done") + + +if __name__ == "__main__": + main() From d03839082416a4b5b95a881203f17ebcf44d15be Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Tue, 2 Jun 2026 19:33:53 -0700 Subject: [PATCH 009/185] - --- dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py b/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py index 5a2b46954b..63dcbcc195 100755 --- a/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py +++ b/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py @@ -31,8 +31,6 @@ uv run --no-sync python \ dimos/robot/unitree/go2/scripts/go2_post_process.py [--recordings-dir DIR] [--force] -Default recordings dir: ~/datasets/go2_recordings (each recording in its own -subdir with mem2.db). Already-processed dbs are skipped unless --force. """ from __future__ import annotations @@ -142,7 +140,7 @@ def main(): ) parser.add_argument( "--recordings-dir", - default=str(Path.home() / "datasets" / "go2_recordings"), + default="./go2_recordings", help="dir containing recording subdirs with mem2.db", ) parser.add_argument("--db", default="", help="process a single mem2.db instead of scanning") From b77ec1aa53352d187a88f1c7bbbbff74c6657915 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Tue, 2 Jun 2026 19:40:14 -0700 Subject: [PATCH 010/185] feat: positional target + most-recent-recording default for go2 post-process TARGET positional accepts a mem2.db, a dir containing one (process just that recording), or a dir to scan. With no TARGET, process the most recently created recording under --recordings-dir. Folds in the old --db flag. --- .../go2/scripts/go2_mid360_post_process.py | 60 +++++++++++++++---- 1 file changed, 50 insertions(+), 10 deletions(-) diff --git a/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py b/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py index 63dcbcc195..11e155d449 100755 --- a/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py +++ b/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py @@ -29,8 +29,11 @@ Run in a python env with dimos + cv2 + gtsam + scipy (from the dimos repo): uv run --no-sync python \ - dimos/robot/unitree/go2/scripts/go2_post_process.py [--recordings-dir DIR] [--force] + dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py [TARGET] [--force] +TARGET may be a `mem2.db`, a recording dir containing one, or a dir to scan for +recordings. With no TARGET it processes the most recently created recording +under --recordings-dir. """ from __future__ import annotations @@ -49,6 +52,43 @@ # is the same frame family gtsam was built from (so the re-anchor composes). The # legacy Go2 onboard `lidar`/`odom` is a different estimator frame -> left as-is. REANCHOR_PAIRS = [("go2_lidar", "go2_odom"), ("fastlio_lidar", "fastlio_odometry")] +DB_NAME = "mem2.db" + + +def _created_time(path: Path) -> float: + """File creation time (st_birthtime on macOS/BSD; falls back to mtime).""" + stat = path.stat() + return getattr(stat, "st_birthtime", stat.st_mtime) + + +def _scan(root: Path) -> list[Path]: + return sorted(path for path in root.rglob(DB_NAME) if "-wal" not in path.name) + + +def resolve_databases(target: str | None, recordings_dir: str) -> list[Path]: + """Pick which mem2.db(s) to process. + + TARGET wins when given: a `mem2.db` file, a dir holding one (process just + that recording), or any other dir (scan it recursively). With no TARGET, + process only the most recently created recording under recordings_dir. + """ + if target: + path = Path(target) + if path.name == DB_NAME: + return [path] + if (path / DB_NAME).exists(): + return [path / DB_NAME] + databases = _scan(path) + if not databases: + raise SystemExit(f"no {DB_NAME} found under {path}") + return databases + + databases = _scan(Path(recordings_dir)) + if not databases: + raise SystemExit(f"no {DB_NAME} found under {recordings_dir}") + most_recent = max(databases, key=_created_time) + print(f"no target given — using most recent recording: {most_recent.parent}") + return [most_recent] def correct_db(db: Path, *, image_stream, apriltag_stream, gtsam_stream, marker_length, dictionary): @@ -138,12 +178,18 @@ def main(): parser = argparse.ArgumentParser( description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter ) + parser.add_argument( + "target", + nargs="?", + default=None, + help="a mem2.db, a recording dir containing one, or a dir to scan. " + "Omit to use the most recently created recording under --recordings-dir.", + ) parser.add_argument( "--recordings-dir", default="./go2_recordings", - help="dir containing recording subdirs with mem2.db", + help="root searched when no target is given", ) - parser.add_argument("--db", default="", help="process a single mem2.db instead of scanning") parser.add_argument("--image-stream", default="color_image") parser.add_argument("--apriltag-stream", default="april_tags") parser.add_argument("--gtsam-stream", default="gtsam_odom") @@ -182,13 +228,7 @@ def main(): ) args = parser.parse_args() - if args.db: - databases = [Path(args.db)] - else: - root = Path(args.recordings_dir) - databases = sorted(path for path in root.rglob("mem2.db") if "-wal" not in path.name) - if not databases: - raise SystemExit(f"no mem2.db found under {root}") + databases = resolve_databases(args.target, args.recordings_dir) print(f"found {len(databases)} recording(s)") for db in databases: try: From 67c3338c5facce59c80759eed8d8d0d785e3b100 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Tue, 2 Jun 2026 20:19:38 -0700 Subject: [PATCH 011/185] clean and add backup stream --- .../sensors/lidar/fastlio2/recorder.py | 42 +------------- .../sensors/lidar/fastlio2/speed_warner.py | 24 +++++--- .../blueprints/smart/unitree_go2_record.py | 55 ++++++++++++++++--- 3 files changed, 64 insertions(+), 57 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/recorder.py b/dimos/hardware/sensors/lidar/fastlio2/recorder.py index 995082ce46..6a5d32add6 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/recorder.py +++ b/dimos/hardware/sensors/lidar/fastlio2/recorder.py @@ -21,19 +21,13 @@ import subprocess import textwrap import time -from typing import Any from pydantic import Field -from reactivex.disposable import Disposable from dimos.core.core import rpc from dimos.core.stream import In from dimos.memory2.module import Recorder, RecorderConfig -from dimos.memory2.stream import Stream -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.Imu import Imu from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.utils.logging_config import setup_logger @@ -112,13 +106,8 @@ class FastLio2Recorder(Recorder): config: FastLio2RecorderConfig - livox_lidar: In[PointCloud2] - livox_imu: In[Imu] - fastlio_lidar: In[PointCloud2] - fastlio_odometry: In[Odometry] - color_image: In[Image] lidar: In[PointCloud2] - odom: In[PoseStamped] + odometry: In[Odometry] # tcpdump fails fast (EPERM, bad iface) within a few ms; pause briefly so poll() catches that. _TCPDUMP_STARTUP_PROBE_SEC: float = 0.3 @@ -137,35 +126,6 @@ def stop(self) -> None: super().stop() self._stop_pcap() - def _port_to_stream(self, name: str, input_topic: In[Any], stream: Stream[Any]) -> None: - """Append each message from *input_topic* to *stream*, attaching world pose via tf. - - Stamped messages use their own ``.frame_id`` and ``.ts``; unstamped - messages (or ones whose frame isn't in the tf graph, e.g. a payload - already in world coords) fall back to ``config.default_frame_id`` — - so every observation gets a robot-pose anchor when tf is publishing. - - Registers the subscription as a disposable on this module. - """ - - default_frame_id = self.config.default_frame_id - tf_tolerance = self.config.tf_tolerance - - def on_msg(msg: Any) -> None: - # Force system time for all messages - ts = time.time() - frame_id = ( - getattr(msg, "child_frame_id", None) - or getattr(msg, "frame_id", None) - or default_frame_id - ) - transform = self.tf.get("world", frame_id, time_point=ts, time_tolerance=tf_tolerance) - pose = transform.to_pose() if transform is not None else None - - stream.append(msg, ts=ts, pose=pose) - - self.register_disposable(Disposable(input_topic.subscribe(on_msg))) - def _start_pcap(self) -> None: cfg = self.config path = Path(cfg.pcap_path).expanduser() diff --git a/dimos/hardware/sensors/lidar/fastlio2/speed_warner.py b/dimos/hardware/sensors/lidar/fastlio2/speed_warner.py index 3532e41614..a0e4b87764 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/speed_warner.py +++ b/dimos/hardware/sensors/lidar/fastlio2/speed_warner.py @@ -15,7 +15,7 @@ import math import time -from dimos.core.module import Module +from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.utils.logging_config import setup_logger @@ -23,12 +23,16 @@ logger = setup_logger() MPH_PER_MPS = 2.23694 -SPEED_LIMIT_MPH_WARNING = 30.0 -_SPEED_STATUS_PRINT_INTERVAL_SEC = 1.0 + + +class SpeedWarnerConfig(ModuleConfig): + # Speed the Go2 can never physically reach; exceeding it means the estimate diverged. + speed_limit_mph_warning: float = 30.0 + status_print_interval_sec: float = 1.0 class SpeedWarner(Module): - """Watches fastlio_odometry; once speed ever exceeds the limit (impossible for the Go2, + """Watches odometry; once speed ever exceeds the limit (impossible for the Go2, so it indicates the FastLio2 estimate has diverged / sensor is about to crash), latches and spams an error on every subsequent odom message until restart. @@ -36,7 +40,9 @@ class SpeedWarner(Module): are always 0. Speed is derived from pose deltas instead. """ - fastlio_odometry: In[Odometry] + config: SpeedWarnerConfig + + odometry: In[Odometry] _tripped: bool = False _max_mph: float = 0.0 @@ -44,7 +50,7 @@ class SpeedWarner(Module): _last_ts: float | None = None _last_print_ts: float = 0.0 - async def handle_fastlio_odometry(self, msg: Odometry) -> None: + async def handle_odometry(self, msg: Odometry) -> None: ts = msg.ts or time.time() pos = (msg.pose.x, msg.pose.y, msg.pose.z) last_pos, last_ts = self._last_pos, self._last_ts @@ -58,16 +64,16 @@ async def handle_fastlio_odometry(self, msg: Odometry) -> None: speed_mph = math.sqrt(dx * dx + dy * dy + dz * dz) / dt * MPH_PER_MPS if speed_mph > self._max_mph: self._max_mph = speed_mph - if ts - self._last_print_ts >= _SPEED_STATUS_PRINT_INTERVAL_SEC: + if ts - self._last_print_ts >= self.config.status_print_interval_sec: self._last_print_ts = ts print( f"\rspeed: {speed_mph:6.2f} mph max: {self._max_mph:6.2f} mph ", end="", flush=True, ) - if not self._tripped and speed_mph > SPEED_LIMIT_MPH_WARNING: + if not self._tripped and speed_mph > self.config.speed_limit_mph_warning: self._tripped = True logger.error( f"!!! FASTLIO ODOMETRY DIVERGED !!! reported {speed_mph:.1f} mph " - f"(limit {SPEED_LIMIT_MPH_WARNING:.1f} mph). Latching warnings." + f"(limit {self.config.speed_limit_mph_warning:.1f} mph). Latching warnings." ) diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py index 970d4f02b5..7370c1557f 100644 --- a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py +++ b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py @@ -15,6 +15,7 @@ import math import os +from pathlib import Path import time from typing import Any @@ -24,15 +25,20 @@ from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.core.global_config import global_config from dimos.core.stream import In +from dimos.hardware.sensors.lidar.fastlio2 import module as _fastlio2_module from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 from dimos.hardware.sensors.lidar.fastlio2.recorder import FastLio2Recorder, _default_recording_dir from dimos.hardware.sensors.lidar.fastlio2.speed_warner import SpeedWarner from dimos.hardware.sensors.lidar.livox.module import Mid360 from dimos.memory2.stream import Stream +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped 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.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.robot.unitree.go2.connection import GO2Connection from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop @@ -86,6 +92,21 @@ class Go2TfHackRecorder(FastLio2Recorder): - everything else (odom streams included) -> no pose """ + fastlio_lidar: In[PointCloud2] + fastlio_odometry: In[Odometry] + go2_lidar: In[PointCloud2] + go2_odom: In[PoseStamped] + color_image: In[Image] + livox_lidar: In[PointCloud2] + livox_imu: In[Imu] + # Shadow the parent's generic companion ports so they're not recorded as + # empty `lidar`/`odom` streams; the go2-prefixed ports above take their place. + lidar: None = None # type: ignore[assignment] + odom: None = None # type: ignore[assignment] + # sanity check + fastlio_lidar_no_cap: In[PointCloud2] + fastlio_odometry_no_cap: In[Odometry] + _latest_fastlio_odom: Odometry | None = None _warning_names: set[str] = set() @@ -129,6 +150,14 @@ def _world_to_base_from_fastlio(self) -> Transform | None: return world_to_mid360 + MID360_TO_BASE +class FastLio2NoCap(FastLio2): + pass + + +# Absolute path to FastLio2's cpp build dir; passed to FastLio2NoCap so the +# trivial subclass doesn't try to resolve `cpp` next to this file. +_FASTLIO2_CPP = str(Path(_fastlio2_module.__file__).resolve().parent / "cpp") + unitree_go2_record = autoconnect( KeyboardTeleop.blueprint(), MovementManager.blueprint(), @@ -150,29 +179,41 @@ def _world_to_base_from_fastlio(self) -> Transform | None: frame_id="world", map_freq=-1, lidar_ip=_LIDAR_IP, - max_velocity_norm_ms=3.1, + max_velocity_norm_ms=3.1, # meters/sec, 3.1 => 7mph, 5=>12mph. We want some padding ).remappings( [ (FastLio2, "lidar", "fastlio_lidar"), (FastLio2, "odometry", "fastlio_odometry"), ] ), - Go2TfHackRecorder.blueprint(lidar_ip=_LIDAR_IP, record_pcap=True).remappings( + FastLio2NoCap.blueprint( + frame_id="world", + map_freq=-1, + lidar_ip=_LIDAR_IP, + max_velocity_norm_ms=100, + cwd=_FASTLIO2_CPP, + ).remappings( + [ + (FastLio2, "lidar", "fastlio_lidar_no_cap"), + (FastLio2, "odometry", "fastlio_odometry_no_cap"), + ] + ), + Go2TfHackRecorder.blueprint(lidar_ip=_LIDAR_IP, record_pcap=True), + SpeedWarner.blueprint().remappings( [ - (Go2TfHackRecorder, "lidar", "go2_lidar"), - (Go2TfHackRecorder, "odom", "go2_odom"), + (SpeedWarner, "odometry", "fastlio_odometry_no_cap"), ] ), - SpeedWarner.blueprint(), ).global_config(n_workers=10, robot_model="unitree_go2") if __name__ == "__main__": - recording_dir = _default_recording_dir() + 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_record, - {FastLio2Recorder.name: {"recording_dir": recording_dir}}, + {Go2TfHackRecorder.name: {"recording_dir": recording_dir}}, ) coordinator.loop() From ad76b45f324ab3351616e2bf0e6b2ba8d7226bb1 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Tue, 2 Jun 2026 20:29:23 -0700 Subject: [PATCH 012/185] feat: replay dimos main.jsonl into the go2 post-process .rrd as logs build_rrd now looks for a main.jsonl next to the mem2.db (else one dir up), replays each JSON line as a rerun TextLog on the `ts` timeline (level + logger + extra fields preserved), and docks a TextLogView below the 3D/camera views when present. --- .../robot/unitree/go2/recording/build_rrd.py | 89 ++++++++++++++++--- 1 file changed, 77 insertions(+), 12 deletions(-) diff --git a/dimos/robot/unitree/go2/recording/build_rrd.py b/dimos/robot/unitree/go2/recording/build_rrd.py index 13e9cb10cf..51ac1028aa 100644 --- a/dimos/robot/unitree/go2/recording/build_rrd.py +++ b/dimos/robot/unitree/go2/recording/build_rrd.py @@ -26,7 +26,10 @@ from __future__ import annotations +from datetime import datetime +import json import math +from pathlib import Path import sqlite3 import numpy as np @@ -334,6 +337,59 @@ def _log_cam_frustums(store, camera_targets): print(f" rrd: {logged} recognition camera frustums (robot's-eye images, 3D only)") +# dimos jsonl level -> rerun TextLog level +_LOG_LEVELS = { + "debug": "DEBUG", + "info": "INFO", + "warning": "WARN", + "error": "ERROR", + "critical": "CRITICAL", +} +# keys rendered structurally; everything else is appended as key=value context +_LOG_STD_KEYS = {"event", "level", "logger", "timestamp", "func_name", "lineno"} + + +def _find_jsonl(db_path: str) -> Path | None: + """A dimos `main.jsonl` for this recording — next to the db, else one dir up.""" + recording_dir = Path(db_path).parent + for candidate in (recording_dir / "main.jsonl", recording_dir.parent / "main.jsonl"): + if candidate.exists(): + return candidate + return None + + +def _iso_to_epoch(value: str) -> float: + return datetime.fromisoformat(value.replace("Z", "+00:00")).timestamp() + + +def _log_jsonl(jsonl_path: Path) -> None: + """Replay a dimos `main.jsonl` as rerun TextLog entries on the `ts` timeline.""" + n = 0 + for line in jsonl_path.read_text().splitlines(): + line = line.strip() + if not line: + continue + try: + entry = json.loads(line) + except json.JSONDecodeError: + continue + timestamp = entry.get("timestamp") + if not timestamp: + continue + body = str(entry.get("event", "")) + extra = " ".join(f"{k}={entry[k]}" for k in entry if k not in _LOG_STD_KEYS) + if extra: + body = f"{body} ({extra})" + if entry.get("logger"): + body = f"[{entry['logger']}] {body}" + rr.set_time(TIMELINE, timestamp=_iso_to_epoch(timestamp)) + rr.log( + "logs", rr.TextLog(body, level=_LOG_LEVELS.get(str(entry.get("level")).lower(), "INFO")) + ) + n += 1 + print(f" rrd: logs <- {jsonl_path.name} ({n} entries)") + + def build_rrd( db_path: str, out_path: str, @@ -346,6 +402,7 @@ def build_rrd( rr.init("go2_post_process", recording_id=str(out_path)) rr.save(str(out_path)) cam_xform = MID360_TO_OPTICAL if mid360_pitch else BASE_TO_OPTICAL + jsonl_path = _find_jsonl(db_path) with SqliteStore(path=db_path) as store: streams = store.list_streams() @@ -370,20 +427,25 @@ def build_rrd( "onboard_map", ) } + views = rrb.Horizontal( + rrb.Spatial3DView( + origin="/world", + name="3D", + overrides=hide, + eye_controls=rrb.EyeControls3D(kind=rrb.Eye3DKind.Orbital, tracking_entity=track), + ), + rrb.Spatial2DView(origin="/world/camera", name="camera"), + column_shares=[3, 1], + ) + # When a dimos main.jsonl is present, dock its log replay below the views. + layout = ( + rrb.Vertical(views, rrb.TextLogView(origin="/logs", name="logs"), row_shares=[4, 1]) + if jsonl_path is not None + else views + ) rr.send_blueprint( rrb.Blueprint( - rrb.Horizontal( - rrb.Spatial3DView( - origin="/world", - name="3D", - overrides=hide, - eye_controls=rrb.EyeControls3D( - kind=rrb.Eye3DKind.Orbital, tracking_entity=track - ), - ), - rrb.Spatial2DView(origin="/world/camera", name="camera"), - column_shares=[3, 1], - ), + layout, rrb.BlueprintPanel(state=rrb.PanelState.Expanded), rrb.TimePanel(state=rrb.PanelState.Expanded), rrb.SelectionPanel(state=rrb.PanelState.Collapsed), @@ -426,4 +488,7 @@ def build_rrd( except Exception: break print(f" rrd: world/camera/image <- color_image ({n} frames, stride {camera_stride})") + + if jsonl_path is not None: + _log_jsonl(jsonl_path) print(f" wrote {out_path}") From 04844a45966c9b2fbbc144f29693ce6bcb6f10d2 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Tue, 2 Jun 2026 20:32:25 -0700 Subject: [PATCH 013/185] feat: --check (sanity-check only) mode that writes summary.json --check runs only the rec_check report on each recording and writes a structured summary.json (files, pcap stats, per-stream rows/hz/pose%, fastlio odometry travel) into the recording dir, skipping GTSAM/re-anchor/.rrd. --- .../robot/unitree/go2/recording/rec_check.py | 63 ++++++++++++++++++- .../go2/scripts/go2_mid360_post_process.py | 14 +++++ 2 files changed, 76 insertions(+), 1 deletion(-) diff --git a/dimos/robot/unitree/go2/recording/rec_check.py b/dimos/robot/unitree/go2/recording/rec_check.py index 5780b1b213..638242be17 100644 --- a/dimos/robot/unitree/go2/recording/rec_check.py +++ b/dimos/robot/unitree/go2/recording/rec_check.py @@ -18,11 +18,13 @@ from __future__ import annotations from datetime import datetime +import json import math from pathlib import Path import sqlite3 import subprocess import sys +from typing import Any STREAMS = ( "livox_imu", @@ -34,6 +36,8 @@ "color_image", ) RECORDINGS_DIR = Path("go2_recordings") +# A pcap with only its global header (no packets) is exactly this many bytes. +PCAP_HEADER_BYTES = 24 def find_dir(argv: list[str]) -> Path: @@ -148,6 +152,63 @@ def format_clock(seconds: float | None) -> str: return datetime.fromtimestamp(seconds).strftime("%H:%M:%S") +def summarize(directory: Path) -> dict[str, Any]: + """The same stats report() prints, as a JSON-able dict.""" + pcap = directory / "raw_mid360.pcap" + db = directory / "mem2.db" + summary: dict[str, Any] = { + "directory": str(directory), + "files": {}, + "pcap": None, + "streams": {}, + "fastlio_odometry_travel": None, + } + for path in (pcap, db, directory / "mem2.db-wal", directory / "mem2.db-shm"): + summary["files"][path.name] = path.stat().st_size if path.exists() else None + + if pcap.exists() and pcap.stat().st_size > PCAP_HEADER_BYTES: + stats = pcap_stats(pcap) + if stats is not None: + packets, first, last = stats + span = last - first + summary["pcap"] = { + "packets": packets, + "first": first, + "last": last, + "span_s": span, + "rate_pkt_s": packets / span if span > 0 else 0, + } + + if not db.exists(): + summary["error"] = "mem2.db missing" + return summary + + connection = sqlite3.connect(db) + cur = connection.cursor() + for name in STREAMS: + n, t0, t1, pose_n = stream_rows(cur, name) + if n == 0: + summary["streams"][name] = {"rows": 0} + continue + span = (t1 - t0) if (t0 and t1) else 0 + summary["streams"][name] = { + "rows": n, + "span_s": span, + "hz": (n - 1) / span if span > 0 else 0, + "pose_pct": 100 * pose_n / n if n else 0, + } + summary["fastlio_odometry_travel"] = odometry_travel(cur) + connection.close() + return summary + + +def write_summary(directory: Path) -> Path: + """Write summarize() to /summary.json and return its path.""" + path = directory / "summary.json" + path.write_text(json.dumps(summarize(directory), indent=2)) + return path + + def main() -> int: return report(find_dir(sys.argv)) @@ -166,7 +227,7 @@ def report(directory: Path) -> int: print(f" {path.name:<20} (missing)") print() - if pcap.exists() and pcap.stat().st_size > 24: + if pcap.exists() and pcap.stat().st_size > PCAP_HEADER_BYTES: stats = pcap_stats(pcap) if stats is None: print("pcap: present (capinfos/tcpdump unavailable to inspect)") diff --git a/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py b/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py index 11e155d449..a1c7bade43 100755 --- a/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py +++ b/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py @@ -136,6 +136,7 @@ def process_db( map_voxel=0.1, cloud_stride=3, mid360_pitch=False, + check_only=False, ): print(f">> {db}") try: @@ -143,6 +144,13 @@ def process_db( except Exception as error: print(f" rec_check skipped: {error}") + if check_only: + try: + print(f" wrote {rec_check.write_summary(db.parent)}") + except Exception as error: + print(f" summary failed: {error}") + return + with SqliteStore(path=str(db)) as store: already_corrected = gtsam_stream in store.list_streams() @@ -196,6 +204,11 @@ def main(): parser.add_argument("--marker-length", type=float, default=0.10) parser.add_argument("--dictionary", default="DICT_APRILTAG_36h11") parser.add_argument("--force", action="store_true", help="reprocess even if gtsam_odom exists") + parser.add_argument( + "--check", + action="store_true", + help="only sanity-check each recording and write summary.json (no GTSAM/re-anchor/.rrd)", + ) parser.add_argument( "--no-gtsam", action="store_true", @@ -246,6 +259,7 @@ def main(): map_voxel=args.map_voxel, cloud_stride=args.cloud_stride, mid360_pitch=args.mid360_pitch, + check_only=args.check, ) except Exception as error: print(f" !! failed: {error}") From 581a1eeeb0fcc748b5ed86eb6514ec953031d66b Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Tue, 2 Jun 2026 20:32:58 -0700 Subject: [PATCH 014/185] fix: only look for main.jsonl next to the mem2.db Drop the one-dir-up fallback in build_rrd's jsonl discovery. --- dimos/robot/unitree/go2/recording/build_rrd.py | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/dimos/robot/unitree/go2/recording/build_rrd.py b/dimos/robot/unitree/go2/recording/build_rrd.py index 51ac1028aa..4cccd5f9df 100644 --- a/dimos/robot/unitree/go2/recording/build_rrd.py +++ b/dimos/robot/unitree/go2/recording/build_rrd.py @@ -350,12 +350,9 @@ def _log_cam_frustums(store, camera_targets): def _find_jsonl(db_path: str) -> Path | None: - """A dimos `main.jsonl` for this recording — next to the db, else one dir up.""" - recording_dir = Path(db_path).parent - for candidate in (recording_dir / "main.jsonl", recording_dir.parent / "main.jsonl"): - if candidate.exists(): - return candidate - return None + """A dimos `main.jsonl` for this recording — next to the db.""" + candidate = Path(db_path).parent / "main.jsonl" + return candidate if candidate.exists() else None def _iso_to_epoch(value: str) -> float: From cf9bbb2ba4f874f2ca5d6c663a1ae0a438fe2e16 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Tue, 2 Jun 2026 20:34:06 -0700 Subject: [PATCH 015/185] - --- dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py index 7370c1557f..6574f75a08 100644 --- a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py +++ b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py @@ -116,7 +116,7 @@ def on_msg(msg: Any) -> None: pose = None if name == "fastlio_odometry": self._latest_fastlio_odom = msg - elif name == "fastlio_lidar": + elif name == "fastlio_lidar" or name == "fastlio_lidar_no_cap": world_to_base = self._world_to_base_from_fastlio() if world_to_base is not None: pose = world_to_base.to_pose() From ec336c9f790a9a7637f7d589fc1de64fb14bc928 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Tue, 2 Jun 2026 21:12:16 -0700 Subject: [PATCH 016/185] clean up --- .../blueprints/smart/unitree_go2_record.py | 37 +++++++++---------- 1 file changed, 18 insertions(+), 19 deletions(-) diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py index 6574f75a08..ab384c7e85 100644 --- a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py +++ b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py @@ -15,7 +15,6 @@ import math import os -from pathlib import Path import time from typing import Any @@ -25,7 +24,6 @@ from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.core.global_config import global_config from dimos.core.stream import In -from dimos.hardware.sensors.lidar.fastlio2 import module as _fastlio2_module from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 from dimos.hardware.sensors.lidar.fastlio2.recorder import FastLio2Recorder, _default_recording_dir from dimos.hardware.sensors.lidar.fastlio2.speed_warner import SpeedWarner @@ -114,8 +112,11 @@ def _port_to_stream(self, name: str, input_topic: In[Any], stream: Stream[Any]) def on_msg(msg: Any) -> None: ts = time.time() pose = None - if name == "fastlio_odometry": + if name == "fastlio_odometry" or name == "fastlio_odometry_no_cap": self._latest_fastlio_odom = msg + world_to_base = self._world_to_base_from_fastlio() + if world_to_base is not None: + pose = world_to_base.to_pose() elif name == "fastlio_lidar" or name == "fastlio_lidar_no_cap": world_to_base = self._world_to_base_from_fastlio() if world_to_base is not None: @@ -154,10 +155,6 @@ class FastLio2NoCap(FastLio2): pass -# Absolute path to FastLio2's cpp build dir; passed to FastLio2NoCap so the -# trivial subclass doesn't try to resolve `cpp` next to this file. -_FASTLIO2_CPP = str(Path(_fastlio2_module.__file__).resolve().parent / "cpp") - unitree_go2_record = autoconnect( KeyboardTeleop.blueprint(), MovementManager.blueprint(), @@ -186,18 +183,20 @@ class FastLio2NoCap(FastLio2): (FastLio2, "odometry", "fastlio_odometry"), ] ), - FastLio2NoCap.blueprint( - frame_id="world", - map_freq=-1, - lidar_ip=_LIDAR_IP, - max_velocity_norm_ms=100, - cwd=_FASTLIO2_CPP, - ).remappings( - [ - (FastLio2, "lidar", "fastlio_lidar_no_cap"), - (FastLio2, "odometry", "fastlio_odometry_no_cap"), - ] - ), + # FastLio2NoCap.blueprint( + # frame_id="world", + # map_freq=-1, + # lidar_ip=_LIDAR_IP, + # max_velocity_norm_ms=100, + # # Absolute path to FastLio2's cpp build dir; passed to FastLio2NoCap so the + # # trivial subclass doesn't try to resolve `cpp` next to this file. + # cwd=str(Path(_fastlio2_module.__file__).resolve().parent / "cpp"), + # ).remappings( + # [ + # (FastLio2, "lidar", "fastlio_lidar_no_cap"), + # (FastLio2, "odometry", "fastlio_odometry_no_cap"), + # ] + # ), Go2TfHackRecorder.blueprint(lidar_ip=_LIDAR_IP, record_pcap=True), SpeedWarner.blueprint().remappings( [ From 7ef91f69dae68f973b7de21cd34c4adbec5ff75c Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 3 Jun 2026 15:51:40 -0700 Subject: [PATCH 017/185] pointlio native module: mirror fastlio2 module (PointLio class, pointlio_native flake/cmake consuming dimos-module-fastlio2 pointlio branch + Estimator/parameters sources) --- .../sensors/lidar/pointlio/config/avia.yaml | 35 + .../lidar/pointlio/config/default.yaml | 33 + .../lidar/pointlio/config/horizon.yaml | 35 + .../sensors/lidar/pointlio/config/marsim.yaml | 35 + .../sensors/lidar/pointlio/config/mid360.yaml | 35 + .../lidar/pointlio/config/ouster64.yaml | 36 + .../lidar/pointlio/config/velodyne.yaml | 37 + .../sensors/lidar/pointlio/cpp/CMakeLists.txt | 113 +++ .../sensors/lidar/pointlio/cpp/README.md | 109 +++ .../lidar/pointlio/cpp/cloud_filter.hpp | 51 ++ .../lidar/pointlio/cpp/config/mid360.json | 38 + .../sensors/lidar/pointlio/cpp/flake.lock | 162 +++++ .../sensors/lidar/pointlio/cpp/flake.nix | 100 +++ .../sensors/lidar/pointlio/cpp/main.cpp | 664 ++++++++++++++++++ .../sensors/lidar/pointlio/cpp/timing.hpp | 114 +++ .../sensors/lidar/pointlio/cpp/voxel_map.hpp | 297 ++++++++ .../lidar/pointlio/fastlio_blueprints.py | 67 ++ .../sensors/lidar/pointlio/fastlio_test.py | 1 + .../hardware/sensors/lidar/pointlio/module.py | 263 +++++++ .../sensors/lidar/pointlio/setup_network.py | 1 + 20 files changed, 2226 insertions(+) create mode 100644 dimos/hardware/sensors/lidar/pointlio/config/avia.yaml create mode 100644 dimos/hardware/sensors/lidar/pointlio/config/default.yaml create mode 100644 dimos/hardware/sensors/lidar/pointlio/config/horizon.yaml create mode 100644 dimos/hardware/sensors/lidar/pointlio/config/marsim.yaml create mode 100644 dimos/hardware/sensors/lidar/pointlio/config/mid360.yaml create mode 100644 dimos/hardware/sensors/lidar/pointlio/config/ouster64.yaml create mode 100644 dimos/hardware/sensors/lidar/pointlio/config/velodyne.yaml create mode 100644 dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt create mode 100644 dimos/hardware/sensors/lidar/pointlio/cpp/README.md create mode 100644 dimos/hardware/sensors/lidar/pointlio/cpp/cloud_filter.hpp create mode 100644 dimos/hardware/sensors/lidar/pointlio/cpp/config/mid360.json create mode 100644 dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock create mode 100644 dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix create mode 100644 dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp create mode 100644 dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp create mode 100644 dimos/hardware/sensors/lidar/pointlio/cpp/voxel_map.hpp create mode 100644 dimos/hardware/sensors/lidar/pointlio/fastlio_blueprints.py create mode 120000 dimos/hardware/sensors/lidar/pointlio/fastlio_test.py create mode 100644 dimos/hardware/sensors/lidar/pointlio/module.py create mode 120000 dimos/hardware/sensors/lidar/pointlio/setup_network.py diff --git a/dimos/hardware/sensors/lidar/pointlio/config/avia.yaml b/dimos/hardware/sensors/lidar/pointlio/config/avia.yaml new file mode 100644 index 0000000000..8447b64658 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/config/avia.yaml @@ -0,0 +1,35 @@ +common: + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + +preprocess: + lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 6 + blind: 4 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 90 + det_range: 450.0 + extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic + extrinsic_T: [ 0.04165, 0.02326, -0.0284 ] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] + +publish: + path_en: false + scan_publish_en: true # false: close all the point cloud output + dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: true + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/pointlio/config/default.yaml b/dimos/hardware/sensors/lidar/pointlio/config/default.yaml new file mode 100644 index 0000000000..688597d850 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/config/default.yaml @@ -0,0 +1,33 @@ +common: + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + time_sync_en: false + time_offset_lidar_to_imu: 0.0 + +preprocess: + lidar_type: 1 # 1 for Livox serials LiDAR + scan_line: 4 + blind: 0.5 # spherical min range (metres) + +mapping: + acc_cov: 0.01 # tighter than mid360 default (0.1) + gyr_cov: 0.01 # tighter than mid360 default (0.1) + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 360 + det_range: 60.0 # reduced from 100 — less noise from distant points + extrinsic_est_en: true # enable live calibration + extrinsic_T: [ -0.011, -0.02329, 0.04412 ] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] + +publish: + path_en: false + scan_publish_en: true + dense_publish_en: true + scan_bodyframe_pub_en: true + +pcd_save: + pcd_save_en: false + interval: -1 diff --git a/dimos/hardware/sensors/lidar/pointlio/config/horizon.yaml b/dimos/hardware/sensors/lidar/pointlio/config/horizon.yaml new file mode 100644 index 0000000000..43db0c3bff --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/config/horizon.yaml @@ -0,0 +1,35 @@ +common: + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + +preprocess: + lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 6 + blind: 4 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 100 + det_range: 260.0 + extrinsic_est_en: true # true: enable the online estimation of IMU-LiDAR extrinsic + extrinsic_T: [ 0.05512, 0.02226, -0.0297 ] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] + +publish: + path_en: false + scan_publish_en: true # false: close all the point cloud output + dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: true + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/pointlio/config/marsim.yaml b/dimos/hardware/sensors/lidar/pointlio/config/marsim.yaml new file mode 100644 index 0000000000..ad6c89121a --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/config/marsim.yaml @@ -0,0 +1,35 @@ +common: + lid_topic: "/quad0_pcl_render_node/sensor_cloud" + imu_topic: "/quad_0/imu" + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + +preprocess: + lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 4 + blind: 0.5 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 90 + det_range: 50.0 + extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic + extrinsic_T: [ -0.0, -0.0, 0.0 ] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] + +publish: + path_en: false + scan_publish_en: true # false: close all the point cloud output + dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: true + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/pointlio/config/mid360.yaml b/dimos/hardware/sensors/lidar/pointlio/config/mid360.yaml new file mode 100644 index 0000000000..512047ee48 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/config/mid360.yaml @@ -0,0 +1,35 @@ +common: + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + +preprocess: + lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 4 + blind: 0.5 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 360 + det_range: 100.0 + extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic + extrinsic_T: [ -0.011, -0.02329, 0.04412 ] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] + +publish: + path_en: false + scan_publish_en: true # false: close all the point cloud output + dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: true + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/pointlio/config/ouster64.yaml b/dimos/hardware/sensors/lidar/pointlio/config/ouster64.yaml new file mode 100644 index 0000000000..9d891bbeba --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/config/ouster64.yaml @@ -0,0 +1,36 @@ +common: + lid_topic: "/os_cloud_node/points" + imu_topic: "/os_cloud_node/imu" + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + +preprocess: + lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 64 + timestamp_unit: 3 # 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. + blind: 4 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 180 + det_range: 150.0 + extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic + extrinsic_T: [ 0.0, 0.0, 0.0 ] + extrinsic_R: [1, 0, 0, + 0, 1, 0, + 0, 0, 1] + +publish: + path_en: false + scan_publish_en: true # false: close all the point cloud output + dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: true + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/pointlio/config/velodyne.yaml b/dimos/hardware/sensors/lidar/pointlio/config/velodyne.yaml new file mode 100644 index 0000000000..450eda48b8 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/config/velodyne.yaml @@ -0,0 +1,37 @@ +common: + lid_topic: "/velodyne_points" + imu_topic: "/imu/data" + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + +preprocess: + lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 32 + scan_rate: 10 # only need to be set for velodyne, unit: Hz, + timestamp_unit: 2 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. + blind: 2 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 180 + det_range: 100.0 + extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, + extrinsic_T: [ 0, 0, 0.28] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] + +publish: + path_en: false + scan_publish_en: true # false: close all the point cloud output + dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: true + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt new file mode 100644 index 0000000000..5b643f74c6 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt @@ -0,0 +1,113 @@ +cmake_minimum_required(VERSION 3.14) +project(pointlio_native CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") + +if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT) + set(CMAKE_INSTALL_PREFIX "${CMAKE_SOURCE_DIR}/result" CACHE PATH "" FORCE) +endif() + +# OpenMP for parallel processing +find_package(OpenMP QUIET) +if(OpenMP_CXX_FOUND) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +endif() + +# MP defines (same logic as FAST-LIO) +message("CPU architecture: ${CMAKE_SYSTEM_PROCESSOR}") +if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)") + include(ProcessorCount) + ProcessorCount(N) + if(N GREATER 4) + add_definitions(-DMP_EN -DMP_PROC_NUM=3) + elseif(N GREATER 3) + add_definitions(-DMP_EN -DMP_PROC_NUM=2) + else() + add_definitions(-DMP_PROC_NUM=1) + endif() +else() + add_definitions(-DMP_PROC_NUM=1) +endif() + +# Fetch dependencies +include(FetchContent) + +# FAST-LIO-NON-ROS (pass -DFASTLIO_DIR= or use local copy) +if(NOT FASTLIO_DIR) + set(FASTLIO_DIR ${CMAKE_CURRENT_SOURCE_DIR}/fast-lio-non-ros) +endif() + +# dimos-lcm C++ message headers +FetchContent_Declare(dimos_lcm + GIT_REPOSITORY https://github.com/dimensionalOS/dimos-lcm.git + GIT_TAG main + GIT_SHALLOW TRUE +) +FetchContent_MakeAvailable(dimos_lcm) + +# LCM +find_package(PkgConfig REQUIRED) +pkg_check_modules(LCM REQUIRED lcm) + +# Eigen3 +find_package(Eigen3 REQUIRED) + +# PCL (only components we need — avoid full PCL which drags in VTK via io) +find_package(PCL 1.8 REQUIRED COMPONENTS common filters) + +# yaml-cpp (FAST-LIO config parsing — standard YAML format) +find_package(yaml-cpp REQUIRED) + +# Livox SDK2 (from nix or /usr/local fallback) +find_library(LIVOX_SDK livox_lidar_sdk_shared) +if(NOT LIVOX_SDK) + message(FATAL_ERROR "Livox SDK2 not found. Available via nix flake in lidar/livox/") +endif() +get_filename_component(LIVOX_SDK_LIB_DIR ${LIVOX_SDK} DIRECTORY) +get_filename_component(LIVOX_SDK_PREFIX ${LIVOX_SDK_LIB_DIR} DIRECTORY) +set(LIVOX_SDK_INCLUDE_DIR ${LIVOX_SDK_PREFIX}/include) + +add_executable(pointlio_native + main.cpp + ${FASTLIO_DIR}/src/preprocess.cpp + ${FASTLIO_DIR}/src/Estimator.cpp + ${FASTLIO_DIR}/src/parameters.cpp + ${FASTLIO_DIR}/include/ikd-Tree/ikd_Tree.cpp +) + +# Shared Livox common headers (livox_sdk_config.hpp etc.) +if(NOT LIVOX_COMMON_DIR) + set(LIVOX_COMMON_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../common) +endif() + +target_include_directories(pointlio_native PRIVATE + ${FASTLIO_DIR}/include + ${FASTLIO_DIR}/include/IKFoM/IKFoM_toolkit + ${FASTLIO_DIR}/src + ${dimos_lcm_SOURCE_DIR}/generated/cpp_lcm_msgs + ${LCM_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR} + ${LIVOX_COMMON_DIR} + ${LIVOX_SDK_INCLUDE_DIR} +) + +target_link_libraries(pointlio_native PRIVATE + ${LCM_LIBRARIES} + ${LIVOX_SDK} + ${PCL_LIBRARIES} + yaml-cpp::yaml-cpp +) + +if(OpenMP_CXX_FOUND) + target_link_libraries(pointlio_native PRIVATE OpenMP::OpenMP_CXX) +endif() + +target_link_directories(pointlio_native PRIVATE + ${LCM_LIBRARY_DIRS} +) + +install(TARGETS pointlio_native DESTINATION bin) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/README.md b/dimos/hardware/sensors/lidar/pointlio/cpp/README.md new file mode 100644 index 0000000000..bbbfdf1929 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/README.md @@ -0,0 +1,109 @@ +# FAST-LIO2 Native Module (C++) + +Real-time LiDAR SLAM using FAST-LIO2 with integrated Livox Mid-360 driver. +Binds Livox SDK2 directly into FAST-LIO-NON-ROS: SDK callbacks feed +CustomMsg/Imu to FastLio, which performs EKF-LOAM SLAM. Registered +(world-frame) point clouds and odometry are published on LCM. + +## Build + +### Nix (recommended) + +```bash +cd dimos/hardware/sensors/lidar/fastlio2/cpp +nix build .#fastlio2_native +``` + +Binary lands at `result/bin/fastlio2_native`. + +The flake pulls Livox SDK2 from the livox sub-flake and +[FAST-LIO-NON-ROS](https://github.com/leshy/FAST-LIO-NON-ROS) from GitHub +automatically. + +### Native (CMake) + +Requires: +- CMake >= 3.14 +- [LCM](https://lcm-proj.github.io/) (`pacman -S lcm` or build from source) +- [Livox SDK2](https://github.com/Livox-SDK/Livox-SDK2) installed to `/usr/local` +- Eigen3, PCL (common, filters), yaml-cpp, Boost, OpenMP +- [FAST-LIO-NON-ROS](https://github.com/leshy/FAST-LIO-NON-ROS) checked out locally + +```bash +cd dimos/hardware/sensors/lidar/fastlio2/cpp +cmake -B build -DFASTLIO_DIR=$HOME/coding/FAST-LIO-NON-ROS +cmake --build build -j$(nproc) +cmake --install build +``` + +Binary lands at `result/bin/fastlio2_native` (same location as nix). + +If `-DFASTLIO_DIR` is omitted, CMake auto-fetches FAST-LIO-NON-ROS from GitHub. + +## Network setup + +The Mid-360 communicates over USB ethernet. Configure the interface: + +```bash +sudo nmcli con add type ethernet ifname usbeth0 con-name livox-mid360 \ + ipv4.addresses 192.168.1.5/24 ipv4.method manual +sudo nmcli con up livox-mid360 +``` + +This persists across reboots. The lidar defaults to `192.168.1.155`. + +## Usage + +Normally launched by `FastLio2` via the NativeModule framework: + +```python +from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 +from dimos.core.coordination.blueprints import autoconnect + +autoconnect( + FastLio2.blueprint(host_ip="192.168.1.5"), + SomeConsumer.blueprint(), +).build().loop() +``` + +### Manual invocation (for debugging) + +```bash +./result/bin/fastlio2_native \ + --lidar '/pointcloud#sensor_msgs.PointCloud2' \ + --odometry '/odometry#nav_msgs.Odometry' \ + --host_ip 192.168.1.5 \ + --lidar_ip 192.168.1.155 \ + --config_path ../config/mid360.yaml +``` + +Topic strings must include the `#type` suffix -- this is the actual LCM channel +name used by dimos subscribers. + +For full vis: +```sh +rerun-bridge +``` + +For LCM traffic: +```sh +lcm-spy +``` + +## Configuration + +FAST-LIO2 config files live in `config/`. The YAML config controls filter +parameters, EKF tuning, and point cloud processing settings. + +## File overview + +| File | Description | +|---------------------------|--------------------------------------------------------------| +| `main.cpp` | Livox SDK2 + FAST-LIO2 integration, EKF SLAM, LCM publishing | +| `cloud_filter.hpp` | Point cloud filtering (range, voxel downsampling) | +| `voxel_map.hpp` | Global voxel map accumulation | +| `dimos_native_module.hpp` | Reusable header for parsing NativeModule CLI args | +| `config/` | FAST-LIO2 YAML configuration files | +| `flake.nix` | Nix flake for hermetic builds | +| `CMakeLists.txt` | Build config, fetches dimos-lcm headers automatically | +| `../module.py` | Python NativeModule wrapper (`FastLio2`) | diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/cloud_filter.hpp b/dimos/hardware/sensors/lidar/pointlio/cpp/cloud_filter.hpp new file mode 100644 index 0000000000..352ba9bef5 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/cloud_filter.hpp @@ -0,0 +1,51 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Point cloud filtering utilities: voxel grid downsampling and +// statistical outlier removal using PCL. + +#ifndef CLOUD_FILTER_HPP_ +#define CLOUD_FILTER_HPP_ + +#include +#include +#include +#include + +struct CloudFilterConfig { + float voxel_size = 0.1f; + int sor_mean_k = 50; + float sor_stddev = 1.0f; +}; + +/// Apply voxel grid downsample + statistical outlier removal in-place. +/// Returns the filtered cloud (new allocation). +template +typename pcl::PointCloud::Ptr filter_cloud( + const typename pcl::PointCloud::Ptr& input, + const CloudFilterConfig& cfg) { + + if (!input || input->empty()) return input; + + // Voxel grid downsample + typename pcl::PointCloud::Ptr voxelized(new pcl::PointCloud()); + pcl::VoxelGrid vg; + vg.setInputCloud(input); + vg.setLeafSize(cfg.voxel_size, cfg.voxel_size, cfg.voxel_size); + vg.filter(*voxelized); + + // Statistical outlier removal + if (cfg.sor_mean_k > 0 && voxelized->size() > static_cast(cfg.sor_mean_k)) { + typename pcl::PointCloud::Ptr cleaned(new pcl::PointCloud()); + pcl::StatisticalOutlierRemoval sor; + sor.setInputCloud(voxelized); + sor.setMeanK(cfg.sor_mean_k); + sor.setStddevMulThresh(cfg.sor_stddev); + sor.filter(*cleaned); + return cleaned; + } + + return voxelized; +} + +#endif diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/config/mid360.json b/dimos/hardware/sensors/lidar/pointlio/cpp/config/mid360.json new file mode 100644 index 0000000000..ff6cc6dbf6 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/config/mid360.json @@ -0,0 +1,38 @@ +{ + "common": { + "time_sync_en": false, + "time_offset_lidar_to_imu": 0.0, + "msr_freq": 50.0, + "main_freq": 5000.0 + }, + "preprocess": { + "lidar_type": 1, + "scan_line": 1, + "blind": 1 + }, + "mapping": { + "acc_cov": 0.1, + "gyr_cov": 0.1, + "b_acc_cov": 0.0001, + "b_gyr_cov": 0.0001, + "fov_degree": 360, + "det_range": 100.0, + "extrinsic_est_en": true, + "extrinsic_T": [ + 0.04165, + 0.02326, + -0.0284 + ], + "extrinsic_R": [ + 1.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 1.0 + ] + } +} diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock new file mode 100644 index 0000000000..ed10ba8629 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock @@ -0,0 +1,162 @@ +{ + "nodes": { + "dimos-lcm": { + "flake": false, + "locked": { + "lastModified": 1769774949, + "narHash": "sha256-icRK7jerqNlwK1WZBrnIP04I2WozzFqTD7qsmnPxQuo=", + "owner": "dimensionalOS", + "repo": "dimos-lcm", + "rev": "0aa72b7b1bd3a65f50f5c03485ee9b728df56afe", + "type": "github" + }, + "original": { + "owner": "dimensionalOS", + "ref": "main", + "repo": "dimos-lcm", + "type": "github" + } + }, + "dimos-lcm_2": { + "flake": false, + "locked": { + "lastModified": 1769774949, + "narHash": "sha256-icRK7jerqNlwK1WZBrnIP04I2WozzFqTD7qsmnPxQuo=", + "owner": "dimensionalOS", + "repo": "dimos-lcm", + "rev": "0aa72b7b1bd3a65f50f5c03485ee9b728df56afe", + "type": "github" + }, + "original": { + "owner": "dimensionalOS", + "ref": "main", + "repo": "dimos-lcm", + "type": "github" + } + }, + "fast-lio": { + "flake": false, + "locked": { + "lastModified": 1778784133, + "narHash": "sha256-bZEmIJlrzcqrHLxCiXXXf7fgi2yjVRuad8z08HN1eMU=", + "owner": "dimensionalOS", + "repo": "dimos-module-fastlio2", + "rev": "d93bc6795babe9e63cc77d2bf2b72294d9afa839", + "type": "github" + }, + "original": { + "owner": "dimensionalOS", + "ref": "v0.3.0-quiet-logs", + "repo": "dimos-module-fastlio2", + "type": "github" + } + }, + "flake-utils": { + "inputs": { + "systems": "systems" + }, + "locked": { + "lastModified": 1731533236, + "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=", + "owner": "numtide", + "repo": "flake-utils", + "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b", + "type": "github" + }, + "original": { + "owner": "numtide", + "repo": "flake-utils", + "type": "github" + } + }, + "lcm-extended": { + "inputs": { + "flake-utils": [ + "flake-utils" + ], + "nixpkgs": [ + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1774902379, + "narHash": "sha256-gRFvEkbXCEoG4jEmsT+i0bMZ5kDHOtAaPsrbStXjdu4=", + "owner": "jeff-hykin", + "repo": "lcm_extended", + "rev": "7d12ad8546d3daae30528a6c28f2c9ff5b10baf7", + "type": "github" + }, + "original": { + "owner": "jeff-hykin", + "repo": "lcm_extended", + "type": "github" + } + }, + "livox-sdk": { + "inputs": { + "dimos-lcm": "dimos-lcm_2", + "flake-utils": [ + "flake-utils" + ], + "lcm-extended": [ + "lcm-extended" + ], + "nixpkgs": [ + "nixpkgs" + ] + }, + "locked": { + "path": "../../livox/cpp", + "type": "path" + }, + "original": { + "path": "../../livox/cpp", + "type": "path" + }, + "parent": [] + }, + "nixpkgs": { + "locked": { + "lastModified": 1770841267, + "narHash": "sha256-9xejG0KoqsoKEGp2kVbXRlEYtFFcDTHjidiuX8hGO44=", + "owner": "NixOS", + "repo": "nixpkgs", + "rev": "ec7c70d12ce2fc37cb92aff673dcdca89d187bae", + "type": "github" + }, + "original": { + "owner": "NixOS", + "ref": "nixos-unstable", + "repo": "nixpkgs", + "type": "github" + } + }, + "root": { + "inputs": { + "dimos-lcm": "dimos-lcm", + "fast-lio": "fast-lio", + "flake-utils": "flake-utils", + "lcm-extended": "lcm-extended", + "livox-sdk": "livox-sdk", + "nixpkgs": "nixpkgs" + } + }, + "systems": { + "locked": { + "lastModified": 1681028828, + "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", + "owner": "nix-systems", + "repo": "default", + "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", + "type": "github" + }, + "original": { + "owner": "nix-systems", + "repo": "default", + "type": "github" + } + } + }, + "root": "root", + "version": 7 +} diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix new file mode 100644 index 0000000000..dcb08e722d --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix @@ -0,0 +1,100 @@ +{ + description = "Point-LIO + Livox Mid-360 native module"; + + inputs = { + nixpkgs.url = "github:NixOS/nixpkgs/nixos-unstable"; + flake-utils.url = "github:numtide/flake-utils"; + livox-sdk.url = "path:../../livox/cpp"; + livox-sdk.inputs.nixpkgs.follows = "nixpkgs"; + livox-sdk.inputs.flake-utils.follows = "flake-utils"; + livox-sdk.inputs.lcm-extended.follows = "lcm-extended"; + dimos-lcm = { + url = "github:dimensionalOS/dimos-lcm/main"; + flake = false; + }; + fast-lio = { + url = "git+file:///Users/jeffhykin/repos/dimos-module-fastlio2?ref=pointlio"; + flake = false; + }; + lcm-extended = { + url = "github:jeff-hykin/lcm_extended"; + inputs.nixpkgs.follows = "nixpkgs"; + inputs.flake-utils.follows = "flake-utils"; + }; + }; + + outputs = { self, nixpkgs, flake-utils, livox-sdk, dimos-lcm, fast-lio, lcm-extended, ... }: + flake-utils.lib.eachDefaultSystem (system: + let + # Overlay fixes for darwin-broken nixpkgs recipes in our transitive + # dep chain (pcl → vtk → pdal → tiledb → libpqxx). Each of these + # should go upstream; kept here so we can build in the meantime. + # + # Gated on isDarwin so Linux keeps binary-cache hits for the stock + # libpqxx / tiledb / pdal / vtk / pcl derivations. Applying the + # override on Linux would change their input hashes and force a + # from-source rebuild of the whole chain for no benefit. + darwinDepFixes = final: prev: + if !prev.stdenv.isDarwin then { } else { + # libpqxx: postgresqlTestHook is in nativeCheckInputs + # unconditionally and that package is marked broken on darwin. + # The list is eagerly evaluated, so simply referencing it aborts + # eval. Upstream fix is to wrap the list in + # `lib.optionals (meta.availableOn ...)`. + libpqxx = prev.libpqxx.overrideAttrs (_old: { + nativeCheckInputs = [ ]; + doCheck = false; + }); + # tiledb: darwin-only patch `generate_embedded_data_header.patch` + # targets a file that doesn't exist in tiledb 2.30.0 (the + # upstream code path was reworked and `file(ARCHIVE_CREATE ...)` + # is no longer used anywhere in the source). Filter out only + # that patch — don't drop everything, in case nixpkgs adds an + # unrelated security patch in a future bump. + tiledb = prev.tiledb.overrideAttrs (old: { + patches = builtins.filter + (p: !(prev.lib.hasSuffix "generate_embedded_data_header.patch" (toString p))) + (old.patches or [ ]); + }); + }; + pkgs = import nixpkgs { + inherit system; + overlays = [ darwinDepFixes ]; + }; + livox-sdk2 = livox-sdk.packages.${system}.livox-sdk2; + lcm = lcm-extended.packages.${system}.lcm; + + livox-common = ../../common; + + pointlio_native = pkgs.stdenv.mkDerivation { + pname = "pointlio_native"; + version = "0.2.0"; + + src = ./.; + + nativeBuildInputs = [ pkgs.cmake pkgs.pkg-config ]; + buildInputs = [ + livox-sdk2 + lcm + pkgs.glib + pkgs.eigen + pkgs.pcl + pkgs.yaml-cpp + pkgs.boost + pkgs.llvmPackages.openmp + ]; + + cmakeFlags = [ + "-DCMAKE_POLICY_VERSION_MINIMUM=3.5" + "-DFETCHCONTENT_SOURCE_DIR_DIMOS_LCM=${dimos-lcm}" + "-DFASTLIO_DIR=${fast-lio}" + "-DLIVOX_COMMON_DIR=${livox-common}" + ]; + }; + in { + packages = { + default = pointlio_native; + inherit pointlio_native; + }; + }); +} diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp new file mode 100644 index 0000000000..8d645031cf --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -0,0 +1,664 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// FAST-LIO2 + Livox Mid-360 native module for dimos NativeModule framework. +// +// Binds Livox SDK2 directly into FAST-LIO-NON-ROS: SDK callbacks feed +// CustomMsg/Imu to FastLio, which performs EKF-LOAM SLAM. Sensor-frame +// (mid360_link) point clouds and odometry are published on LCM. +// +// Usage: +// ./fastlio2_native \ +// --lidar '/lidar#sensor_msgs.PointCloud2' \ +// --odometry '/odometry#nav_msgs.Odometry' \ +// --config_path /path/to/default.yaml \ +// --host_ip 192.168.1.5 --lidar_ip 192.168.1.155 + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "livox_sdk_config.hpp" + +#include "cloud_filter.hpp" +#include "dimos_native_module.hpp" +#include "timing.hpp" + +// dimos LCM message headers +#include "geometry_msgs/Quaternion.hpp" +#include "geometry_msgs/Vector3.hpp" +#include "nav_msgs/Odometry.hpp" +#include "sensor_msgs/Imu.hpp" +#include "sensor_msgs/PointCloud2.hpp" +#include "sensor_msgs/PointField.hpp" + +// FAST-LIO (header-only core, compiled sources linked via CMake) +#include "fast_lio.hpp" +#include "fast_lio_debug.hpp" + +using livox_common::GRAVITY_MS2; +using livox_common::DATA_TYPE_IMU; +using livox_common::DATA_TYPE_CARTESIAN_HIGH; +using livox_common::DATA_TYPE_CARTESIAN_LOW; + +// --------------------------------------------------------------------------- +// Global state +// --------------------------------------------------------------------------- + +static std::atomic g_running{true}; +static lcm::LCM* g_lcm = nullptr; +static FastLio* g_fastlio = nullptr; + +static double get_publish_ts() { + return std::chrono::duration( + std::chrono::system_clock::now().time_since_epoch()).count(); +} + +static std::string g_lidar_topic; +static std::string g_odometry_topic; +static std::string g_frame_id; // required via --frame_id +static std::string g_child_frame_id; // required via --child_frame_id +static float g_frequency = 10.0f; + +// Initial pose offset (applied to all SLAM outputs) +static double g_init_x = 0.0; +static double g_init_y = 0.0; +static double g_init_z = 0.0; +static double g_init_qx = 0.0; +static double g_init_qy = 0.0; +static double g_init_qz = 0.0; +static double g_init_qw = 1.0; + +// Hamilton product: q_out = q1 * q2 +static void quat_mul(double ax, double ay, double az, double aw, + double bx, double by, double bz, double bw, + double& ox, double& oy, double& oz, double& ow) { + ow = aw*bw - ax*bx - ay*by - az*bz; + ox = aw*bx + ax*bw + ay*bz - az*by; + oy = aw*by - ax*bz + ay*bw + az*bx; + oz = aw*bz + ax*by - ay*bx + az*bw; +} + +// Rotate vector by quaternion: v_out = q * v * q_inv +static void quat_rotate(double qx, double qy, double qz, double qw, + double vx, double vy, double vz, + double& ox, double& oy, double& oz) { + double tx = 2.0 * (qy*vz - qz*vy); + double ty = 2.0 * (qz*vx - qx*vz); + double tz = 2.0 * (qx*vy - qy*vx); + ox = vx + qw*tx + (qy*tz - qz*ty); + oy = vy + qw*ty + (qz*tx - qx*tz); + oz = vz + qw*tz + (qx*ty - qy*tx); +} + +static bool has_init_pose() { + return g_init_x != 0.0 || g_init_y != 0.0 || g_init_z != 0.0 || + g_init_qx != 0.0 || g_init_qy != 0.0 || g_init_qz != 0.0 || g_init_qw != 1.0; +} + +// Frame accumulator (Livox SDK raw → CustomMsg) +static std::mutex g_pc_mutex; +static std::vector g_accumulated_points; +static uint64_t g_frame_start_ns = 0; +static bool g_frame_has_timestamp = false; + +// --------------------------------------------------------------------------- +// Helpers +// --------------------------------------------------------------------------- + +static uint64_t get_timestamp_ns(const LivoxLidarEthernetPacket* pkt) { + uint64_t ns = 0; + std::memcpy(&ns, pkt->timestamp, sizeof(uint64_t)); + return ns; +} + +using dimos::time_from_seconds; +using dimos::make_header; + +// --------------------------------------------------------------------------- +// Publish lidar point cloud in the sensor body frame (g_frame_id / mid360_link) +// --------------------------------------------------------------------------- +// +// `cloud` is FAST-LIO's undistorted scan in the sensor's own frame +// (get_body_cloud), so points are published as-is with no world registration. +static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, + const std::string& topic = "") { + const std::string& chan = topic.empty() ? g_lidar_topic : topic; + if (!g_lcm || !cloud || cloud->empty() || chan.empty()) return; + + int num_points = static_cast(cloud->size()); + + sensor_msgs::PointCloud2 pc; + pc.header = make_header(g_frame_id, timestamp); + pc.height = 1; + pc.width = num_points; + pc.is_bigendian = 0; + pc.is_dense = 1; + + // x, y, z, intensity (float32 each) + pc.fields_length = 4; + pc.fields.resize(4); + + auto make_field = [](const std::string& name, int32_t offset) { + sensor_msgs::PointField f; + f.name = name; + f.offset = offset; + f.datatype = sensor_msgs::PointField::FLOAT32; + f.count = 1; + return f; + }; + + pc.fields[0] = make_field("x", 0); + pc.fields[1] = make_field("y", 4); + pc.fields[2] = make_field("z", 8); + pc.fields[3] = make_field("intensity", 12); + + pc.point_step = 16; + pc.row_step = pc.point_step * num_points; + + pc.data_length = pc.row_step; + pc.data.resize(pc.data_length); + + for (int i = 0; i < num_points; ++i) { + float* dst = reinterpret_cast(pc.data.data() + i * 16); + dst[0] = cloud->points[i].x; + dst[1] = cloud->points[i].y; + dst[2] = cloud->points[i].z; + dst[3] = cloud->points[i].intensity; + } + + g_lcm->publish(chan, &pc); +} + +// --------------------------------------------------------------------------- +// Publish odometry +// --------------------------------------------------------------------------- + +static void publish_odometry(const custom_messages::Odometry& odom, double timestamp) { + if (!g_lcm) return; + + nav_msgs::Odometry msg; + msg.header = make_header(g_frame_id, timestamp); + msg.child_frame_id = g_child_frame_id; + + // p_out = R_init * p_slam + t_init + if (has_init_pose()) { + double rx, ry, rz; + quat_rotate(g_init_qx, g_init_qy, g_init_qz, g_init_qw, + odom.pose.pose.position.x, + odom.pose.pose.position.y, + odom.pose.pose.position.z, + rx, ry, rz); + msg.pose.pose.position.x = rx + g_init_x; + msg.pose.pose.position.y = ry + g_init_y; + msg.pose.pose.position.z = rz + g_init_z; + + double ox, oy, oz, ow; + quat_mul(g_init_qx, g_init_qy, g_init_qz, g_init_qw, + odom.pose.pose.orientation.x, + odom.pose.pose.orientation.y, + odom.pose.pose.orientation.z, + odom.pose.pose.orientation.w, + ox, oy, oz, ow); + msg.pose.pose.orientation.x = ox; + msg.pose.pose.orientation.y = oy; + msg.pose.pose.orientation.z = oz; + msg.pose.pose.orientation.w = ow; + } else { + msg.pose.pose.position.x = odom.pose.pose.position.x; + msg.pose.pose.position.y = odom.pose.pose.position.y; + msg.pose.pose.position.z = odom.pose.pose.position.z; + msg.pose.pose.orientation.x = odom.pose.pose.orientation.x; + msg.pose.pose.orientation.y = odom.pose.pose.orientation.y; + msg.pose.pose.orientation.z = odom.pose.pose.orientation.z; + msg.pose.pose.orientation.w = odom.pose.pose.orientation.w; + } + + for (int i = 0; i < 36; ++i) { + msg.pose.covariance[i] = odom.pose.covariance[i]; + } + + // Twist zeroed — FAST-LIO doesn't output velocity. + msg.twist.twist.linear.x = 0; + msg.twist.twist.linear.y = 0; + msg.twist.twist.linear.z = 0; + msg.twist.twist.angular.x = 0; + msg.twist.twist.angular.y = 0; + msg.twist.twist.angular.z = 0; + std::memset(msg.twist.covariance, 0, sizeof(msg.twist.covariance)); + + g_lcm->publish(g_odometry_topic, &msg); +} + + +// --------------------------------------------------------------------------- +// Livox SDK callbacks +// --------------------------------------------------------------------------- + +static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/, + LivoxLidarEthernetPacket* data, void* /*client_data*/) { + if (!g_running.load() || data == nullptr) return; + + uint64_t ts_ns = get_timestamp_ns(data); + uint16_t dot_num = data->dot_num; + + // Per-point intra-packet offset (matches livox_ros_driver2). Without it all + // points share one timestamp and per-point deskew is lost. time_interval + // unit is 0.1us, so *100 → ns. + const uint64_t point_interval_ns = + dot_num > 0 ? static_cast(data->time_interval) * 100 / dot_num : 0; + + std::lock_guard lock(g_pc_mutex); + + if (!g_frame_has_timestamp) { + g_frame_start_ns = ts_ns; + g_frame_has_timestamp = true; + } + + if (data->data_type == DATA_TYPE_CARTESIAN_HIGH) { + auto* pts = reinterpret_cast(data->data); + for (uint16_t i = 0; i < dot_num; ++i) { + custom_messages::CustomPoint cp; + cp.x = static_cast(pts[i].x) / 1000.0; // mm → m + cp.y = static_cast(pts[i].y) / 1000.0; + cp.z = static_cast(pts[i].z) / 1000.0; + cp.reflectivity = pts[i].reflectivity; + cp.tag = pts[i].tag; + cp.line = 0; // Mid-360: single line + cp.offset_time = static_cast((ts_ns - g_frame_start_ns) + i * point_interval_ns); + g_accumulated_points.push_back(cp); + } + } else if (data->data_type == DATA_TYPE_CARTESIAN_LOW) { + auto* pts = reinterpret_cast(data->data); + for (uint16_t i = 0; i < dot_num; ++i) { + custom_messages::CustomPoint cp; + cp.x = static_cast(pts[i].x) / 100.0; // cm → m + cp.y = static_cast(pts[i].y) / 100.0; + cp.z = static_cast(pts[i].z) / 100.0; + cp.reflectivity = pts[i].reflectivity; + cp.tag = pts[i].tag; + cp.line = 0; + cp.offset_time = static_cast((ts_ns - g_frame_start_ns) + i * point_interval_ns); + g_accumulated_points.push_back(cp); + } + } +} + +static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, + LivoxLidarEthernetPacket* data, void* /*client_data*/) { + if (!g_running.load() || data == nullptr || !g_fastlio) return; + + uint64_t pkt_ts_ns = get_timestamp_ns(data); + // Live IMU-drop instrumentation: a dropped datagram shows as a sensor-ts + // jump; wall gaps exceeding sensor gaps mean callback starvation. + { + static std::atomic last_pkt_ts_ns{0}; + static std::atomic imu_pkt_count{0}; + static std::atomic imu_gap_count{0}; + static std::atomic max_sensor_gap_us{0}; + using clk = std::chrono::steady_clock; + static auto last_wall = clk::now(); + auto now_wall = clk::now(); + uint64_t prev = last_pkt_ts_ns.exchange(pkt_ts_ns); + uint64_t n = imu_pkt_count.fetch_add(1) + 1; + if (prev != 0 && pkt_ts_ns > prev) { + uint64_t sensor_gap_us = (pkt_ts_ns - prev) / 1000; + uint64_t wall_gap_us = std::chrono::duration_cast( + now_wall - last_wall).count(); + uint64_t cur_max = max_sensor_gap_us.load(); + while (sensor_gap_us > cur_max && + !max_sensor_gap_us.compare_exchange_weak(cur_max, sensor_gap_us)) {} + if (sensor_gap_us > 15000) { + imu_gap_count.fetch_add(1); + fprintf(stderr, "[imu-gap] sensor_gap=%.1fms wall_gap=%.1fms pkt#%llu\n", + sensor_gap_us / 1000.0, wall_gap_us / 1000.0, + static_cast(n)); + } + } + last_wall = now_wall; + if (n % 1000 == 0) { + fprintf(stderr, "[imu-stats] pkts=%llu gaps>15ms=%llu max_sensor_gap=%.1fms\n", + static_cast(n), + static_cast(imu_gap_count.load()), + max_sensor_gap_us.load() / 1000.0); + } + } + + double ts = static_cast(pkt_ts_ns) / 1e9; + auto* imu_pts = reinterpret_cast(data->data); + uint16_t dot_num = data->dot_num; + + for (uint16_t i = 0; i < dot_num; ++i) { + auto imu_msg = boost::make_shared(); + imu_msg->header.stamp = custom_messages::Time().fromSec(ts); + imu_msg->header.seq = 0; + imu_msg->header.frame_id = "livox_frame"; + + imu_msg->orientation.x = 0.0; + imu_msg->orientation.y = 0.0; + imu_msg->orientation.z = 0.0; + imu_msg->orientation.w = 1.0; + for (int j = 0; j < 9; ++j) + imu_msg->orientation_covariance[j] = 0.0; + + imu_msg->angular_velocity.x = static_cast(imu_pts[i].gyro_x); + imu_msg->angular_velocity.y = static_cast(imu_pts[i].gyro_y); + imu_msg->angular_velocity.z = static_cast(imu_pts[i].gyro_z); + for (int j = 0; j < 9; ++j) + imu_msg->angular_velocity_covariance[j] = 0.0; + + // Point-LIO expects accel in g (EKF does its own scaling). SDK already + // reports g, so feed raw — scaling by GRAVITY_MS2 would double-scale and + // trip the satu_acc check at rest. + imu_msg->linear_acceleration.x = static_cast(imu_pts[i].acc_x); + imu_msg->linear_acceleration.y = static_cast(imu_pts[i].acc_y); + imu_msg->linear_acceleration.z = static_cast(imu_pts[i].acc_z); + for (int j = 0; j < 9; ++j) + imu_msg->linear_acceleration_covariance[j] = 0.0; + + g_fastlio->feed_imu(imu_msg); + } +} + +static void on_info_change(const uint32_t handle, const LivoxLidarInfo* info, + void* /*client_data*/) { + if (info == nullptr) return; + + char sn[17] = {}; + std::memcpy(sn, info->sn, 16); + char ip[17] = {}; + std::memcpy(ip, info->lidar_ip, 16); + + if (fastlio_debug) { + printf("[fastlio2] Device connected: handle=%u type=%u sn=%s ip=%s\n", + handle, info->dev_type, sn, ip); + } + + SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, nullptr, nullptr); + EnableLivoxLidarImuData(handle, nullptr, nullptr); +} + +// --------------------------------------------------------------------------- +// Signal handling +// --------------------------------------------------------------------------- + +static void signal_handler(int /*sig*/) { + g_running.store(false); +} + +// --------------------------------------------------------------------------- +// Main +// --------------------------------------------------------------------------- + +int main(int argc, char** argv) { + dimos::NativeModule mod(argc, argv); + + // Required: LCM topics for output ports + g_lidar_topic = mod.has("lidar") ? mod.topic("lidar") : ""; + g_odometry_topic = mod.has("odometry") ? mod.topic("odometry") : ""; + + if (g_lidar_topic.empty() && g_odometry_topic.empty()) { + fprintf(stderr, "Error: at least one of --lidar or --odometry is required\n"); + return 1; + } + + // FAST-LIO config path + std::string config_path = mod.arg("config_path", ""); + if (config_path.empty()) { + fprintf(stderr, "Error: --config_path is required\n"); + return 1; + } + + // FAST-LIO internal processing rates + double msr_freq = mod.arg_float("msr_freq", 50.0f); + double main_freq = mod.arg_float("main_freq", 5000.0f); + + // Livox hardware config + std::string host_ip = mod.arg("host_ip", "192.168.1.5"); + std::string lidar_ip = mod.arg("lidar_ip", "192.168.1.155"); + g_frequency = mod.arg_float("frequency", 10.0f); + g_frame_id = mod.arg_required("frame_id"); + g_child_frame_id = mod.arg_required("odom_frame_id"); + float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f); + float odom_freq = mod.arg_float("odom_freq", 50.0f); + CloudFilterConfig filter_cfg; + filter_cfg.voxel_size = mod.arg_float("voxel_size", 0.1f); + filter_cfg.sor_mean_k = mod.arg_int("sor_mean_k", 50); + filter_cfg.sor_stddev = mod.arg_float("sor_stddev", 1.0f); + + // Propagates to the FAST-LIO core via the `fastlio_debug` global. + bool debug = mod.arg_bool("debug", false); + fastlio_debug = debug; + + // SDK network ports (defaults from SdkPorts struct in livox_sdk_config.hpp) + livox_common::SdkPorts ports; + const livox_common::SdkPorts port_defaults; + ports.cmd_data = mod.arg_int("cmd_data_port", port_defaults.cmd_data); + ports.push_msg = mod.arg_int("push_msg_port", port_defaults.push_msg); + ports.point_data = mod.arg_int("point_data_port", port_defaults.point_data); + ports.imu_data = mod.arg_int("imu_data_port", port_defaults.imu_data); + ports.log_data = mod.arg_int("log_data_port", port_defaults.log_data); + ports.host_cmd_data = mod.arg_int("host_cmd_data_port", port_defaults.host_cmd_data); + ports.host_push_msg = mod.arg_int("host_push_msg_port", port_defaults.host_push_msg); + ports.host_point_data = mod.arg_int("host_point_data_port", port_defaults.host_point_data); + ports.host_imu_data = mod.arg_int("host_imu_data_port", port_defaults.host_imu_data); + ports.host_log_data = mod.arg_int("host_log_data_port", port_defaults.host_log_data); + + // Initial pose offset [x, y, z, qx, qy, qz, qw] + { + std::string init_str = mod.arg("init_pose", ""); + if (!init_str.empty()) { + double vals[7] = {0, 0, 0, 0, 0, 0, 1}; + int n = 0; + size_t pos = 0; + while (pos < init_str.size() && n < 7) { + size_t comma = init_str.find(',', pos); + if (comma == std::string::npos) comma = init_str.size(); + vals[n++] = std::stod(init_str.substr(pos, comma - pos)); + pos = comma + 1; + } + g_init_x = vals[0]; g_init_y = vals[1]; g_init_z = vals[2]; + g_init_qx = vals[3]; g_init_qy = vals[4]; g_init_qz = vals[5]; g_init_qw = vals[6]; + } + } + + if (debug) { + printf("[fastlio2] Starting FAST-LIO2 + Livox Mid-360 native module\n"); + if (has_init_pose()) { + printf("[fastlio2] init_pose: xyz=(%.3f, %.3f, %.3f) quat=(%.4f, %.4f, %.4f, %.4f)\n", + g_init_x, g_init_y, g_init_z, g_init_qx, g_init_qy, g_init_qz, g_init_qw); + } + printf("[fastlio2] lidar topic: %s\n", + g_lidar_topic.empty() ? "(disabled)" : g_lidar_topic.c_str()); + printf("[fastlio2] odometry topic: %s\n", + g_odometry_topic.empty() ? "(disabled)" : g_odometry_topic.c_str()); + printf("[fastlio2] config: %s\n", config_path.c_str()); + printf("[fastlio2] host_ip: %s lidar_ip: %s frequency: %.1f Hz\n", + host_ip.c_str(), lidar_ip.c_str(), g_frequency); + printf("[fastlio2] pointcloud_freq: %.1f Hz odom_freq: %.1f Hz\n", + pointcloud_freq, odom_freq); + printf("[fastlio2] voxel_size: %.3f sor_mean_k: %d sor_stddev: %.1f\n", + filter_cfg.voxel_size, filter_cfg.sor_mean_k, filter_cfg.sor_stddev); + } + + // Signal handlers + signal(SIGTERM, signal_handler); + signal(SIGINT, signal_handler); + + // Init LCM + lcm::LCM lcm; + if (!lcm.good()) { + fprintf(stderr, "Error: LCM init failed\n"); + return 1; + } + g_lcm = &lcm; + + if (debug) printf("[fastlio2] Initializing FAST-LIO...\n"); + FastLio fast_lio(config_path, msr_freq, main_freq); + g_fastlio = &fast_lio; + if (debug) printf("[fastlio2] FAST-LIO initialized.\n"); + + // Main-loop state. Body lives in `run_main_iter`, driven by the wall-paced + // main thread. + auto frame_interval = std::chrono::microseconds( + static_cast(1e6 / g_frequency)); + std::optional last_emit; + const double process_period_ms = 1000.0 / main_freq; + + auto pc_interval = std::chrono::microseconds( + static_cast(1e6 / pointcloud_freq)); + auto odom_interval = std::chrono::microseconds( + static_cast(1e6 / odom_freq)); + std::optional last_pc_publish; + std::optional last_odom_publish; + + + // Per-section timing for `run_main_iter`, active only with --debug. + // maybe_flush() below prints a summary every second. + static timing::Section t_iter{"run_main_iter"}; + static timing::Section t_emit_check{"emit.lock+swap"}; + static timing::Section t_feed_lidar{"fast_lio.feed_lidar"}; + static timing::Section t_process{"fast_lio.process"}; + static timing::Section t_get_world_cloud{"fast_lio.get_body_cloud"}; + static timing::Section t_filter_cloud{"filter_cloud"}; + static timing::Section t_publish_lidar{"publish_lidar"}; + static timing::Section t_publish_odom{"publish_odometry"}; + + auto run_main_iter = [&](std::chrono::steady_clock::time_point now) { + timing::Scope iter_scope(t_iter); + // Lazy-seed rate-limit bookmarks on the first iteration so they align + // with the wall clock. + if (!last_emit.has_value()) { + last_emit = now; + } + if (!last_pc_publish.has_value()) { + last_pc_publish = now; + } + if (!last_odom_publish.has_value()) { + last_odom_publish = now; + } + + // At frame rate: drain accumulated points into a CustomMsg and feed + // FAST-LIO. Hold g_pc_mutex across the rate-limit check AND swap so the + // clock + accumulator are observed atomically (no packet slips between). + std::vector points; + uint64_t frame_start = 0; + { + timing::Scope s(t_emit_check); + std::lock_guard lock(g_pc_mutex); + if (now - *last_emit >= frame_interval) { + if (!g_accumulated_points.empty()) { + points.swap(g_accumulated_points); + frame_start = g_frame_start_ns; + g_frame_has_timestamp = false; + } + last_emit = now; + } + } + if (!points.empty()) { + auto lidar_msg = boost::make_shared(); + lidar_msg->header.seq = 0; + lidar_msg->header.stamp = custom_messages::Time().fromSec( + static_cast(frame_start) / 1e9); + lidar_msg->header.frame_id = "livox_frame"; + lidar_msg->timebase = frame_start; + lidar_msg->lidar_id = 0; + for (int i = 0; i < 3; i++) lidar_msg->rsvd[i] = 0; + lidar_msg->point_num = static_cast(points.size()); + lidar_msg->points = std::move(points); + timing::Scope s(t_feed_lidar); + fast_lio.feed_lidar(lidar_msg); + } + + // One FAST-LIO IESKF step (cheap when queues empty). + { + timing::Scope s(t_process); + fast_lio.process(); + } + + auto pose = fast_lio.get_pose(); + if (!pose.empty() && (pose[0] != 0.0 || pose[1] != 0.0 || pose[2] != 0.0)) { + double ts = get_publish_ts(); + + const bool lidar_due = + !g_lidar_topic.empty() && now - *last_pc_publish >= pc_interval; + + // get_body_cloud + filter_cloud (SOR) is the loop's costliest step, + // so build it only when a publish is due. + if (lidar_due) { + auto body_cloud = ([&]() { + timing::Scope s(t_get_world_cloud); + return fast_lio.get_body_cloud(); + })(); + if (body_cloud && !body_cloud->empty()) { + auto filtered = ([&]() { + timing::Scope s(t_filter_cloud); + return filter_cloud(body_cloud, filter_cfg); + })(); + timing::Scope s(t_publish_lidar); + publish_lidar(filtered, ts); + last_pc_publish = now; + } + } + + // Pose + covariance at odom_freq. + if (!g_odometry_topic.empty() && now - *last_odom_publish >= odom_interval) { + timing::Scope s(t_publish_odom); + publish_odometry(fast_lio.get_odometry(), ts); + last_odom_publish = now; + } + } + + timing::maybe_flush(std::chrono::steady_clock::now()); + }; + + // Packet source: Livox SDK callbacks from its own threads feed the + // accumulator/EKF; the main thread below owns run_main_iter. + if (!livox_common::init_livox_sdk(host_ip, lidar_ip, ports, debug)) { + return 1; + } + SetLivoxLidarPointCloudCallBack(on_point_cloud, nullptr); + SetLivoxLidarImuDataCallback(on_imu_data, nullptr); + SetLivoxLidarInfoChangeCallback(on_info_change, nullptr); + if (!LivoxLidarSdkStart()) { + fprintf(stderr, "Error: LivoxLidarSdkStart failed\n"); + LivoxLidarSdkUninit(); + return 1; + } + if (debug) printf("[fastlio2] SDK started, waiting for device...\n"); + + while (g_running.load()) { + auto loop_start = std::chrono::high_resolution_clock::now(); + run_main_iter(std::chrono::steady_clock::now()); + + lcm.handleTimeout(0); + + // Rate control (~main_freq, 5kHz default). + auto loop_end = std::chrono::high_resolution_clock::now(); + auto elapsed_ms = std::chrono::duration(loop_end - loop_start).count(); + if (elapsed_ms < process_period_ms) { + std::this_thread::sleep_for(std::chrono::microseconds( + static_cast((process_period_ms - elapsed_ms) * 1000))); + } + } + + // Cleanup + if (debug) printf("[fastlio2] Shutting down...\n"); + g_fastlio = nullptr; + LivoxLidarSdkUninit(); + g_lcm = nullptr; + + if (debug) printf("[fastlio2] Done.\n"); + return 0; +} diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp b/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp new file mode 100644 index 0000000000..ddd72eac47 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp @@ -0,0 +1,114 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Lightweight per-section timing for `run_main_iter`. Active only when the +// global `fastlio_debug` flag is set, so non-debug runs pay one branch per +// scope. +// +// Usage: +// static timing::Section sec{"filter_cloud"}; +// { timing::Scope s(sec); /* work */ } +// timing::maybe_flush(now); // periodically + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "fast_lio_debug.hpp" + +namespace timing { + +struct Section { + const char* name; + std::atomic count{0}; + std::atomic total_ns{0}; + std::atomic max_ns{0}; + + explicit Section(const char* n); + + void add(uint64_t ns) { + count.fetch_add(1, std::memory_order_relaxed); + total_ns.fetch_add(ns, std::memory_order_relaxed); + uint64_t prev = max_ns.load(std::memory_order_relaxed); + while (ns > prev && + !max_ns.compare_exchange_weak(prev, ns, std::memory_order_relaxed)) { + } + } +}; + +inline std::vector& registry() { + static std::vector r; + return r; +} + +inline Section::Section(const char* n) : name(n) { + registry().push_back(this); +} + +struct Scope { + Section& sec; + std::chrono::steady_clock::time_point t0; + bool active; + + explicit Scope(Section& s) : sec(s), active(fastlio_debug) { + if (active) { + t0 = std::chrono::steady_clock::now(); + } + } + + ~Scope() { + if (!active) { + return; + } + auto dt = std::chrono::duration_cast( + std::chrono::steady_clock::now() - t0).count(); + sec.add(static_cast(dt)); + } +}; + +// Print one line per section to stderr every FLUSH_INTERVAL, then reset. +// Mutex serialises flushes across threads (SDK callbacks vs main loop). +inline void maybe_flush(std::chrono::steady_clock::time_point now) { + if (!fastlio_debug) { + return; + } + constexpr auto FLUSH_INTERVAL = std::chrono::seconds(1); + static std::mutex mtx; + static std::chrono::steady_clock::time_point last; + std::lock_guard lock(mtx); + if (last.time_since_epoch().count() == 0) { + last = now; + return; + } + if (now - last < FLUSH_INTERVAL) { + return; + } + auto dt_ms = std::chrono::duration(now - last).count(); + last = now; + + for (Section* s : registry()) { + uint64_t c = s->count.exchange(0, std::memory_order_relaxed); + uint64_t tot = s->total_ns.exchange(0, std::memory_order_relaxed); + uint64_t mx = s->max_ns.exchange(0, std::memory_order_relaxed); + if (c == 0) { + std::fprintf(stderr, "[timing] %-24s n=0\n", s->name); + continue; + } + double mean_us = static_cast(tot) / static_cast(c) / 1e3; + double max_us = static_cast(mx) / 1e3; + double total_ms = static_cast(tot) / 1e6; + double rate_hz = static_cast(c) * 1000.0 / dt_ms; + std::fprintf(stderr, + "[timing] %-24s n=%5lu rate=%7.1fHz mean=%8.3fus max=%9.3fus tot=%7.2fms\n", + s->name, + static_cast(c), + rate_hz, mean_us, max_us, total_ms); + } +} + +} // namespace timing diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/voxel_map.hpp b/dimos/hardware/sensors/lidar/pointlio/cpp/voxel_map.hpp new file mode 100644 index 0000000000..a50740cd04 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/voxel_map.hpp @@ -0,0 +1,297 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Efficient global voxel map using a hash map. +// Supports O(1) insert/update, distance-based pruning, and +// raycasting-based free space clearing via Amanatides & Woo 3D DDA. +// FOV is discovered dynamically from incoming point cloud data. + +#ifndef VOXEL_MAP_HPP_ +#define VOXEL_MAP_HPP_ + +#include +#include +#include + +#include +#include + +struct VoxelKey { + int32_t x, y, z; + bool operator==(const VoxelKey& o) const { return x == o.x && y == o.y && z == o.z; } +}; + +struct VoxelKeyHash { + size_t operator()(const VoxelKey& k) const { + // Fast spatial hash — large primes reduce collisions for grid coords + size_t h = static_cast(k.x) * 73856093u; + h ^= static_cast(k.y) * 19349669u; + h ^= static_cast(k.z) * 83492791u; + return h; + } +}; + +struct Voxel { + float x, y, z; // running centroid + float intensity; + uint32_t count; // points merged into this voxel + uint8_t miss_count; // consecutive scans where a ray passed through without hitting +}; + +/// Config for raycast-based free space clearing. +struct RaycastConfig { + int subsample = 4; // raycast every Nth point + int max_misses = 3; // erase after this many consecutive misses + float fov_margin_rad = 0.035f; // ~2° safety margin added to discovered FOV +}; + +class VoxelMap { +public: + explicit VoxelMap(float voxel_size, float max_range = 100.0f) + : voxel_size_(voxel_size), max_range_(max_range) { + map_.reserve(500000); + } + + /// Insert a point cloud into the map, merging into existing voxels. + /// Resets miss_count for hit voxels. + template + void insert(const typename pcl::PointCloud::Ptr& cloud) { + if (!cloud) return; + float inv = 1.0f / voxel_size_; + for (const auto& pt : cloud->points) { + VoxelKey key{ + static_cast(std::floor(pt.x * inv)), + static_cast(std::floor(pt.y * inv)), + static_cast(std::floor(pt.z * inv))}; + + auto it = map_.find(key); + if (it != map_.end()) { + // Running average update + auto& v = it->second; + float n = static_cast(v.count); + float n1 = n + 1.0f; + v.x = (v.x * n + pt.x) / n1; + v.y = (v.y * n + pt.y) / n1; + v.z = (v.z * n + pt.z) / n1; + v.intensity = (v.intensity * n + pt.intensity) / n1; + v.count++; + v.miss_count = 0; + } else { + map_.emplace(key, Voxel{pt.x, pt.y, pt.z, pt.intensity, 1, 0}); + } + } + } + + /// Cast rays from sensor origin through each point in the cloud. + /// Discovers the sensor FOV from the cloud's elevation angle range, + /// then marks intermediate voxels as missed and erases those exceeding + /// the miss threshold within the discovered FOV. + /// + /// Orientation quaternion (qx,qy,qz,qw) is body→world. + template + void raycast_clear(float ox, float oy, float oz, + float qx, float qy, float qz, float qw, + const typename pcl::PointCloud::Ptr& cloud, + const RaycastConfig& cfg) { + if (!cloud || cloud->empty() || cfg.max_misses <= 0) return; + + // Phase 0: discover FOV from this scan's elevation angles in sensor-local frame + update_fov(ox, oy, oz, qx, qy, qz, qw, cloud); + + // Skip raycasting until we have a valid FOV (need at least a few scans) + if (!fov_valid_) return; + + float inv = 1.0f / voxel_size_; + int n_pts = static_cast(cloud->size()); + float fov_up = fov_up_ + cfg.fov_margin_rad; + float fov_down = fov_down_ - cfg.fov_margin_rad; + + // Phase 1: walk rays, increment miss_count for intermediate voxels + for (int i = 0; i < n_pts; i += cfg.subsample) { + const auto& pt = cloud->points[i]; + raycast_single(ox, oy, oz, pt.x, pt.y, pt.z, inv); + } + + // Phase 2: erase voxels that exceeded miss threshold and are within FOV + for (auto it = map_.begin(); it != map_.end();) { + if (it->second.miss_count > static_cast(cfg.max_misses)) { + if (in_sensor_fov(ox, oy, oz, qx, qy, qz, qw, + it->second.x, it->second.y, it->second.z, + fov_up, fov_down)) { + it = map_.erase(it); + continue; + } + } + ++it; + } + } + + /// Remove voxels farther than max_range from the given position. + void prune(float px, float py, float pz) { + float r2 = max_range_ * max_range_; + for (auto it = map_.begin(); it != map_.end();) { + float dx = it->second.x - px; + float dy = it->second.y - py; + float dz = it->second.z - pz; + if (dx * dx + dy * dy + dz * dz > r2) + it = map_.erase(it); + else + ++it; + } + } + + /// Export all voxel centroids as a point cloud. + template + typename pcl::PointCloud::Ptr to_cloud() const { + typename pcl::PointCloud::Ptr cloud( + new pcl::PointCloud(map_.size(), 1)); + size_t i = 0; + for (const auto& [key, v] : map_) { + auto& pt = cloud->points[i++]; + pt.x = v.x; + pt.y = v.y; + pt.z = v.z; + pt.intensity = v.intensity; + } + return cloud; + } + + size_t size() const { return map_.size(); } + void clear() { map_.clear(); } + void set_max_range(float r) { max_range_ = r; } + float fov_up_deg() const { return fov_up_ * 180.0f / static_cast(M_PI); } + float fov_down_deg() const { return fov_down_ * 180.0f / static_cast(M_PI); } + bool fov_valid() const { return fov_valid_; } + +private: + std::unordered_map map_; + float voxel_size_; + float max_range_; + + // Dynamically discovered sensor FOV (accumulated over scans) + float fov_up_ = -static_cast(M_PI); // start narrow, expand from data + float fov_down_ = static_cast(M_PI); + int fov_scan_count_ = 0; + bool fov_valid_ = false; + static constexpr int FOV_WARMUP_SCANS = 5; // require N scans before trusting FOV + + /// Update discovered FOV from a scan's elevation angles in sensor-local frame. + template + void update_fov(float ox, float oy, float oz, + float qx, float qy, float qz, float qw, + const typename pcl::PointCloud::Ptr& cloud) { + // Inverse quaternion for world→sensor rotation + float nqx = -qx, nqy = -qy, nqz = -qz; + + for (const auto& pt : cloud->points) { + float wx = pt.x - ox, wy = pt.y - oy, wz = pt.z - oz; + + // Rotate to sensor-local frame + float tx = 2.0f * (nqy * wz - nqz * wy); + float ty = 2.0f * (nqz * wx - nqx * wz); + float tz = 2.0f * (nqx * wy - nqy * wx); + float lx = wx + qw * tx + (nqy * tz - nqz * ty); + float ly = wy + qw * ty + (nqz * tx - nqx * tz); + float lz = wz + qw * tz + (nqx * ty - nqy * tx); + + float horiz_dist = std::sqrt(lx * lx + ly * ly); + if (horiz_dist < 1e-6f) continue; + float elevation = std::atan2(lz, horiz_dist); + + if (elevation > fov_up_) fov_up_ = elevation; + if (elevation < fov_down_) fov_down_ = elevation; + } + + if (++fov_scan_count_ >= FOV_WARMUP_SCANS && !fov_valid_) { + fov_valid_ = true; + printf("[voxel_map] FOV discovered: [%.1f, %.1f] deg\n", + fov_down_deg(), fov_up_deg()); + } + } + + /// Amanatides & Woo 3D DDA: walk from (ox,oy,oz) to (px,py,pz), + /// incrementing miss_count for all intermediate voxels. + void raycast_single(float ox, float oy, float oz, + float px, float py, float pz, float inv) { + float dx = px - ox, dy = py - oy, dz = pz - oz; + float len = std::sqrt(dx * dx + dy * dy + dz * dz); + if (len < 1e-6f) return; + dx /= len; dy /= len; dz /= len; + + int32_t cx = static_cast(std::floor(ox * inv)); + int32_t cy = static_cast(std::floor(oy * inv)); + int32_t cz = static_cast(std::floor(oz * inv)); + int32_t ex = static_cast(std::floor(px * inv)); + int32_t ey = static_cast(std::floor(py * inv)); + int32_t ez = static_cast(std::floor(pz * inv)); + + int sx = (dx >= 0) ? 1 : -1; + int sy = (dy >= 0) ? 1 : -1; + int sz = (dz >= 0) ? 1 : -1; + + // tMax: parametric distance along ray to next voxel boundary per axis + // tDelta: parametric distance to cross one full voxel per axis + float tMaxX = (std::abs(dx) < 1e-10f) ? 1e30f + : (((dx > 0 ? cx + 1 : cx) * voxel_size_ - ox) / dx); + float tMaxY = (std::abs(dy) < 1e-10f) ? 1e30f + : (((dy > 0 ? cy + 1 : cy) * voxel_size_ - oy) / dy); + float tMaxZ = (std::abs(dz) < 1e-10f) ? 1e30f + : (((dz > 0 ? cz + 1 : cz) * voxel_size_ - oz) / dz); + + float tDeltaX = (std::abs(dx) < 1e-10f) ? 1e30f : std::abs(voxel_size_ / dx); + float tDeltaY = (std::abs(dy) < 1e-10f) ? 1e30f : std::abs(voxel_size_ / dy); + float tDeltaZ = (std::abs(dz) < 1e-10f) ? 1e30f : std::abs(voxel_size_ / dz); + + // Walk through voxels (skip endpoint — it was hit) + int max_steps = static_cast(len * inv) + 3; // safety bound + for (int step = 0; step < max_steps; ++step) { + if (cx == ex && cy == ey && cz == ez) break; // reached endpoint + + VoxelKey key{cx, cy, cz}; + auto it = map_.find(key); + if (it != map_.end() && it->second.miss_count < 255) { + it->second.miss_count++; + } + + // Step to next voxel on the axis with smallest tMax + if (tMaxX < tMaxY && tMaxX < tMaxZ) { + cx += sx; tMaxX += tDeltaX; + } else if (tMaxY < tMaxZ) { + cy += sy; tMaxY += tDeltaY; + } else { + cz += sz; tMaxZ += tDeltaZ; + } + } + } + + /// Check if a voxel centroid falls within the sensor's vertical FOV. + /// Rotates the vector (sensor→voxel) into sensor-local frame using the + /// inverse of the body→world quaternion, then checks elevation angle. + static bool in_sensor_fov(float ox, float oy, float oz, + float qx, float qy, float qz, float qw, + float vx, float vy, float vz, + float fov_up_rad, float fov_down_rad) { + // Vector from sensor origin to voxel in world frame + float wx = vx - ox, wy = vy - oy, wz = vz - oz; + + // Rotate by quaternion inverse (conjugate): q* = (-qx,-qy,-qz,qw) + float nqx = -qx, nqy = -qy, nqz = -qz; + // t = 2 * cross(q.xyz, v) + float tx = 2.0f * (nqy * wz - nqz * wy); + float ty = 2.0f * (nqz * wx - nqx * wz); + float tz = 2.0f * (nqx * wy - nqy * wx); + // v' = v + qw * t + cross(q.xyz, t) + float lx = wx + qw * tx + (nqy * tz - nqz * ty); + float ly = wy + qw * ty + (nqz * tx - nqx * tz); + float lz = wz + qw * tz + (nqx * ty - nqy * tx); + + // Elevation angle in sensor-local frame + float horiz_dist = std::sqrt(lx * lx + ly * ly); + if (horiz_dist < 1e-6f) return true; // directly above/below, treat as in FOV + float elevation = std::atan2(lz, horiz_dist); + + return elevation >= fov_down_rad && elevation <= fov_up_rad; + } +}; + +#endif diff --git a/dimos/hardware/sensors/lidar/pointlio/fastlio_blueprints.py b/dimos/hardware/sensors/lidar/pointlio/fastlio_blueprints.py new file mode 100644 index 0000000000..89e8ec2c14 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/fastlio_blueprints.py @@ -0,0 +1,67 @@ +# 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 dimos.core.coordination.blueprints import autoconnect +from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 +from dimos.mapping.ray_tracing.module import RayTracingVoxelMap +from dimos.mapping.voxels import VoxelGridMapper +from dimos.visualization.vis_module import vis_module + +voxel_size = 0.05 + + +mid360_fastlio = autoconnect( + FastLio2.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=-1), + vis_module("rerun"), +).global_config(n_workers=2, robot_model="mid360_fastlio2") + +mid360_fastlio_voxels = autoconnect( + FastLio2.blueprint(), + VoxelGridMapper.blueprint(voxel_size=voxel_size, carve_columns=False), + vis_module( + "rerun", + rerun_config={ + "visual_override": { + "world/lidar": None, + }, + }, + ), +).global_config(n_workers=3, robot_model="mid360_fastlio2_voxels") + +mid360_fastlio_voxels_native = autoconnect( + FastLio2.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=3.0), + vis_module( + "rerun", + rerun_config={ + "visual_override": { + "world/lidar": None, + }, + }, + ), +).global_config(n_workers=2, robot_model="mid360_fastlio2") + + +mid360_fastlio_ray_trace = autoconnect( + FastLio2.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=-1), + RayTracingVoxelMap.blueprint(voxel_size=voxel_size), + vis_module( + "rerun", + rerun_config={ + "visual_override": { + "world/lidar": None, + }, + }, + ), +).global_config(n_workers=5, robot_model="mid360_fastlio2_ray_trace") diff --git a/dimos/hardware/sensors/lidar/pointlio/fastlio_test.py b/dimos/hardware/sensors/lidar/pointlio/fastlio_test.py new file mode 120000 index 0000000000..89f5f294ea --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/fastlio_test.py @@ -0,0 +1 @@ +/Users/jeffhykin/repos/dimos6/.ignore.enhance/items/dimos/hardware/sensors/lidar/fastlio2/fastlio_test.py \ No newline at end of file diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py new file mode 100644 index 0000000000..65be5dee6f --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -0,0 +1,263 @@ +# 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. + +"""Python NativeModule wrapper for the Point-LIO + Livox Mid-360 binary. + +Binds Livox SDK2 directly into Point-LIO for real-time LiDAR SLAM. +Outputs registered (world-frame) point clouds and odometry with covariance. + +Usage:: + + from dimos.hardware.sensors.lidar.pointlio.module import PointLio + from dimos.core.coordination.blueprints import autoconnect + + from dimos.core.coordination.module_coordinator import ModuleCoordinator + ModuleCoordinator.build(autoconnect( + PointLio.blueprint(host_ip="192.168.1.5"), + SomeConsumer.blueprint(), + )).loop() +""" + +from __future__ import annotations + +import ipaddress +from pathlib import Path +import socket +import time +from typing import TYPE_CHECKING, Annotated + +from pydantic.experimental.pipeline import validate_as +from reactivex.disposable import Disposable + +from dimos.core.core import rpc +from dimos.core.native_module import NativeModule, NativeModuleConfig +from dimos.core.stream import Out +from dimos.hardware.sensors.lidar.livox.ports import ( + SDK_CMD_DATA_PORT, + SDK_HOST_CMD_DATA_PORT, + SDK_HOST_IMU_DATA_PORT, + SDK_HOST_LOG_DATA_PORT, + SDK_HOST_POINT_DATA_PORT, + SDK_HOST_PUSH_MSG_PORT, + SDK_IMU_DATA_PORT, + SDK_LOG_DATA_PORT, + SDK_POINT_DATA_PORT, + SDK_PUSH_MSG_PORT, +) +from dimos.msgs.geometry_msgs.Pose import Pose +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.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.navigation.nav_stack.frames import FRAME_ODOM +from dimos.spec import perception +from dimos.utils.generic import get_local_ips +from dimos.utils.logging_config import setup_logger + +_CONFIG_DIR = Path(__file__).parent / "config" +_logger = setup_logger() + + +class PointLioConfig(NativeModuleConfig): + cwd: str | None = "cpp" + executable: str = "result/bin/pointlio_native" + build_command: str | None = "nix build .#pointlio_native" + # Livox SDK hardware config + host_ip: str = "192.168.1.5" + lidar_ip: str = "192.168.1.155" + frequency: float = 10.0 + + # Sensor mount pose — position + orientation of the sensor relative to ground. + # Converted to init_pose CLI arg [x, y, z, qx, qy, qz, qw] in model_post_init. + mount: Pose = Pose() + + # frame_id is the header frame for BOTH the point cloud and the odometry + # message (the Mid-360 sensor frame). The TF published by the module is a + # separate odom_parent_frame_id -> odom_frame_id transform. + frame_id: str = "mid360_link" + # TF publish frames (odom -> base_link): the sensor pose expressed as the + # base_link pose in the odom frame. + odom_parent_frame_id: str = FRAME_ODOM + odom_frame_id: str = "base_link" + + # FAST-LIO internal processing rates + msr_freq: float = 50.0 + main_freq: float = 5000.0 + + # Output publish rates (Hz) + pointcloud_freq: float = 10.0 + odom_freq: float = 30.0 + + # Point cloud filtering + voxel_size: float = 0.1 + sor_mean_k: int = 50 + sor_stddev: float = 1.0 + + # FAST-LIO YAML config (relative to config/ dir, or absolute path) + # C++ binary reads YAML directly via yaml-cpp + config: Annotated[ + Path, validate_as(...).transform(lambda p: p if p.is_absolute() else _CONFIG_DIR / p) + ] = Path("default.yaml") + + debug: bool = False + + # SDK port configuration (see livox/ports.py for defaults) + cmd_data_port: int = SDK_CMD_DATA_PORT + push_msg_port: int = SDK_PUSH_MSG_PORT + point_data_port: int = SDK_POINT_DATA_PORT + imu_data_port: int = SDK_IMU_DATA_PORT + log_data_port: int = SDK_LOG_DATA_PORT + host_cmd_data_port: int = SDK_HOST_CMD_DATA_PORT + host_push_msg_port: int = SDK_HOST_PUSH_MSG_PORT + host_point_data_port: int = SDK_HOST_POINT_DATA_PORT + host_imu_data_port: int = SDK_HOST_IMU_DATA_PORT + host_log_data_port: int = SDK_HOST_LOG_DATA_PORT + + # Resolved in __post_init__, passed as --config_path to the binary + config_path: str | None = None + + # init_pose is computed from mount; config is resolved to config_path + init_pose: list[float] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] + cli_exclude: frozenset[str] = frozenset({"config", "mount", "odom_parent_frame_id"}) + + def model_post_init(self, __context: object) -> None: + """Resolve config_path and compute init_pose from mount.""" + super().model_post_init(__context) + cfg = self.config + if not cfg.is_absolute(): + cfg = _CONFIG_DIR / cfg + self.config_path = str(cfg.resolve()) + m = self.mount + self.init_pose = [ + m.x, + m.y, + m.z, + m.orientation.x, + m.orientation.y, + m.orientation.z, + m.orientation.w, + ] + + +class PointLio(NativeModule, perception.Lidar, perception.Odometry): + config: PointLioConfig + + lidar: Out[PointCloud2] + odometry: Out[Odometry] + + @rpc + def start(self) -> None: + self._validate_network() + super().start() + self.register_disposable( + Disposable(self.odometry.transport.subscribe(self._on_odom_for_tf, self.odometry)) + ) + + def _on_odom_for_tf(self, msg: Odometry) -> None: + self.tf.publish( + Transform( + frame_id=self.config.odom_parent_frame_id, + child_frame_id=self.config.odom_frame_id, + translation=Vector3( + msg.pose.position.x, + msg.pose.position.y, + msg.pose.position.z, + ), + rotation=Quaternion( + msg.pose.orientation.x, + msg.pose.orientation.y, + msg.pose.orientation.z, + msg.pose.orientation.w, + ), + ts=msg.ts or time.time(), + ) + ) + + @rpc + def stop(self) -> None: + super().stop() + + def _validate_network(self) -> None: + host_ip = self.config.host_ip + lidar_ip = self.config.lidar_ip + local_ips = [ip for ip, _iface in get_local_ips()] + + _logger.info( + "PointLio network check", + host_ip=host_ip, + lidar_ip=lidar_ip, + local_ips=local_ips, + ) + + # Check if host_ip is actually assigned to this machine. + if host_ip not in local_ips: + try: + lidar_net = ipaddress.IPv4Network(f"{lidar_ip}/24", strict=False) + same_subnet = [ip for ip in local_ips if ipaddress.IPv4Address(ip) in lidar_net] + except (ValueError, TypeError): + same_subnet = [] + + if same_subnet: + picked = same_subnet[0] + _logger.warning( + f"PointLio: host_ip={host_ip!r} not found locally. " + f"Auto-correcting to {picked!r} (same subnet as lidar {lidar_ip}).", + configured_ip=host_ip, + corrected_ip=picked, + lidar_ip=lidar_ip, + local_ips=local_ips, + ) + self.config.host_ip = picked + host_ip = picked + else: + subnet_prefix = ".".join(lidar_ip.split(".")[:3]) + msg = ( + f"PointLio: host_ip={host_ip!r} is not assigned to any local interface.\n" + f" Lidar IP: {lidar_ip}\n" + f" Local IPs found: {', '.join(local_ips) or '(none)'}\n" + f" No local IP found on the same subnet as lidar ({lidar_ip}).\n" + f" The lidar network interface may be down or unconfigured.\n" + f" → Check: ip addr | grep {subnet_prefix}\n" + f" → Or assign an IP: " + f"sudo ip addr add {subnet_prefix}.5/24 dev \n" + ) + _logger.error(msg) + raise RuntimeError(msg) + + # Check if we can bind a UDP socket on host_ip (port 0 = ephemeral). + try: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.bind((host_ip, 0)) + except OSError as e: + _logger.error( + f"PointLio: Cannot bind UDP socket on host_ip={host_ip!r}: {e}\n" + f" Another process may be using the Livox SDK ports.\n" + f" → Check: ss -ulnp | grep {host_ip}" + ) + raise RuntimeError( + f"PointLio: Cannot bind UDP on {host_ip}: {e}. " + f"Check if another Livox/PointLio process is running." + ) from e + + _logger.info( + "PointLio network check passed", + host_ip=host_ip, + lidar_ip=lidar_ip, + ) + + +# Verify protocol port compliance (mypy will flag missing ports) +if TYPE_CHECKING: + PointLio() diff --git a/dimos/hardware/sensors/lidar/pointlio/setup_network.py b/dimos/hardware/sensors/lidar/pointlio/setup_network.py new file mode 120000 index 0000000000..60620716ac --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/setup_network.py @@ -0,0 +1 @@ +/Users/jeffhykin/repos/dimos6/.ignore.enhance/items/dimos/hardware/sensors/lidar/fastlio2/setup_network.py \ No newline at end of file From 2b31c914a3fc138f74e7161da3c828685e195f3d Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 3 Jun 2026 15:53:16 -0700 Subject: [PATCH 018/185] pointlio native: define ROOT_DIR for the laserMapping debug paths --- dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt index 5b643f74c6..1c5aed391c 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt @@ -4,6 +4,7 @@ project(pointlio_native CXX) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") +add_definitions(-DROOT_DIR="/tmp/pointlio_") if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT) set(CMAKE_INSTALL_PREFIX "${CMAKE_SOURCE_DIR}/result" CACHE PATH "" FORCE) From 0500790dc87afc3062ffbbae8352fba6ac705366 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 3 Jun 2026 15:56:46 -0700 Subject: [PATCH 019/185] pointlio native: lock fast-lio input to built pointlio rev (pointlio_native builds) --- .../sensors/lidar/pointlio/cpp/flake.lock | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock index ed10ba8629..7fb0a1ed0e 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock @@ -37,18 +37,18 @@ "fast-lio": { "flake": false, "locked": { - "lastModified": 1778784133, - "narHash": "sha256-bZEmIJlrzcqrHLxCiXXXf7fgi2yjVRuad8z08HN1eMU=", - "owner": "dimensionalOS", - "repo": "dimos-module-fastlio2", - "rev": "d93bc6795babe9e63cc77d2bf2b72294d9afa839", - "type": "github" + "lastModified": 1780527354, + "narHash": "sha256-2itM7Xqd3XJIr82gvRzmosthzA8qLtWXl9vyXGFObqA=", + "ref": "pointlio", + "rev": "fc979456837be26dd4d4e8fe0a53a7de3f9baf33", + "revCount": 66, + "type": "git", + "url": "file:///Users/jeffhykin/repos/dimos-module-fastlio2" }, "original": { - "owner": "dimensionalOS", - "ref": "v0.3.0-quiet-logs", - "repo": "dimos-module-fastlio2", - "type": "github" + "ref": "pointlio", + "type": "git", + "url": "file:///Users/jeffhykin/repos/dimos-module-fastlio2" } }, "flake-utils": { From e0e1ab47e8b2689d5b857624a6f74251a25013e7 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 3 Jun 2026 16:51:06 -0700 Subject: [PATCH 020/185] clean --- .../sensors/camera/realsense/camera.py | 132 ++++++++++++++---- 1 file changed, 104 insertions(+), 28 deletions(-) diff --git a/dimos/hardware/sensors/camera/realsense/camera.py b/dimos/hardware/sensors/camera/realsense/camera.py index 6257720608..69f9de0b5a 100644 --- a/dimos/hardware/sensors/camera/realsense/camera.py +++ b/dimos/hardware/sensors/camera/realsense/camera.py @@ -14,7 +14,6 @@ from __future__ import annotations -import atexit import threading import time from typing import TYPE_CHECKING @@ -26,11 +25,11 @@ from scipy.spatial.transform import Rotation from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT +from dimos.core.coordination.blueprints import autoconnect from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import Out -from dimos.core.transport import LCMTransport from dimos.hardware.sensors.camera.spec import ( OPTICAL_ROTATION, DepthCameraConfig, @@ -41,9 +40,11 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.sensor_msgs.Imu import Imu from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.spec import perception from dimos.utils.reactive import backpressure +from dimos.visualization.vis_module import vis_module if TYPE_CHECKING: import pyrealsense2 as rs # type: ignore[import-not-found,import-untyped] @@ -67,6 +68,7 @@ class RealSenseCameraConfig(ModuleConfig, DepthCameraConfig): align_depth_to_color: bool = True enable_depth: bool = True enable_pointcloud: bool = False + enable_imu: bool = True pointcloud_fps: float = 5.0 camera_info_fps: float = 1.0 serial_number: str | None = None @@ -79,6 +81,7 @@ class RealSenseCamera(DepthCameraHardware, Module, perception.DepthCamera): pointcloud: Out[PointCloud2] camera_info: Out[CameraInfo] depth_camera_info: Out[CameraInfo] + imu: Out[Imu] @property def _camera_link(self) -> str: @@ -100,6 +103,10 @@ def _depth_frame(self) -> str: def _depth_optical_frame(self) -> str: return f"{self.config.camera_name}_depth_optical_frame" + @property + def _imu_frame(self) -> str: + return f"{self.config.camera_name}_imu_optical_frame" + def __init__(self, *args, **kwargs) -> None: # type: ignore[no-untyped-def] super().__init__(*args, **kwargs) self._pipeline: rs.pipeline | None = None @@ -115,6 +122,10 @@ def __init__(self, *args, **kwargs) -> None: # type: ignore[no-untyped-def] self._latest_color_img: Image | None = None self._latest_depth_img: Image | None = None self._pointcloud_lock = threading.Lock() + # IMU state + self._imu_pipeline: rs.pipeline | None = None + self._depth_to_imu_extrinsics: rs.extrinsics | None = None + self._latest_accel: Vector3 | None = None @rpc def start(self) -> None: @@ -176,6 +187,61 @@ def start(self) -> None: ) ) + if self.config.enable_imu: + self._start_imu() + + def _start_imu(self) -> None: + import pyrealsense2 as rs + + imu_pipeline = rs.pipeline() + imu_config = rs.config() + if self.config.serial_number: + imu_config.enable_device(self.config.serial_number) + + try: + imu_config.enable_stream(rs.stream.accel, rs.format.motion_xyz32f) + imu_config.enable_stream(rs.stream.gyro, rs.format.motion_xyz32f) + imu_profile = imu_pipeline.start(imu_config, self._on_imu_frame) + except RuntimeError as error: + print(f"RealSense IMU unavailable, disabling IMU stream: {error}") + return + + self._imu_pipeline = imu_pipeline + + if self._profile is not None and self.config.enable_depth: + depth_stream = self._profile.get_stream(rs.stream.depth) + accel_stream = imu_profile.get_stream(rs.stream.accel) + self._depth_to_imu_extrinsics = depth_stream.get_extrinsics_to(accel_stream) + + def _on_imu_frame(self, frame: rs.frame) -> None: + if frame.is_frameset(): + for sub_frame in frame.as_frameset(): + self._handle_motion_frame(sub_frame) + else: + self._handle_motion_frame(frame) + + def _handle_motion_frame(self, frame: rs.frame) -> None: + import pyrealsense2 as rs + + motion = frame.as_motion_frame() + if not motion: + return + + motion_data = motion.get_motion_data() + stream_type = motion.get_profile().stream_type() + + if stream_type == rs.stream.accel: + self._latest_accel = Vector3(motion_data.x, motion_data.y, motion_data.z) + elif stream_type == rs.stream.gyro and self._latest_accel is not None: + # Gyro drives publishing, paired with the most recent accel sample. + self.imu.publish( + Imu( + angular_velocity=Vector3(motion_data.x, motion_data.y, motion_data.z), + linear_acceleration=self._latest_accel, + frame_id=self._imu_frame, + ) + ) + def _publish_camera_info(self) -> None: ts = time.time() if self._color_camera_info: @@ -381,6 +447,16 @@ def _publish_tf(self, ts: float) -> None: ) transforms.append(color_to_color_optical) + # camera_link -> imu (physical motion-sensor extrinsics) + if self._depth_to_imu_extrinsics is not None: + imu_tf = self._extrinsics_to_transform( + self._depth_to_imu_extrinsics, + self._camera_link, + self._imu_frame, + ts, + ) + transforms.append(imu_tf) + self.tf.publish(*transforms) def _generate_pointcloud(self) -> None: @@ -408,6 +484,13 @@ def _generate_pointcloud(self) -> None: def stop(self) -> None: self._running = False + if self._imu_pipeline: + try: + self._imu_pipeline.stop() + except Exception: + pass # Pipeline might already be stopped + self._imu_pipeline = None + # Stop pipeline first to unblock wait_for_frames() if self._pipeline: try: @@ -426,6 +509,8 @@ def stop(self) -> None: self._profile = None self._align = None self._color_to_depth_extrinsics = None + self._depth_to_imu_extrinsics = None + self._latest_accel = None self._latest_color_img = None self._latest_depth_img = None super().stop() @@ -443,34 +528,25 @@ def get_depth_scale(self) -> float: return self._depth_scale -def main() -> None: - dimos = ModuleCoordinator() - dimos.start() +def _color_camera_info_to_rerun(camera_info: CameraInfo) -> object: + # Re-parent the pinhole onto the color image entity + its optical frame so + # the color image renders inside the camera frustum in the 3D view. + return camera_info.to_rerun( + image_topic="world/color_image", + optical_frame="camera_color_optical_frame", + ) - camera = dimos.deploy(RealSenseCamera, enable_pointcloud=True, pointcloud_fps=5.0) - camera.color_image.transport = LCMTransport("/camera/color", Image) - camera.depth_image.transport = LCMTransport("/camera/depth", Image) - camera.pointcloud.transport = LCMTransport("/camera/pointcloud", PointCloud2) - camera.camera_info.transport = LCMTransport("/camera/color_info", CameraInfo) - camera.depth_camera_info.transport = LCMTransport("/camera/depth_info", CameraInfo) - def cleanup() -> None: - try: - dimos.stop() - except Exception: - pass - - atexit.register(cleanup) - dimos.start_all_modules() - - try: - while True: - time.sleep(0.1) - except (KeyboardInterrupt, SystemExit): - pass - finally: - atexit.unregister(cleanup) - cleanup() +def main() -> None: + blueprint = autoconnect( + RealSenseCamera.blueprint(enable_pointcloud=True, pointcloud_fps=5.0), + vis_module( + "rerun", + rerun_config={"visual_override": {"world/camera_info": _color_camera_info_to_rerun}}, + ), + ) + + ModuleCoordinator.build(blueprint).loop() if __name__ == "__main__": From aea4372ee7c59ca0c9610ea21515355f6c7d0b19 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 3 Jun 2026 16:57:54 -0700 Subject: [PATCH 021/185] rename --- dimos/hardware/sensors/lidar/fastlio2/recorder.py | 2 +- dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/recorder.py b/dimos/hardware/sensors/lidar/fastlio2/recorder.py index 6a5d32add6..bfbd4edcdb 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/recorder.py +++ b/dimos/hardware/sensors/lidar/fastlio2/recorder.py @@ -64,7 +64,7 @@ def _stop_when_parent_dies(cmd: list[str]) -> list[str]: def _default_recording_dir() -> Path: - return Path("go2_recordings") / _stamp() + return Path("recordings") / _stamp() class FastLio2RecorderConfig(RecorderConfig): diff --git a/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py b/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py index a1c7bade43..49efc4fbed 100755 --- a/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py +++ b/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py @@ -195,7 +195,7 @@ def main(): ) parser.add_argument( "--recordings-dir", - default="./go2_recordings", + default="./recordings", help="root searched when no target is given", ) parser.add_argument("--image-stream", default="color_image") From d069c99710155c82107ff80b05f2044c9bef8625 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 3 Jun 2026 18:29:32 -0700 Subject: [PATCH 022/185] add mid360_realsense --- .../recording/go2_mid360/post_process.py | 57 ++++++ .../recording/go2_mid360/record.py} | 0 .../mid360_realsense/post_process.py | 64 +++++++ .../recording/mid360_realsense/record.py | 178 ++++++++++++++++++ .../recording/utils}/apriltags.py | 11 +- .../recording/utils}/build_rrd.py | 23 ++- .../recording/utils}/camera.py | 0 .../recording/utils}/gtsam_gt.py | 4 +- .../recording/utils}/lidar_reanchor.py | 0 .../recording/utils/post_process.py} | 100 ++++++---- .../recording/utils}/rec_check.py | 18 +- dimos/robot/unitree/go2/config.py | 32 ++++ 12 files changed, 427 insertions(+), 60 deletions(-) create mode 100755 dimos/mapping/recording/go2_mid360/post_process.py rename dimos/{robot/unitree/go2/blueprints/smart/unitree_go2_record.py => mapping/recording/go2_mid360/record.py} (100%) create mode 100644 dimos/mapping/recording/mid360_realsense/post_process.py create mode 100644 dimos/mapping/recording/mid360_realsense/record.py rename dimos/{robot/unitree/go2/recording => mapping/recording/utils}/apriltags.py (96%) rename dimos/{robot/unitree/go2/recording => mapping/recording/utils}/build_rrd.py (96%) rename dimos/{robot/unitree/go2/recording => mapping/recording/utils}/camera.py (100%) rename dimos/{robot/unitree/go2/recording => mapping/recording/utils}/gtsam_gt.py (98%) rename dimos/{robot/unitree/go2/recording => mapping/recording/utils}/lidar_reanchor.py (100%) rename dimos/{robot/unitree/go2/scripts/go2_mid360_post_process.py => mapping/recording/utils/post_process.py} (75%) mode change 100755 => 100644 rename dimos/{robot/unitree/go2/recording => mapping/recording/utils}/rec_check.py (96%) create mode 100644 dimos/robot/unitree/go2/config.py diff --git a/dimos/mapping/recording/go2_mid360/post_process.py b/dimos/mapping/recording/go2_mid360/post_process.py new file mode 100755 index 0000000000..d257eee8d9 --- /dev/null +++ b/dimos/mapping/recording/go2_mid360/post_process.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python +# 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. + +"""Post-process Go2 + Livox recordings: AprilTag-corrected groundtruth + .rrd. + +Thin wrapper over dimos/mapping/recording/utils/post_process.py with the Go2 +front-camera calibration and the lidar/odom pairs present in these recordings. + + uv run --no-sync python \ + dimos/mapping/recording/go2_mid360/post_process.py [TARGET] [--force] + +TARGET may be a `mem2.db`, a recording dir containing one, or a dir to scan for +recordings. With no TARGET it processes the most recently created recording +under --recordings-dir. +""" + +from __future__ import annotations + +from pathlib import Path + +from dimos.mapping.recording.utils.post_process import CameraParams, run +from dimos.robot.unitree.go2.config import ( + GO2_FRONT_CAMERA_DISTORTION, + GO2_FRONT_CAMERA_INTRINSICS, + GO2_FRONT_CAMERA_OPTICAL_IN_BASE, + GO2_FRONT_CAMERA_RESOLUTION, +) + +# Lidar/odom pairs that may be re-anchored onto gtsam_odom — only when their odom +# is the same frame family gtsam was built from. The legacy Go2 onboard +# `lidar`/`odom` is a different estimator frame -> left as-is. +REANCHOR_PAIRS = [("go2_lidar", "go2_odom"), ("fastlio_lidar", "fastlio_odometry")] + + +def load_camera(db: Path) -> CameraParams: + return ( + GO2_FRONT_CAMERA_INTRINSICS, + GO2_FRONT_CAMERA_DISTORTION, + GO2_FRONT_CAMERA_OPTICAL_IN_BASE, + GO2_FRONT_CAMERA_RESOLUTION, + ) + + +if __name__ == "__main__": + run(description=__doc__, reanchor_pairs=REANCHOR_PAIRS, load_camera=load_camera) diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py b/dimos/mapping/recording/go2_mid360/record.py similarity index 100% rename from dimos/robot/unitree/go2/blueprints/smart/unitree_go2_record.py rename to dimos/mapping/recording/go2_mid360/record.py diff --git a/dimos/mapping/recording/mid360_realsense/post_process.py b/dimos/mapping/recording/mid360_realsense/post_process.py new file mode 100644 index 0000000000..6c47d73612 --- /dev/null +++ b/dimos/mapping/recording/mid360_realsense/post_process.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python +# 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. + +"""Post-process RealSense + Mid-360 recordings: AprilTag-corrected groundtruth + .rrd. + +Thin wrapper over dimos/mapping/recording/utils/post_process.py. Intrinsics come +from the recorded RealSense camera_info; the camera_optical mount is fixed by the +record blueprint (1cm forward + 1cm below the lidar). + + uv run --no-sync python \ + dimos/mapping/recording/mid360_realsense/post_process.py [TARGET] [--force] + +TARGET may be a `mem2.db`, a recording dir containing one, or a dir to scan for +recordings. With no TARGET it processes the most recently created recording +under --recordings-dir. +""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np + +from dimos.mapping.recording.utils.post_process import CameraParams, run +from dimos.memory2.store.sqlite import SqliteStore +from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo + +# The RealSense + Mid-360 recording only has the fastlio lidar/odom pair; its odom +# is the same frame family gtsam was built from, so the re-anchor composes. +REANCHOR_PAIRS = [("fastlio_lidar", "fastlio_odometry")] +CAMERA_INFO_STREAM = "realsense_camera_info" + +# camera_optical pose in mid360_link (the frame fastlio odom is anchored to), +# matching the camera mount in record.py: 1cm forward + 1cm below the lidar, +# then the REP-103 optical rotation. +REALSENSE_OPTICAL_IN_BASE = [0.01, 0.0, -0.01, -0.5, 0.5, -0.5, 0.5] + + +def load_camera(db: Path) -> CameraParams: + """Read intrinsics/distortion/resolution from the recorded RealSense + camera_info stream; the optical mount is fixed by the record blueprint.""" + with SqliteStore(path=str(db)) as store: + if CAMERA_INFO_STREAM not in store.list_streams(): + raise SystemExit(f"no '{CAMERA_INFO_STREAM}' stream in {db} — cannot get intrinsics") + info = next(iter(store.stream(CAMERA_INFO_STREAM, CameraInfo))).data + intrinsics = np.array(info.K, dtype=np.float64).reshape(3, 3) + distortion = np.array(info.D, dtype=np.float64) + return intrinsics, distortion, REALSENSE_OPTICAL_IN_BASE, (info.width, info.height) + + +if __name__ == "__main__": + run(description=__doc__, reanchor_pairs=REANCHOR_PAIRS, load_camera=load_camera) diff --git a/dimos/mapping/recording/mid360_realsense/record.py b/dimos/mapping/recording/mid360_realsense/record.py new file mode 100644 index 0000000000..f7ea815f49 --- /dev/null +++ b/dimos/mapping/recording/mid360_realsense/record.py @@ -0,0 +1,178 @@ +#!/usr/bin/env python3 +# Copyright 2025-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. + +import os +import time +from typing import Any + +from reactivex.disposable import Disposable + +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.core.stream import In +from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera +from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 +from dimos.hardware.sensors.lidar.fastlio2.recorder import FastLio2Recorder, _default_recording_dir +from dimos.hardware.sensors.lidar.livox.module import Mid360 +from dimos.memory2.stream import Stream +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.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.Imu import Imu +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.utils.logging_config import set_run_log_dir, setup_logger + +logger = setup_logger() + +# Camera is mounted 1cm forward and 1cm below the lidar. In the mid360_link +# frame (x forward, y left, z up) that is +1cm on x and -1cm on z. +MID360_TO_CAMERA = Transform( + translation=Vector3(0.01, 0.0, -0.01), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + frame_id="mid360_link", + child_frame_id="camera", +) + +# Camera optical frame: standard ROS optical rotation (x-right, y-down, z-forward). +MID360_TO_CAMERA_OPTICAL = MID360_TO_CAMERA + Transform( + translation=Vector3(0.0, 0.0, 0.0), + rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), + frame_id="camera", + child_frame_id="camera_optical", +) + + +_LIDAR_IP = os.getenv("LIDAR_IP", "192.168.1.107") + + +class TfHackRecorder(FastLio2Recorder): + """Records with statically-applied transforms instead of querying tf. + + FastLio2 tracks the Mid-360 (``mid360_link``) and reports its pose in the + ``world`` frame as ``fastlio_odometry``; its registered cloud is likewise + already in that world frame. Recorded observations are anchored from the + latest fastlio odom and the fixed camera mount: + + - ``fastlio_lidar`` / ``fastlio_odometry`` -> ``mid360_link`` pose in world + - ``color_image`` / ``realsense_depth_image`` / ``realsense_pointcloud`` + -> ``camera_optical`` pose in world + - everything else (odom, camera_info, imu) -> no pose + """ + + fastlio_lidar: In[PointCloud2] + fastlio_odometry: In[Odometry] + color_image: In[Image] + realsense_depth_image: In[Image] + realsense_pointcloud: In[PointCloud2] + realsense_camera_info: In[CameraInfo] + realsense_depth_camera_info: In[CameraInfo] + realsense_imu: In[Imu] + livox_lidar: In[PointCloud2] + livox_imu: In[Imu] + # Shadow the parent's generic companion ports so they're not recorded as + # empty `lidar`/`odom` streams; the prefixed ports above take their place. + lidar: None = None # type: ignore[assignment] + odom: None = None # type: ignore[assignment] + + _latest_fastlio_odom: Odometry | None = None + _warning_names: set[str] = set() + + def _port_to_stream(self, name: str, input_topic: In[Any], stream: Stream[Any]) -> None: + def on_msg(msg: Any) -> None: + ts = time.time() + pose = None + if name == "fastlio_odometry": + self._latest_fastlio_odom = msg + world_to_mid360 = self._world_to_mid360_from_fastlio() + if world_to_mid360 is not None: + pose = world_to_mid360.to_pose() + elif name == "fastlio_lidar": + world_to_mid360 = self._world_to_mid360_from_fastlio() + if world_to_mid360 is not None: + pose = world_to_mid360.to_pose() + elif name in ("color_image", "realsense_depth_image", "realsense_pointcloud"): + world_to_mid360 = self._world_to_mid360_from_fastlio() + if world_to_mid360 is not None: + pose = (world_to_mid360 + MID360_TO_CAMERA_OPTICAL).to_pose() + elif "odom" in name or "camera_info" in name or "imu" in name: + pass + else: + if name not in self._warning_names: + self._warning_names.add(name) + logger.warning(f"cannot compute pose for {name}; recording without pose") + + stream.append(msg, ts=ts, pose=pose) + + self.register_disposable(Disposable(input_topic.subscribe(on_msg))) + + def _world_to_mid360_from_fastlio(self) -> Transform | None: + odom = self._latest_fastlio_odom + if odom is None: + return None + return Transform( + translation=odom.position, + rotation=odom.orientation, + frame_id="world", + child_frame_id="mid360_link", + ts=odom.ts, + ) + + +realsense_mid360_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"), + (RealSenseCamera, "imu", "realsense_imu"), + ] + ), + Mid360.blueprint( + lidar_ip=_LIDAR_IP, + ).remappings( + [ + (Mid360, "lidar", "livox_lidar"), + (Mid360, "imu", "livox_imu"), + ] + ), + FastLio2.blueprint( + frame_id="world", + map_freq=-1, + lidar_ip=_LIDAR_IP, + ).remappings( + [ + (FastLio2, "lidar", "fastlio_lidar"), + (FastLio2, "odometry", "fastlio_odometry"), + ] + ), + TfHackRecorder.blueprint(lidar_ip=_LIDAR_IP, record_pcap=True), +).global_config(n_workers=6) + + +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( + realsense_mid360_record, + {TfHackRecorder.name: {"recording_dir": recording_dir}}, + ) + coordinator.loop() diff --git a/dimos/robot/unitree/go2/recording/apriltags.py b/dimos/mapping/recording/utils/apriltags.py similarity index 96% rename from dimos/robot/unitree/go2/recording/apriltags.py rename to dimos/mapping/recording/utils/apriltags.py index 79f92987cd..306de707f8 100644 --- a/dimos/robot/unitree/go2/recording/apriltags.py +++ b/dimos/mapping/recording/utils/apriltags.py @@ -32,7 +32,6 @@ from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.Image import Image -from dimos.robot.unitree.go2.recording.camera import CAMERA_DISTORTION, CAMERA_INTRINSICS def make_detector(dictionary_name: str): @@ -45,15 +44,15 @@ def _object_points(marker_length_m: float) -> np.ndarray: return np.array([[-h, h, 0.0], [h, h, 0.0], [h, -h, 0.0], [-h, -h, 0.0]], dtype=np.float32) -def estimate_marker_pose(corners_pixels, marker_length_m): +def estimate_marker_pose(corners_pixels, marker_length_m, intrinsics, distortion): """solvePnP a single tag -> (rotation_vector, translation_vector) in the camera_optical frame, or None if it failed.""" image_corners = corners_pixels.reshape(4, 1, 2).astype(np.float32) found, rotation_vector, translation_vector = cv2.solvePnP( _object_points(marker_length_m), image_corners, - CAMERA_INTRINSICS, - CAMERA_DISTORTION, + intrinsics, + distortion, flags=cv2.SOLVEPNP_IPPE_SQUARE, ) return (rotation_vector, translation_vector) if found else None @@ -117,6 +116,8 @@ def cluster_medoid(cluster: list[dict], rotation_weight_m_per_rad: float) -> dic def detect_apriltags( store, + intrinsics, + distortion, image_stream="color_image", stream_name="april_tags", marker_length=0.10, @@ -140,7 +141,7 @@ def detect_apriltags( if marker_ids is None: continue for corners, marker_id in zip(all_corners, marker_ids.flatten(), strict=False): - pose = estimate_marker_pose(corners, marker_length) + pose = estimate_marker_pose(corners, marker_length, intrinsics, distortion) if pose is None: continue rotation_vector, translation_vector = pose diff --git a/dimos/robot/unitree/go2/recording/build_rrd.py b/dimos/mapping/recording/utils/build_rrd.py similarity index 96% rename from dimos/robot/unitree/go2/recording/build_rrd.py rename to dimos/mapping/recording/utils/build_rrd.py index 4cccd5f9df..ddccfdce33 100644 --- a/dimos/robot/unitree/go2/recording/build_rrd.py +++ b/dimos/mapping/recording/utils/build_rrd.py @@ -41,7 +41,6 @@ from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.robot.unitree.go2.recording.camera import CAMERA_INTRINSICS TIMELINE = "ts" # one distinct base color per point cloud (height modulates brightness within each) @@ -74,9 +73,6 @@ def _mat7(p) -> np.ndarray: _M_FC_MID360 = _mat([-0.032, 0.0, 0.12], [0.0, math.sin(_PITCH_HALF), 0.0, math.cos(_PITCH_HALF)]) _M_FC_OPTICAL = _mat([0.0, 0.0, 0.0], [-0.5, 0.5, -0.5, 0.5]) # REP-103 optical in camera body MID360_TO_OPTICAL = np.linalg.inv(_M_FC_MID360) @ _M_FC_OPTICAL # legacy: optical pts -> mid360 pts -# default (correct-transform data): odom frame already aligned with the camera body, -# so just the camera mount offset + optical convention, no mid360 pitch hack. -BASE_TO_OPTICAL = _mat([0.32715, 0.0, 0.04297], [-0.5, 0.5, -0.5, 0.5]) def _pose7(p): @@ -212,7 +208,7 @@ def _has_rows(conn, stream): return False -def _log_apriltags(store, db_path, cam_xform, max_views_per_tag=40): +def _log_apriltags(store, db_path, cam_xform, intrinsics, resolution, max_views_per_tag=40): """Place every AprilTag recognition in 3D via the corrected trajectory: T_world_tag = T_world_base(t) . T_base_optical . T_cam_tag. Per marker logs the detection cloud + a labeled marker, and for a sample of recognitions: @@ -294,10 +290,10 @@ def optical_in_world(timestamp): f"({len(positions)} detections, {len(sampled)} views, via {traj_stream})" ) - _log_cam_frustums(store, camera_targets) + _log_cam_frustums(store, camera_targets, intrinsics, resolution) -def _log_cam_frustums(store, camera_targets): +def _log_cam_frustums(store, camera_targets, intrinsics, resolution): """Place the robot's-eye color image on a pinhole frustum at the camera pose for each (entity, T_world_optical, ts) target — rendered only in 3D.""" if not camera_targets or "color_image" not in store.list_streams(): @@ -322,8 +318,8 @@ def _log_cam_frustums(store, camera_targets): rr.log( entity, rr.Pinhole( - image_from_camera=CAMERA_INTRINSICS, - resolution=[1280, 720], + image_from_camera=intrinsics, + resolution=list(resolution), camera_xyz=rr.ViewCoordinates.RDF, image_plane_distance=0.6, ), @@ -390,15 +386,18 @@ def _log_jsonl(jsonl_path: Path) -> None: def build_rrd( db_path: str, out_path: str, + intrinsics, + optical_in_base, + resolution, *, map_voxel: float = 0.1, cloud_stride: int = 3, camera_stride: int = 30, mid360_pitch: bool = False, ): - rr.init("go2_post_process", recording_id=str(out_path)) + rr.init("recording_post_process", recording_id=str(out_path)) rr.save(str(out_path)) - cam_xform = MID360_TO_OPTICAL if mid360_pitch else BASE_TO_OPTICAL + cam_xform = MID360_TO_OPTICAL if mid360_pitch else _mat7(optical_in_base) jsonl_path = _find_jsonl(db_path) with SqliteStore(path=db_path) as store: @@ -462,7 +461,7 @@ def build_rrd( _log_frames(store, stream, f"world/{ent}_lidar", cloud_stride, map_voxel, base) _log_map(store, stream, f"world/{ent}_map", map_voxel, base) - _log_apriltags(store, db_path, cam_xform) + _log_apriltags(store, db_path, cam_xform, intrinsics, resolution) for stream, entity, base in [ ("gtsam_odom", "world/gtsam_path", (0, 220, 0)), # corrected GT -> green diff --git a/dimos/robot/unitree/go2/recording/camera.py b/dimos/mapping/recording/utils/camera.py similarity index 100% rename from dimos/robot/unitree/go2/recording/camera.py rename to dimos/mapping/recording/utils/camera.py diff --git a/dimos/robot/unitree/go2/recording/gtsam_gt.py b/dimos/mapping/recording/utils/gtsam_gt.py similarity index 98% rename from dimos/robot/unitree/go2/recording/gtsam_gt.py rename to dimos/mapping/recording/utils/gtsam_gt.py index 97b0c03423..1de1b5eb77 100644 --- a/dimos/robot/unitree/go2/recording/gtsam_gt.py +++ b/dimos/mapping/recording/utils/gtsam_gt.py @@ -28,7 +28,6 @@ import numpy as np from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.robot.unitree.go2.recording.camera import CAMERA_OPTICAL_IN_BASE def _pose_from7(pose7): @@ -78,6 +77,7 @@ def pick_pose_stream(connection) -> str: def build_gtsam_gt( db_path, markers, + optical_in_base, *, node_stride=3, odom_rot_sig=0.004, @@ -107,7 +107,7 @@ def build_gtsam_gt( f"{len(markers)} tag obs" ) - base_to_optical = _pose_from7(CAMERA_OPTICAL_IN_BASE) + base_to_optical = _pose_from7(optical_in_base) def nearest_node(timestamp): node_index = int(np.searchsorted(node_timestamps, timestamp)) diff --git a/dimos/robot/unitree/go2/recording/lidar_reanchor.py b/dimos/mapping/recording/utils/lidar_reanchor.py similarity index 100% rename from dimos/robot/unitree/go2/recording/lidar_reanchor.py rename to dimos/mapping/recording/utils/lidar_reanchor.py diff --git a/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py b/dimos/mapping/recording/utils/post_process.py old mode 100755 new mode 100644 similarity index 75% rename from dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py rename to dimos/mapping/recording/utils/post_process.py index 49efc4fbed..2657051dd0 --- a/dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py +++ b/dimos/mapping/recording/utils/post_process.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python # Copyright 2026 Dimensional Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -13,47 +12,43 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Post-process Go2 + Livox recordings: add AprilTag-corrected groundtruth + .rrd. +"""Shared post-process runner for recording rigs (robot-agnostic). -Thin orchestrator over dimos/robot/unitree/go2/recording/*. For every `mem2.db` -under a recordings directory it: +For every `mem2.db` under a recordings directory it: 1. prints a recording sanity check (rec_check), - 2. detects AprilTags -> `april_tags` stream (recording.apriltags), - 3. solves a drift-corrected trajectory -> `gtsam_odom` (recording.gtsam_gt), - 4. re-anchors the lidar onto it -> `_corrected` (recording.lidar_reanchor), - 5. writes a Rerun `.rrd` visualization (recording.build_rrd). + 2. detects AprilTags -> `april_tags` stream (apriltags), + 3. solves a drift-corrected trajectory -> `gtsam_odom` (gtsam_gt), + 4. re-anchors each lidar onto it -> `_corrected` (lidar_reanchor), + 5. writes a Rerun `.rrd` visualization (build_rrd). A tag seen at several times pins the odometry chain and removes accumulated drift. Also writes `gtsam_odom.tum` next to each db (relocalization groundtruth). -Run in a python env with dimos + cv2 + gtsam + scipy (from the dimos repo): - - uv run --no-sync python \ - dimos/robot/unitree/go2/scripts/go2_mid360_post_process.py [TARGET] [--force] - -TARGET may be a `mem2.db`, a recording dir containing one, or a dir to scan for -recordings. With no TARGET it processes the most recently created recording -under --recordings-dir. +Each rig calls `run()` with its own re-anchor pairs and a `load_camera(db)` +returning `(intrinsics, distortion, optical_in_base, resolution)`. """ from __future__ import annotations import argparse +from collections.abc import Callable from pathlib import Path +import numpy as np + +from dimos.mapping.recording.utils import rec_check +from dimos.mapping.recording.utils.apriltags import detect_apriltags +from dimos.mapping.recording.utils.build_rrd import build_rrd +from dimos.mapping.recording.utils.gtsam_gt import build_gtsam_gt, write_gtsam_odom +from dimos.mapping.recording.utils.lidar_reanchor import reanchor_stream from dimos.memory2.store.sqlite import SqliteStore -from dimos.robot.unitree.go2.recording import rec_check -from dimos.robot.unitree.go2.recording.apriltags import detect_apriltags -from dimos.robot.unitree.go2.recording.build_rrd import build_rrd -from dimos.robot.unitree.go2.recording.gtsam_gt import build_gtsam_gt, write_gtsam_odom -from dimos.robot.unitree.go2.recording.lidar_reanchor import reanchor_stream -# Lidar/odom pairs that may be re-anchored onto gtsam_odom — only when their odom -# is the same frame family gtsam was built from (so the re-anchor composes). The -# legacy Go2 onboard `lidar`/`odom` is a different estimator frame -> left as-is. -REANCHOR_PAIRS = [("go2_lidar", "go2_odom"), ("fastlio_lidar", "fastlio_odometry")] DB_NAME = "mem2.db" +# (intrinsics 3x3, distortion, optical_in_base [x,y,z,qx,qy,qz,qw], (width, height)) +CameraParams = tuple[np.ndarray, np.ndarray, list[float], tuple[int, int]] +ReanchorPairs = list[tuple[str, str]] + def _created_time(path: Path) -> float: """File creation time (st_birthtime on macOS/BSD; falls back to mtime).""" @@ -91,21 +86,33 @@ def resolve_databases(target: str | None, recordings_dir: str) -> list[Path]: return [most_recent] -def correct_db(db: Path, *, image_stream, apriltag_stream, gtsam_stream, marker_length, dictionary): +def correct_db( + db: Path, + *, + intrinsics, + distortion, + optical_in_base, + reanchor_pairs: ReanchorPairs, + image_stream, + apriltag_stream, + gtsam_stream, + marker_length, + dictionary, +): """AprilTag detection -> GTSAM trajectory -> re-anchor lidar. Returns True if a corrected trajectory was written.""" with SqliteStore(path=str(db)) as store: detections = detect_apriltags( - store, image_stream, apriltag_stream, marker_length, dictionary + store, intrinsics, distortion, image_stream, apriltag_stream, marker_length, dictionary ) if not detections: print(" no AprilTags detected — skipping gtsam_odom (no landmark constraints)") return False - trajectory = build_gtsam_gt(str(db), detections) + trajectory = build_gtsam_gt(str(db), detections, optical_in_base) with SqliteStore(path=str(db)) as store: write_gtsam_odom(store, trajectory, gtsam_stream, db.parent / "gtsam_odom.tum") stream_names = store.list_streams() - for lidar_stream, odom_stream in REANCHOR_PAIRS: + for lidar_stream, odom_stream in reanchor_pairs: if lidar_stream in stream_names and odom_stream in stream_names: try: reanchor_stream( @@ -124,6 +131,11 @@ def correct_db(db: Path, *, image_stream, apriltag_stream, gtsam_stream, marker_ def process_db( db: Path, *, + intrinsics, + distortion, + optical_in_base, + resolution, + reanchor_pairs: ReanchorPairs, image_stream, apriltag_stream, gtsam_stream, @@ -161,6 +173,10 @@ def process_db( else: correct_db( db, + intrinsics=intrinsics, + distortion=distortion, + optical_in_base=optical_in_base, + reanchor_pairs=reanchor_pairs, image_stream=image_stream, apriltag_stream=apriltag_stream, gtsam_stream=gtsam_stream, @@ -173,6 +189,9 @@ def process_db( build_rrd( str(db), str(db.parent / f"{db.parent.name}.rrd"), + intrinsics, + optical_in_base, + resolution, camera_stride=camera_freq, map_voxel=map_voxel, cloud_stride=cloud_stride, @@ -182,9 +201,16 @@ def process_db( print(f" rrd failed: {error}") -def main(): +def run( + *, + description: str | None, + reanchor_pairs: ReanchorPairs, + load_camera: Callable[[Path], CameraParams], +) -> None: + """Parse CLI args and post-process each resolved recording. `load_camera` + supplies the rig's `(intrinsics, distortion, optical_in_base, resolution)`.""" parser = argparse.ArgumentParser( - description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + description=description, formatter_class=argparse.RawDescriptionHelpFormatter ) parser.add_argument( "target", @@ -237,7 +263,7 @@ def main(): "--mid360-pitch", action="store_true", help="apply the legacy mid360->camera 44deg pitch correction (old fastlio " - "recordings; new go2_* data has correct transforms, leave off)", + "recordings; new data stores correct transforms, leave off)", ) args = parser.parse_args() @@ -245,8 +271,14 @@ def main(): print(f"found {len(databases)} recording(s)") for db in databases: try: + intrinsics, distortion, optical_in_base, resolution = load_camera(db) process_db( db, + intrinsics=intrinsics, + distortion=distortion, + optical_in_base=optical_in_base, + resolution=resolution, + reanchor_pairs=reanchor_pairs, image_stream=args.image_stream, apriltag_stream=args.apriltag_stream, gtsam_stream=args.gtsam_stream, @@ -264,7 +296,3 @@ def main(): except Exception as error: print(f" !! failed: {error}") print("done") - - -if __name__ == "__main__": - main() diff --git a/dimos/robot/unitree/go2/recording/rec_check.py b/dimos/mapping/recording/utils/rec_check.py similarity index 96% rename from dimos/robot/unitree/go2/recording/rec_check.py rename to dimos/mapping/recording/utils/rec_check.py index 638242be17..2ca04b1b1f 100644 --- a/dimos/robot/unitree/go2/recording/rec_check.py +++ b/dimos/mapping/recording/utils/rec_check.py @@ -13,7 +13,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Sanity-check a go2 recording dir: pcap + mem2.db sizes, stream rates, pose travel.""" +"""Sanity-check a recording dir: pcap + mem2.db sizes, stream rates, pose travel.""" from __future__ import annotations @@ -27,15 +27,23 @@ from typing import Any STREAMS = ( + "lidar", + "odom", + "color_image", "livox_imu", "livox_lidar", - "lidar", "fastlio_lidar", "fastlio_odometry", - "odom", - "color_image", + "go2_odom", + "go2_color_image", + "realsense_color_image", + "realsense_depth_image", + "realsense_pointcloud", + "realsense_camera_info", + "realsense_depth_camera_info", + "realsense_imu", ) -RECORDINGS_DIR = Path("go2_recordings") +RECORDINGS_DIR = Path("recordings") # A pcap with only its global header (no packets) is exactly this many bytes. PCAP_HEADER_BYTES = 24 diff --git a/dimos/robot/unitree/go2/config.py b/dimos/robot/unitree/go2/config.py new file mode 100644 index 0000000000..daf9ce5517 --- /dev/null +++ b/dimos/robot/unitree/go2/config.py @@ -0,0 +1,32 @@ +# 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. + +"""Go2 hardware configuration shared across blueprints and tools.""" + +from __future__ import annotations + +import numpy as np + +# Go2 front RGB camera rig (1280x720). Same values as front_camera_720.yaml. +# GO2_FRONT_CAMERA_OPTICAL_IN_BASE is the camera_optical pose in base_link, +# matching the URDF front_camera mount + REP-103 optical convention. +GO2_FRONT_CAMERA_INTRINSICS = np.array( + [[797.47561649, 0.0, 643.53521678], [0.0, 796.48721128, 349.27836053], [0.0, 0.0, 1.0]], + dtype=np.float64, +) +GO2_FRONT_CAMERA_DISTORTION = np.array( + [-0.07309429, -0.02341141, -0.00693059, 0.00923868], dtype=np.float64 +) +GO2_FRONT_CAMERA_OPTICAL_IN_BASE = [0.32715, 0.0, 0.04297, -0.5, 0.5, -0.5, 0.5] +GO2_FRONT_CAMERA_RESOLUTION = (1280, 720) From d30c0455c010634686bcbb11e5173186f5a57007 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 3 Jun 2026 20:15:05 -0700 Subject: [PATCH 023/185] new recording system --- dimos/mapping/recording/go2_mid360/record.py | 36 +-- .../recording/go2_mid360/static_transforms.py | 173 ++++++++++++ .../recording/mid360_realsense/record.py | 39 +-- .../mid360_realsense/static_transforms.py | 256 ++++++++++++++++++ 4 files changed, 453 insertions(+), 51 deletions(-) create mode 100644 dimos/mapping/recording/go2_mid360/static_transforms.py create mode 100644 dimos/mapping/recording/mid360_realsense/static_transforms.py diff --git a/dimos/mapping/recording/go2_mid360/record.py b/dimos/mapping/recording/go2_mid360/record.py index ab384c7e85..84ebf84076 100644 --- a/dimos/mapping/recording/go2_mid360/record.py +++ b/dimos/mapping/recording/go2_mid360/record.py @@ -13,7 +13,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import math import os import time from typing import Any @@ -28,11 +27,13 @@ from dimos.hardware.sensors.lidar.fastlio2.recorder import FastLio2Recorder, _default_recording_dir from dimos.hardware.sensors.lidar.fastlio2.speed_warner import SpeedWarner from dimos.hardware.sensors.lidar.livox.module import Mid360 +from dimos.mapping.recording.go2_mid360.static_transforms import ( + BASE_TO_CAMERA_OPTICAL, + MID360_TO_BASE, +) from dimos.memory2.stream import Stream from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -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.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.Imu import Imu @@ -44,35 +45,6 @@ logger = setup_logger() -# mid360_link is physically measured relative to camera (easier than measuring to base_link): -# relative to camera the lidar is 3.2cm back, 12cm up, pitched 44deg down. -# base_link -> front_camera -> mid360_link -BASE_TO_FRONT_CAMERA = Transform( - translation=Vector3(0.32715, -0.00003, 0.04297), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", - child_frame_id="front_camera", -) - -_MID360_PITCH_HALF = math.radians(44.0) / 2.0 -BASE_TO_MID360 = BASE_TO_FRONT_CAMERA + Transform( - translation=Vector3(-0.032, 0.0, 0.12), - rotation=Quaternion(0.0, math.sin(_MID360_PITCH_HALF), 0.0, math.cos(_MID360_PITCH_HALF)), - frame_id="front_camera", - child_frame_id="mid360_link", -) -MID360_TO_BASE = BASE_TO_MID360.inverse() - -# base_link -> camera_optical using the URDF front_camera mount plus the -# standard ROS optical-frame rotation (x-right, y-down, z-forward). -BASE_TO_CAMERA_OPTICAL = BASE_TO_FRONT_CAMERA + Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), - frame_id="front_camera", - child_frame_id="camera_optical", -) - - _LIDAR_IP = os.getenv("LIDAR_IP", "192.168.1.107") diff --git a/dimos/mapping/recording/go2_mid360/static_transforms.py b/dimos/mapping/recording/go2_mid360/static_transforms.py new file mode 100644 index 0000000000..dcd263a408 --- /dev/null +++ b/dimos/mapping/recording/go2_mid360/static_transforms.py @@ -0,0 +1,173 @@ +"""Static transforms between the Go2 body, its front camera, and the Mid-360 odom frame. + +Computes the rigid mounts used while recording (Mid-360 IMU -> base_link, base_link -> +camera optical) and renders every frame in Rerun as XYZ basis arrows plus a simple box +for the robot body (self-contained: no mesh/URDF files needed). + +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). + +FAST-LIO odometry is the Mid-360 IMU ("body") frame +--------------------------------------------------- +hku-mars/FAST_LIO runs its EKF on the IMU state and publishes child_frame_id="body" (the +IMU pose in the gravity-aligned world frame), so the odom FAST-LIO reports is the Mid-360 +frame tracked here as `mid360_link`. +- https://github.com/hku-mars/FAST_LIO/blob/main/src/laserMapping.cpp +""" + +import argparse +import math +from pathlib import Path + +import numpy as np + +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.geometry_msgs.Vector3 import Vector3 + +ROOT = "go2" + +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) + +AXIS_COLORS = [[255, 0, 0], [0, 255, 0], [0, 0, 255]] # X red, Y green, Z blue + +# Approximate Go2 trunk box (~65 x 31 x 15 cm), centered on base_link. The front face lands +# at the front_camera mount (x ~= 0.327). Adjust if you want the full leg-span bounding box. +GO2_BODY_HALF_SIZES = (0.325, 0.155, 0.075) +GO2_BODY_CENTER = (0.0, 0.0, 0.0) +GO2_BODY_COLOR = [120, 120, 130] + +# (name, parent_name, translation_xyz, rpy) — parent None means attached to ROOT +FRAMES: list[tuple[str, str | None, tuple[float, float, float], tuple[float, float, float]]] = [ + ("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), +] + +PARENT_OF: dict[str, str | None] = {name: parent for name, parent, _, _ in FRAMES} +EDGE_OF: dict[str, tuple[tuple[float, float, float], tuple[float, float, float]]] = { + name: (translation, rpy) for name, _parent, translation, rpy in FRAMES +} + + +def rpy_to_matrix(roll: float, pitch: float, yaw: float) -> np.ndarray: + """URDF fixed-axis rpy -> rotation matrix (Rz @ Ry @ Rx).""" + cos_r, sin_r = math.cos(roll), math.sin(roll) + cos_p, sin_p = math.cos(pitch), math.sin(pitch) + cos_y, sin_y = math.cos(yaw), math.sin(yaw) + rot_x = np.array([[1, 0, 0], [0, cos_r, -sin_r], [0, sin_r, cos_r]]) + rot_y = np.array([[cos_p, 0, sin_p], [0, 1, 0], [-sin_p, 0, cos_p]]) + rot_z = np.array([[cos_y, -sin_y, 0], [sin_y, cos_y, 0], [0, 0, 1]]) + return rot_z @ rot_y @ rot_x + + +def pose_relative_to_root(name: str) -> Transform: + """Compose the frame edges from ROOT down to ``name`` (root -> name).""" + chain: list[str] = [] + cursor: str | None = name + while cursor is not None: + chain.append(cursor) + cursor = PARENT_OF[cursor] + pose: Transform | None = None + for frame_name in reversed(chain): + translation, rpy = EDGE_OF[frame_name] + edge = Transform( + translation=Vector3(*translation), rotation=Quaternion.from_euler(Vector3(*rpy)) + ) + pose = edge if pose is None else pose + edge + assert pose is not None + return pose + + +def transform_between(source: str, target: str, frame_id: str, child_frame_id: str) -> Transform: + """Static transform source -> target (frame_id=source, child_frame_id=target).""" + result = pose_relative_to_root(source).inverse() + pose_relative_to_root(target) + result.frame_id = frame_id + result.child_frame_id = child_frame_id + return result + + +BASE_TO_FRONT_CAMERA = transform_between("base_link", "front_camera", "base_link", "front_camera") +BASE_TO_MID360 = transform_between("base_link", "mid360_link", "base_link", "mid360_link") +# Mid-360 IMU (FAST-LIO odom) frame -> robot base. Compose with world->mid360 from odom to +# anchor recorded observations to base_link. +MID360_TO_BASE = transform_between("mid360_link", "base_link", "mid360_link", "base_link") +# base_link -> camera optical frame (x-right, y-down, z-forward) for anchoring images. +BASE_TO_CAMERA_OPTICAL = transform_between( + "base_link", "camera_optical", "base_link", "camera_optical" +) + + +def entity_path(name: str) -> str: + chain = [name] + cursor = PARENT_OF[name] + while cursor is not None: + chain.append(cursor) + cursor = PARENT_OF[cursor] + chain.append(ROOT) + return "/".join(reversed(chain)) + + +def render(axis_length: float, rrd_path: Path | None) -> None: + import rerun as rr + + rr.init("go2_mid360_frames", spawn=rrd_path is None) + if rrd_path is not None: + rr.save(str(rrd_path)) + + rr.log(ROOT, rr.ViewCoordinates.RIGHT_HAND_Z_UP, static=True) + + paths = {name: entity_path(name) for name in PARENT_OF} + + axes = np.eye(3) * axis_length + for name, _parent, translation, rpy in FRAMES: + rr.log( + paths[name], + rr.Transform3D(translation=list(translation), mat3x3=rpy_to_matrix(*rpy)), + rr.Arrows3D( + vectors=axes, origins=np.zeros((3, 3)), colors=AXIS_COLORS, labels=["X", "Y", "Z"] + ), + ) + rr.log( + f"{paths[name]}/label", + rr.Points3D([[0.0, 0.0, 0.0]], labels=[name], show_labels=True, radii=axis_length / 15), + ) + + rr.log( + f"{paths['base_link']}/body", + rr.Boxes3D( + centers=[list(GO2_BODY_CENTER)], + half_sizes=[list(GO2_BODY_HALF_SIZES)], + colors=[GO2_BODY_COLOR], + ), + ) + + if rrd_path is not None: + print(f"wrote {rrd_path}") + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--axis-length", type=float, default=0.1, help="basis-vector length in meters" + ) + parser.add_argument( + "--rrd", type=Path, default=None, help="save to this .rrd instead of spawning the viewer" + ) + args = parser.parse_args() + + for transform in (BASE_TO_FRONT_CAMERA, BASE_TO_MID360, MID360_TO_BASE, BASE_TO_CAMERA_OPTICAL): + print(transform) + render(args.axis_length, args.rrd) + + +if __name__ == "__main__": + main() diff --git a/dimos/mapping/recording/mid360_realsense/record.py b/dimos/mapping/recording/mid360_realsense/record.py index f7ea815f49..ca077c66c0 100644 --- a/dimos/mapping/recording/mid360_realsense/record.py +++ b/dimos/mapping/recording/mid360_realsense/record.py @@ -27,10 +27,12 @@ from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 from dimos.hardware.sensors.lidar.fastlio2.recorder import FastLio2Recorder, _default_recording_dir from dimos.hardware.sensors.lidar.livox.module import Mid360 +from dimos.mapping.recording.mid360_realsense.static_transforms import ( + MID360_TO_WORLD, + REALSENSE_COLOR_OPTICAL_FRAME_TO_MID360_IMU_FRAME, +) from dimos.memory2.stream import Stream -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.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image @@ -40,22 +42,14 @@ logger = setup_logger() -# Camera is mounted 1cm forward and 1cm below the lidar. In the mid360_link -# frame (x forward, y left, z up) that is +1cm on x and -1cm on z. -MID360_TO_CAMERA = Transform( - translation=Vector3(0.01, 0.0, -0.01), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="mid360_link", - child_frame_id="camera", -) +# FAST-LIO odom is the Mid-360 IMU frame; map it to the RealSense color optical +# frame. See static_transforms.py for the geometry and sources. +MID360_TO_CAMERA_OPTICAL = REALSENSE_COLOR_OPTICAL_FRAME_TO_MID360_IMU_FRAME.inverse() -# Camera optical frame: standard ROS optical rotation (x-right, y-down, z-forward). -MID360_TO_CAMERA_OPTICAL = MID360_TO_CAMERA + Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), - frame_id="camera", - child_frame_id="camera_optical", -) +# FAST-LIO reports the Mid-360 pose in its own start frame. Re-anchor that onto a flat world +# frame whose origin is the camera screw and whose axes are level (the camera is angled up). +# Assumes FAST-LIO initializes at the rig's starting pose. +WORLD_TO_MID360 = MID360_TO_WORLD.inverse() _LIDAR_IP = os.getenv("LIDAR_IP", "192.168.1.107") @@ -110,6 +104,8 @@ def on_msg(msg: Any) -> None: world_to_mid360 = self._world_to_mid360_from_fastlio() if world_to_mid360 is not None: pose = (world_to_mid360 + MID360_TO_CAMERA_OPTICAL).to_pose() + elif name == "go2_odom" or name == "odom": + pose = msg elif "odom" in name or "camera_info" in name or "imu" in name: pass else: @@ -125,13 +121,18 @@ def _world_to_mid360_from_fastlio(self) -> Transform | None: odom = self._latest_fastlio_odom if odom is None: return None - return Transform( + fastlio_pose = Transform( translation=odom.position, rotation=odom.orientation, - frame_id="world", + frame_id="fastlio_world", child_frame_id="mid360_link", ts=odom.ts, ) + world_to_mid360 = WORLD_TO_MID360 + fastlio_pose + world_to_mid360.frame_id = "world" + world_to_mid360.child_frame_id = "mid360_link" + world_to_mid360.ts = odom.ts + return world_to_mid360 realsense_mid360_record = autoconnect( diff --git a/dimos/mapping/recording/mid360_realsense/static_transforms.py b/dimos/mapping/recording/mid360_realsense/static_transforms.py new file mode 100644 index 0000000000..7ab8db1522 --- /dev/null +++ b/dimos/mapping/recording/mid360_realsense/static_transforms.py @@ -0,0 +1,256 @@ +"""Static transforms between the RealSense D435i frames and the Mid-360 odom frame. + +Computes rigid transforms (RealSense color/gyro/depth -> Mid-360 IMU frame) for use when +recording, and renders every frame in Rerun as XYZ basis arrows plus simple boxes for the +camera and lidar bodies (self-contained: no mesh files needed). + +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. +- Livox Mid-360 User Manual (Dimensions + Coordinates): + https://www.livoxtech.com/mid-360/downloads + +FAST-LIO odometry is the IMU ("body") frame, NOT the lidar frame +--------------------------------------------------------------- +hku-mars/FAST_LIO runs its EKF on the IMU state: publish_odometry() sets +child_frame_id = "body" and fills the pose from state_point (the IMU pose in the +gravity-aligned "camera_init" world frame). RGBpointBodyLidarToIMU() transforms lidar +points into the IMU frame via offset_R_L_I / offset_T_L_I, confirming "body" == IMU. +- https://github.com/hku-mars/FAST_LIO/blob/main/src/laserMapping.cpp + +The lidar-to-IMU extrinsic comes from the official Mid-360 config; it matches the local +dimos config (dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml): + extrinsic_T: [ -0.011, -0.02329, 0.04412 ] # lidar origin expressed in IMU frame +Flipping the sign gives the IMU position in lidar coords = (11.0, 23.29, -44.12) mm, +the value Livox publishes for the IMU in the point-cloud coordinate system. The large +y = 23.29 mm term is why the IMU sits near a corner of the body, not the center. +- https://github.com/hku-mars/FAST_LIO/blob/main/config/mid360.yaml +- https://github.com/Livox-SDK/livox_ros_driver2/issues/139 + +So the odom frame FAST-LIO reports is `imu_frame` below, not `lidar_frame`. +""" + +import argparse +import math +from pathlib import Path + +import numpy as np + +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.geometry_msgs.Vector3 import Vector3 + +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 + +ROOT = "d435i" + +# 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) + +AXIS_COLORS = [[255, 0, 0], [0, 255, 0], [0, 0, 255]] # X red, Y green, Z blue + +# D435i body box (90 x 25 x 25 mm), centered on bottom_screw_frame in x/y and lifted in +# z so its bottom face sits on the screw plane (x fwd, y left, z up). +CAMERA_BOX_HALF_SIZES = (0.0125, 0.045, 0.0125) +CAMERA_BOX_CENTER = (0.0, 0.0, CAMERA_BOX_HALF_SIZES[2]) +CAMERA_BOX_COLOR = [80, 160, 230] + +BOX_HALF_SIZES = (0.0325, 0.0325, 0.030) # Mid-360 body: 65 x 65 x 60 mm +BOX_COLOR = [230, 160, 40] + +# 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) + +# (name, parent_name, translation_xyz, rpy) — parent None means attached to ROOT +FRAMES: list[tuple[str, str | None, tuple[float, float, float], tuple[float, float, float]]] = [ + ("bottom_screw_frame", None, (0.0, 0.0, 0.0), (0.0, 0.0, 0.0)), + # Flat world: the screw frame leveled out. The camera points up CAMERA_ANGLE_UP, so + # pitching the screw frame down by that angle gives a gravity-flat frame at the screw. + ("world", "bottom_screw_frame", (0.0, 0.0, 0.0), (0.0, CAMERA_ANGLE_UP, 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)), +] + +PARENT_OF: dict[str, str | None] = {name: parent for name, parent, _, _ in FRAMES} +EDGE_OF: dict[str, tuple[tuple[float, float, float], tuple[float, float, float]]] = { + name: (translation, rpy) for name, _parent, translation, rpy in FRAMES +} + + +def rpy_to_matrix(roll: float, pitch: float, yaw: float) -> np.ndarray: + """URDF fixed-axis rpy -> rotation matrix (Rz @ Ry @ Rx).""" + cos_r, sin_r = math.cos(roll), math.sin(roll) + cos_p, sin_p = math.cos(pitch), math.sin(pitch) + cos_y, sin_y = math.cos(yaw), math.sin(yaw) + rot_x = np.array([[1, 0, 0], [0, cos_r, -sin_r], [0, sin_r, cos_r]]) + rot_y = np.array([[cos_p, 0, sin_p], [0, 1, 0], [-sin_p, 0, cos_p]]) + rot_z = np.array([[cos_y, -sin_y, 0], [sin_y, cos_y, 0], [0, 0, 1]]) + return rot_z @ rot_y @ rot_x + + +def pose_relative_to_root(name: str) -> Transform: + """Compose the frame edges from ROOT down to ``name`` (root -> name).""" + chain: list[str] = [] + cursor: str | None = name + while cursor is not None: + chain.append(cursor) + cursor = PARENT_OF[cursor] + pose: Transform | None = None + for frame_name in reversed(chain): + translation, rpy = EDGE_OF[frame_name] + edge = Transform( + translation=Vector3(*translation), rotation=Quaternion.from_euler(Vector3(*rpy)) + ) + pose = edge if pose is None else pose + edge + assert pose is not None + return pose + + +def transform_between(source: str, target: str, frame_id: str, child_frame_id: str) -> Transform: + """Static transform source -> target (frame_id=source, child_frame_id=target).""" + result = pose_relative_to_root(source).inverse() + pose_relative_to_root(target) + result.frame_id = frame_id + result.child_frame_id = child_frame_id + return result + + +# RealSense frame -> Mid-360 IMU (odom) frame. The IMU frame is what FAST-LIO reports as +# odom. Use `.inverse()` for the Mid-360 -> RealSense direction, or swap to the +# *_optical_frame source if you need the image-optical convention. +REALSENSE_COLOR_FRAME_TO_MID360_IMU_FRAME = transform_between( + "color_frame", "imu_frame", "realsense_color_frame", "mid360_imu_frame" +) +REALSENSE_GYRO_FRAME_TO_MID360_IMU_FRAME = transform_between( + "gyro_frame", "imu_frame", "realsense_gyro_frame", "mid360_imu_frame" +) +REALSENSE_DEPTH_FRAME_TO_MID360_IMU_FRAME = transform_between( + "depth_frame", "imu_frame", "realsense_depth_frame", "mid360_imu_frame" +) +# Optical convention (x-right, y-down, z-forward) — what image/pointcloud data uses. +REALSENSE_COLOR_OPTICAL_FRAME_TO_MID360_IMU_FRAME = transform_between( + "color_optical_frame", "imu_frame", "realsense_color_optical_frame", "mid360_imu_frame" +) +# Mid-360 IMU (FAST-LIO odom) frame -> flat world frame at the camera screw. Invert for the +# world -> mid360 direction used to re-anchor FAST-LIO odometry while recording. +MID360_TO_WORLD = transform_between("imu_frame", "world", "mid360_imu_frame", "world") + + +def entity_path(name: str) -> str: + chain = [name] + cursor = PARENT_OF[name] + while cursor is not None: + chain.append(cursor) + cursor = PARENT_OF[cursor] + chain.append(ROOT) + return "/".join(reversed(chain)) + + +def render(axis_length: float, rrd_path: Path | None) -> None: + import rerun as rr + + rr.init("mid360_realsense_frames", spawn=rrd_path is None) + if rrd_path is not None: + rr.save(str(rrd_path)) + + rr.log(ROOT, rr.ViewCoordinates.RIGHT_HAND_Z_UP, static=True) + + paths = {name: entity_path(name) for name in PARENT_OF} + + axes = np.eye(3) * axis_length + for name, _parent, translation, rpy in FRAMES: + rr.log( + paths[name], + rr.Transform3D(translation=list(translation), mat3x3=rpy_to_matrix(*rpy)), + rr.Arrows3D( + vectors=axes, origins=np.zeros((3, 3)), colors=AXIS_COLORS, labels=["X", "Y", "Z"] + ), + ) + rr.log( + f"{paths[name]}/label", + rr.Points3D([[0.0, 0.0, 0.0]], labels=[name], show_labels=True, radii=axis_length / 15), + ) + + rr.log( + f"{paths['bottom_screw_frame']}/box", + rr.Boxes3D( + centers=[list(CAMERA_BOX_CENTER)], + half_sizes=[list(CAMERA_BOX_HALF_SIZES)], + colors=[CAMERA_BOX_COLOR], + ), + ) + rr.log( + f"{paths['box_center']}/box", + rr.Boxes3D( + centers=[[0.0, 0.0, 0.0]], half_sizes=[list(BOX_HALF_SIZES)], colors=[BOX_COLOR] + ), + ) + + if rrd_path is not None: + print(f"wrote {rrd_path}") + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--axis-length", type=float, default=0.015, help="basis-vector length in meters" + ) + parser.add_argument( + "--rrd", type=Path, default=None, help="save to this .rrd instead of spawning the viewer" + ) + args = parser.parse_args() + + for transform in ( + REALSENSE_COLOR_FRAME_TO_MID360_IMU_FRAME, + REALSENSE_GYRO_FRAME_TO_MID360_IMU_FRAME, + REALSENSE_DEPTH_FRAME_TO_MID360_IMU_FRAME, + REALSENSE_COLOR_OPTICAL_FRAME_TO_MID360_IMU_FRAME, + MID360_TO_WORLD, + ): + print(transform) + render(args.axis_length, args.rrd) + + +if __name__ == "__main__": + main() From f41faa88882bb61b0f0620a645bcbee0c51523de Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 3 Jun 2026 20:18:04 -0700 Subject: [PATCH 024/185] formatting --- .../recording/go2_mid360/static_transforms.py | 14 ++++++++++++++ .../recording/mid360_realsense/record.py | 18 +++++++++++------- .../mid360_realsense/static_transforms.py | 14 ++++++++++++++ 3 files changed, 39 insertions(+), 7 deletions(-) diff --git a/dimos/mapping/recording/go2_mid360/static_transforms.py b/dimos/mapping/recording/go2_mid360/static_transforms.py index dcd263a408..6cc315c80e 100644 --- a/dimos/mapping/recording/go2_mid360/static_transforms.py +++ b/dimos/mapping/recording/go2_mid360/static_transforms.py @@ -1,3 +1,17 @@ +# 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 transforms between the Go2 body, its front camera, and the Mid-360 odom frame. Computes the rigid mounts used while recording (Mid-360 IMU -> base_link, base_link -> diff --git a/dimos/mapping/recording/mid360_realsense/record.py b/dimos/mapping/recording/mid360_realsense/record.py index ca077c66c0..613986f766 100644 --- a/dimos/mapping/recording/mid360_realsense/record.py +++ b/dimos/mapping/recording/mid360_realsense/record.py @@ -46,10 +46,16 @@ # frame. See static_transforms.py for the geometry and sources. MID360_TO_CAMERA_OPTICAL = REALSENSE_COLOR_OPTICAL_FRAME_TO_MID360_IMU_FRAME.inverse() -# FAST-LIO reports the Mid-360 pose in its own start frame. Re-anchor that onto a flat world -# frame whose origin is the camera screw and whose axes are level (the camera is angled up). -# Assumes FAST-LIO initializes at the rig's starting pose. -WORLD_TO_MID360 = MID360_TO_WORLD.inverse() +# FAST-LIO inits level (gravity zeroes pitch/roll) with its origin at the Mid-360 IMU, so its +# world frame already shares our flat world's orientation and only differs by the screw->IMU +# lever arm. Re-anchor onto the camera screw with a pure translation -- no rotation, since both +# frames are level and share heading (the rig's screw->IMU offset is pure pitch). Applying the +# full mid360->world rotation here would wrongly tilt the trajectory by the lidar's ~26deg pitch. +WORLD_TO_FASTLIO_WORLD = Transform( + translation=MID360_TO_WORLD.inverse().translation, + frame_id="world", + child_frame_id="fastlio_world", +) _LIDAR_IP = os.getenv("LIDAR_IP", "192.168.1.107") @@ -128,9 +134,7 @@ def _world_to_mid360_from_fastlio(self) -> Transform | None: child_frame_id="mid360_link", ts=odom.ts, ) - world_to_mid360 = WORLD_TO_MID360 + fastlio_pose - world_to_mid360.frame_id = "world" - world_to_mid360.child_frame_id = "mid360_link" + world_to_mid360 = WORLD_TO_FASTLIO_WORLD + fastlio_pose world_to_mid360.ts = odom.ts return world_to_mid360 diff --git a/dimos/mapping/recording/mid360_realsense/static_transforms.py b/dimos/mapping/recording/mid360_realsense/static_transforms.py index 7ab8db1522..b93eb8edd9 100644 --- a/dimos/mapping/recording/mid360_realsense/static_transforms.py +++ b/dimos/mapping/recording/mid360_realsense/static_transforms.py @@ -1,3 +1,17 @@ +# 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 transforms between the RealSense D435i frames and the Mid-360 odom frame. Computes rigid transforms (RealSense color/gyro/depth -> Mid-360 IMU frame) for use when From a14bae88748c4f9b37d7b9959e4e802b13898f46 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 3 Jun 2026 20:53:02 -0700 Subject: [PATCH 025/185] pointlio native: point fast-lio input at github remote (was macOS-only path) The fast-lio input was pinned to file:///Users/jeffhykin/... which only exists on the Mac. Repoint to the dimensionalOS/dimos-module-fastlio2 pointlio branch on github so the flake builds on Linux. Same locked rev. --- dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock | 4 ++-- dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock index 7fb0a1ed0e..e5b0b2989f 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock @@ -43,12 +43,12 @@ "rev": "fc979456837be26dd4d4e8fe0a53a7de3f9baf33", "revCount": 66, "type": "git", - "url": "file:///Users/jeffhykin/repos/dimos-module-fastlio2" + "url": "ssh://git@github.com/dimensionalOS/dimos-module-fastlio2" }, "original": { "ref": "pointlio", "type": "git", - "url": "file:///Users/jeffhykin/repos/dimos-module-fastlio2" + "url": "ssh://git@github.com/dimensionalOS/dimos-module-fastlio2" } }, "flake-utils": { diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix index dcb08e722d..04601d815e 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix @@ -13,7 +13,7 @@ flake = false; }; fast-lio = { - url = "git+file:///Users/jeffhykin/repos/dimos-module-fastlio2?ref=pointlio"; + url = "git+ssh://git@github.com/dimensionalOS/dimos-module-fastlio2?ref=pointlio"; flake = false; }; lcm-extended = { From 8d98e1346b84886d36b85090dd61f4b69805dfb7 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 3 Jun 2026 21:17:34 -0700 Subject: [PATCH 026/185] pointlio native: add mid360-pointlio blueprints Rename the mirrored fastlio_blueprints.py to pointlio_blueprints.py and wire it to PointLio (was incorrectly using FastLio2). Adds mid360-pointlio and mid360-pointlio-voxels to the blueprint registry. --- .../lidar/pointlio/fastlio_blueprints.py | 67 ------------------- .../lidar/pointlio/pointlio_blueprints.py | 40 +++++++++++ dimos/robot/all_blueprints.py | 3 + 3 files changed, 43 insertions(+), 67 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/pointlio/fastlio_blueprints.py create mode 100644 dimos/hardware/sensors/lidar/pointlio/pointlio_blueprints.py diff --git a/dimos/hardware/sensors/lidar/pointlio/fastlio_blueprints.py b/dimos/hardware/sensors/lidar/pointlio/fastlio_blueprints.py deleted file mode 100644 index 89e8ec2c14..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/fastlio_blueprints.py +++ /dev/null @@ -1,67 +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 dimos.core.coordination.blueprints import autoconnect -from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 -from dimos.mapping.ray_tracing.module import RayTracingVoxelMap -from dimos.mapping.voxels import VoxelGridMapper -from dimos.visualization.vis_module import vis_module - -voxel_size = 0.05 - - -mid360_fastlio = autoconnect( - FastLio2.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=-1), - vis_module("rerun"), -).global_config(n_workers=2, robot_model="mid360_fastlio2") - -mid360_fastlio_voxels = autoconnect( - FastLio2.blueprint(), - VoxelGridMapper.blueprint(voxel_size=voxel_size, carve_columns=False), - vis_module( - "rerun", - rerun_config={ - "visual_override": { - "world/lidar": None, - }, - }, - ), -).global_config(n_workers=3, robot_model="mid360_fastlio2_voxels") - -mid360_fastlio_voxels_native = autoconnect( - FastLio2.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=3.0), - vis_module( - "rerun", - rerun_config={ - "visual_override": { - "world/lidar": None, - }, - }, - ), -).global_config(n_workers=2, robot_model="mid360_fastlio2") - - -mid360_fastlio_ray_trace = autoconnect( - FastLio2.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=-1), - RayTracingVoxelMap.blueprint(voxel_size=voxel_size), - vis_module( - "rerun", - rerun_config={ - "visual_override": { - "world/lidar": None, - }, - }, - ), -).global_config(n_workers=5, robot_model="mid360_fastlio2_ray_trace") diff --git a/dimos/hardware/sensors/lidar/pointlio/pointlio_blueprints.py b/dimos/hardware/sensors/lidar/pointlio/pointlio_blueprints.py new file mode 100644 index 0000000000..7a413404f4 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/pointlio_blueprints.py @@ -0,0 +1,40 @@ +# 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 dimos.core.coordination.blueprints import autoconnect +from dimos.hardware.sensors.lidar.pointlio.module import PointLio +from dimos.mapping.voxels import VoxelGridMapper +from dimos.visualization.vis_module import vis_module + +voxel_size = 0.05 + + +mid360_pointlio = autoconnect( + PointLio.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=-1), + vis_module("rerun"), +).global_config(n_workers=2, robot_model="mid360_pointlio") + +mid360_pointlio_voxels = autoconnect( + PointLio.blueprint(), + VoxelGridMapper.blueprint(voxel_size=voxel_size, carve_columns=False), + vis_module( + "rerun", + rerun_config={ + "visual_override": { + "world/lidar": None, + }, + }, + ), +).global_config(n_workers=3, robot_model="mid360_pointlio_voxels") diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index efd0f563ba..2a353f089f 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -69,6 +69,8 @@ "mid360-fastlio-ray-trace": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_ray_trace", "mid360-fastlio-voxels": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels", "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", "openarm-mock-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints:openarm_mock_planner_coordinator", "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints:openarm_planner_coordinator", "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", @@ -199,6 +201,7 @@ "pgo": "dimos.navigation.nav_stack.modules.pgo.pgo.PGO", "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", "quest-teleop-module": "dimos.teleop.quest.quest_teleop_module.QuestTeleopModule", "ray-tracing-voxel-map": "dimos.mapping.ray_tracing.module.RayTracingVoxelMap", "real-sense-camera": "dimos.hardware.sensors.camera.realsense.camera.RealSenseCamera", From dc42455edf67d06d1a346bcc535c9d6dc1eb6d5c Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Thu, 4 Jun 2026 11:05:09 -0700 Subject: [PATCH 027/185] fastlio2: make pcap capture interface configurable via DIMOS_PCAP_IFACE The tcpdump capture interface was hardcoded to enp2s0, which is only correct on one host. Default it from the DIMOS_PCAP_IFACE env var (falling back to enp2s0) so other machines can override without a code change. --- dimos/hardware/sensors/lidar/fastlio2/recorder.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/recorder.py b/dimos/hardware/sensors/lidar/fastlio2/recorder.py index bfbd4edcdb..932e36a73e 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/recorder.py +++ b/dimos/hardware/sensors/lidar/fastlio2/recorder.py @@ -80,7 +80,10 @@ class FastLio2RecorderConfig(RecorderConfig): # enable. pcap_path defaults to /mid360.pcap when unset. record_pcap: bool = False pcap_path: Path | None = None - record_pcap_iface: str = "enp2s0" + # Capture interface for tcpdump. Machine-specific, so it defaults from the + # DIMOS_PCAP_IFACE env var (falling back to enp2s0) to avoid hardcoding a + # value that's only correct on one host. + record_pcap_iface: str = Field(default_factory=lambda: os.environ.get("DIMOS_PCAP_IFACE", "enp2s0")) record_pcap_snaplen: int = 2048 lidar_ip: str = "192.168.1.107" From 1b7c6f75d76573a6f449c4c590c07df41153a468 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Thu, 4 Jun 2026 11:06:22 -0700 Subject: [PATCH 028/185] realsense: fix IMU stream startup and depth->IMU extrinsics Two fixes for the D4xx IMU path: - enable_stream for accel/gyro now passes an explicit 200 Hz rate. Without an fps, librealsense falls back to accel @ 63 Hz, which D4xx firmware doesn't offer, raising 'Couldn't resolve requests'. - The accel profile used for depth->IMU extrinsics is now pulled from the device's own sensor list instead of the separately-started IMU pipeline. Cross-pipeline stream profiles aren't linked in the extrinsics graph, which raised 'Requested extrinsics are not available!'. --- .../hardware/sensors/camera/realsense/camera.py | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/dimos/hardware/sensors/camera/realsense/camera.py b/dimos/hardware/sensors/camera/realsense/camera.py index 69f9de0b5a..2bb9ff3679 100644 --- a/dimos/hardware/sensors/camera/realsense/camera.py +++ b/dimos/hardware/sensors/camera/realsense/camera.py @@ -199,8 +199,10 @@ def _start_imu(self) -> None: imu_config.enable_device(self.config.serial_number) try: - imu_config.enable_stream(rs.stream.accel, rs.format.motion_xyz32f) - imu_config.enable_stream(rs.stream.gyro, rs.format.motion_xyz32f) + # Explicit IMU rates required: without an fps librealsense falls back to + # accel @ 63 Hz, which D4xx firmware doesn't offer -> "Couldn't resolve requests". + imu_config.enable_stream(rs.stream.accel, rs.format.motion_xyz32f, 200) + imu_config.enable_stream(rs.stream.gyro, rs.format.motion_xyz32f, 200) imu_profile = imu_pipeline.start(imu_config, self._on_imu_frame) except RuntimeError as error: print(f"RealSense IMU unavailable, disabling IMU stream: {error}") @@ -210,7 +212,15 @@ def _start_imu(self) -> None: if self._profile is not None and self.config.enable_depth: depth_stream = self._profile.get_stream(rs.stream.depth) - accel_stream = imu_profile.get_stream(rs.stream.accel) + # The accel profile must come from the device's extrinsics graph, not the + # separately-started IMU pipeline -- cross-pipeline profiles aren't linked and + # get_extrinsics_to() raises "Requested extrinsics are not available!". + accel_stream = next( + profile + for sensor in self._profile.get_device().query_sensors() + for profile in sensor.get_stream_profiles() + if profile.stream_type() == rs.stream.accel + ) self._depth_to_imu_extrinsics = depth_stream.get_extrinsics_to(accel_stream) def _on_imu_frame(self, frame: rs.frame) -> None: From 11dc45708de4459cec2ea474d92bba26312c811c Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 4 Jun 2026 12:04:47 -0700 Subject: [PATCH 029/185] feat: add lie-down/stand-up keys to keyboard teleop Bind Z (lie down) and X (stand up) in the pygame teleop via an optional GO2ConnectionSpec ref, so it stays a no-op for non-Go2 blueprints. --- dimos/robot/unitree/go2/connection_spec.py | 2 ++ dimos/robot/unitree/keyboard_teleop.py | 20 ++++++++++++++++++++ 2 files changed, 22 insertions(+) diff --git a/dimos/robot/unitree/go2/connection_spec.py b/dimos/robot/unitree/go2/connection_spec.py index dd6aab9c40..3ea52f9266 100644 --- a/dimos/robot/unitree/go2/connection_spec.py +++ b/dimos/robot/unitree/go2/connection_spec.py @@ -19,3 +19,5 @@ class GO2ConnectionSpec(Spec, Protocol): def publish_request(self, topic: str, data: dict[str, Any]) -> dict[Any, Any]: ... + def liedown(self) -> bool: ... + def standup(self) -> bool: ... diff --git a/dimos/robot/unitree/keyboard_teleop.py b/dimos/robot/unitree/keyboard_teleop.py index 07af844c60..f22dd4abfb 100644 --- a/dimos/robot/unitree/keyboard_teleop.py +++ b/dimos/robot/unitree/keyboard_teleop.py @@ -25,6 +25,7 @@ from dimos.core.stream import Out from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.robot.unitree.go2.connection_spec import GO2ConnectionSpec from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -51,6 +52,8 @@ class KeyboardTeleop(Module): cmd_vel: Out[Twist] + _go2: GO2ConnectionSpec | None = None + _stop_event: threading.Event _keys_held: set[int] | None = None _thread: threading.Thread | None = None @@ -105,6 +108,18 @@ def stop(self) -> None: super().stop() + def _call_go2_pose(self, action: str) -> None: + if self._go2 is None: + logger.warning(f"{action} ignored: no Go2 connection wired into teleop") + return + try: + if action == "liedown": + self._go2.liedown() + else: + self._go2.standup() + except Exception as error: + logger.error(f"{action} command failed: {error}") + def _pygame_loop(self) -> None: if self._keys_held is None: raise RuntimeError("_keys_held not initialized") @@ -133,6 +148,10 @@ def _pygame_loop(self) -> None: elif event.key == pygame.K_ESCAPE: # ESC quits self._stop_event.set() + elif event.key == pygame.K_z: + self._call_go2_pose("liedown") + elif event.key == pygame.K_x: + self._call_go2_pose("standup") elif event.type == pygame.KEYUP: self._keys_held.discard(event.key) @@ -232,6 +251,7 @@ def _update_display(self, twist: Twist) -> None: help_texts = [ "WS: Move | AD: Turn | QE: Strafe", "Shift: Boost | Ctrl: Slow", + "Z: Lie Down | X: Stand Up", "Space: E-Stop | ESC: Quit", ] for text in help_texts: From 9065332de3b9601e56793267e3919bebdc3081b4 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 4 Jun 2026 18:27:14 -0700 Subject: [PATCH 030/185] fixup pose --- dimos/mapping/recording/go2_mid360/record.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dimos/mapping/recording/go2_mid360/record.py b/dimos/mapping/recording/go2_mid360/record.py index 84ebf84076..15fc601e60 100644 --- a/dimos/mapping/recording/go2_mid360/record.py +++ b/dimos/mapping/recording/go2_mid360/record.py @@ -93,13 +93,13 @@ def on_msg(msg: Any) -> None: world_to_base = self._world_to_base_from_fastlio() if world_to_base is not None: pose = world_to_base.to_pose() - elif name == "color_image": + elif name in ("color_image", "go2_color_image"): # anchor images to world frame as defined by fastlio odom world_to_base = self._world_to_base_from_fastlio() if world_to_base is not None: pose = (world_to_base + BASE_TO_CAMERA_OPTICAL).to_pose() - elif "odom" in name: - pass + elif name == "go2_odom" or name == "odom": + pose = msg else: if name not in self._warning_names: self._warning_names.add(name) From f8a6b65da3dd125aff4dfebb28f08fca88b1794f Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 4 Jun 2026 19:14:02 -0700 Subject: [PATCH 031/185] pointlio native: replay harness, deterministic clock, pcap tooling + race-fix flake relock Adds the offline replay/inspection tooling for the pointlio_native module: pcap_replay.hpp (streams a Livox pcap into the SDK callbacks), deterministic_clock + dual-thread replay options, the ruwik2_pt3 replay harness, the pcap_to_db tool (append pointlio_odometry into an existing memory db at ~30Hz, streaming), validated Point-LIO mid360.yaml, and the live smoke-test helper. flake.lock relocks onto the fastlio2 pointlio rev carrying the mtx_buffer race fix; main.cpp comment corrected to attribute the divergence fix to that lock (not the publish-rate gating). --- .../sensors/lidar/pointlio/config/mid360.yaml | 83 +++- .../lidar/pointlio/cpp/config/mid360.yaml | 72 +++ .../sensors/lidar/pointlio/cpp/flake.lock | 15 +- .../sensors/lidar/pointlio/cpp/flake.nix | 2 +- .../sensors/lidar/pointlio/cpp/main.cpp | 456 +++++++++++++++--- .../lidar/pointlio/cpp/pcap_replay.hpp | 364 ++++++++++++++ .../sensors/lidar/pointlio/cpp/timing.hpp | 30 +- .../hardware/sensors/lidar/pointlio/module.py | 57 ++- .../sensors/lidar/pointlio/tools/__init__.py | 0 .../lidar/pointlio/tools/demo_live_test.py | 121 +++++ .../lidar/pointlio/tools/pcap_to_db.py | 271 +++++++++++ .../lidar/pointlio/tools/replay_ruwik2_pt3.py | 261 ++++++++++ 12 files changed, 1598 insertions(+), 134 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/pointlio/cpp/config/mid360.yaml create mode 100644 dimos/hardware/sensors/lidar/pointlio/cpp/pcap_replay.hpp create mode 100644 dimos/hardware/sensors/lidar/pointlio/tools/__init__.py create mode 100644 dimos/hardware/sensors/lidar/pointlio/tools/demo_live_test.py create mode 100644 dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py create mode 100644 dimos/hardware/sensors/lidar/pointlio/tools/replay_ruwik2_pt3.py diff --git a/dimos/hardware/sensors/lidar/pointlio/config/mid360.yaml b/dimos/hardware/sensors/lidar/pointlio/config/mid360.yaml index 512047ee48..1839646d5a 100644 --- a/dimos/hardware/sensors/lidar/pointlio/config/mid360.yaml +++ b/dimos/hardware/sensors/lidar/pointlio/config/mid360.yaml @@ -1,35 +1,72 @@ +# Non-ROS Point-LIO config (Mid-360). Parsed by parameters.cpp via yaml-cpp. common: - lid_topic: "/livox/lidar" - imu_topic: "/livox/imu" - time_sync_en: false # ONLY turn on when external time synchronization is really not possible - time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). - # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + con_frame: false + con_frame_num: 1 + cut_frame: false + cut_frame_time_interval: 0.1 + time_lag_imu_to_lidar: 0.0 preprocess: - lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + lidar_type: 1 # 1 = Livox CustomMsg scan_line: 4 + scan_rate: 10 + timestamp_unit: 3 # 3 = nanosecond blind: 0.5 + point_filter_num: 3 mapping: - acc_cov: 0.1 - gyr_cov: 0.1 - b_acc_cov: 0.0001 + use_imu_as_input: false # false = IMU-as-output model (Point-LIO's robust path) + prop_at_freq_of_imu: true + check_satu: true + init_map_size: 10 + space_down_sample: true + satu_acc: 5.5 # g (acc_norm=1.0). Just below the ruwik2 6.9g spikes. + satu_gyro: 35.0 + acc_norm: 1.0 # IMU accel in g + plane_thr: 0.1 + filter_size_surf: 0.5 + filter_size_map: 0.5 + cube_side_length: 1000.0 + det_range: 100.0 + fov_degree: 360.0 + imu_en: true + start_in_aggressive_motion: false + extrinsic_est_en: false + imu_time_inte: 0.005 + lidar_meas_cov: 0.01 + acc_cov_input: 0.1 + vel_cov: 20.0 + gyr_cov_input: 0.01 + gyr_cov_output: 1000.0 + acc_cov_output: 500.0 b_gyr_cov: 0.0001 - fov_degree: 360 - det_range: 100.0 - extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic - extrinsic_T: [ -0.011, -0.02329, 0.04412 ] - extrinsic_R: [ 1, 0, 0, - 0, 1, 0, - 0, 0, 1] + b_acc_cov: 0.0001 + imu_meas_acc_cov: 0.01 + imu_meas_omg_cov: 0.01 + match_s: 81.0 + gravity_align: true + gravity: [0.0, 0.0, -9.810] + gravity_init: [0.0, 0.0, -9.810] + extrinsic_T: [-0.011, -0.02329, 0.04412] + extrinsic_R: [1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0] + +odometry: + publish_odometry_without_downsample: false + odom_only: false + odom_header_frame_id: "camera_init" + odom_child_frame_id: "aft_mapped" publish: - path_en: false - scan_publish_en: true # false: close all the point cloud output - dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. - scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + path_en: false + scan_publish_en: false + scan_bodyframe_pub_en: false + +runtime_pos_log_enable: false pcd_save: - pcd_save_en: true - interval: -1 # how many LiDAR frames saved in each pcd file; - # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. + pcd_save_en: false + interval: -1 diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/config/mid360.yaml b/dimos/hardware/sensors/lidar/pointlio/cpp/config/mid360.yaml new file mode 100644 index 0000000000..1839646d5a --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/config/mid360.yaml @@ -0,0 +1,72 @@ +# Non-ROS Point-LIO config (Mid-360). Parsed by parameters.cpp via yaml-cpp. +common: + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + con_frame: false + con_frame_num: 1 + cut_frame: false + cut_frame_time_interval: 0.1 + time_lag_imu_to_lidar: 0.0 + +preprocess: + lidar_type: 1 # 1 = Livox CustomMsg + scan_line: 4 + scan_rate: 10 + timestamp_unit: 3 # 3 = nanosecond + blind: 0.5 + point_filter_num: 3 + +mapping: + use_imu_as_input: false # false = IMU-as-output model (Point-LIO's robust path) + prop_at_freq_of_imu: true + check_satu: true + init_map_size: 10 + space_down_sample: true + satu_acc: 5.5 # g (acc_norm=1.0). Just below the ruwik2 6.9g spikes. + satu_gyro: 35.0 + acc_norm: 1.0 # IMU accel in g + plane_thr: 0.1 + filter_size_surf: 0.5 + filter_size_map: 0.5 + cube_side_length: 1000.0 + det_range: 100.0 + fov_degree: 360.0 + imu_en: true + start_in_aggressive_motion: false + extrinsic_est_en: false + imu_time_inte: 0.005 + lidar_meas_cov: 0.01 + acc_cov_input: 0.1 + vel_cov: 20.0 + gyr_cov_input: 0.01 + gyr_cov_output: 1000.0 + acc_cov_output: 500.0 + b_gyr_cov: 0.0001 + b_acc_cov: 0.0001 + imu_meas_acc_cov: 0.01 + imu_meas_omg_cov: 0.01 + match_s: 81.0 + gravity_align: true + gravity: [0.0, 0.0, -9.810] + gravity_init: [0.0, 0.0, -9.810] + extrinsic_T: [-0.011, -0.02329, 0.04412] + extrinsic_R: [1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0] + +odometry: + publish_odometry_without_downsample: false + odom_only: false + odom_header_frame_id: "camera_init" + odom_child_frame_id: "aft_mapped" + +publish: + path_en: false + scan_publish_en: false + scan_bodyframe_pub_en: false + +runtime_pos_log_enable: false + +pcd_save: + pcd_save_en: false + interval: -1 diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock index e5b0b2989f..6472a83e88 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock @@ -37,18 +37,13 @@ "fast-lio": { "flake": false, "locked": { - "lastModified": 1780527354, - "narHash": "sha256-2itM7Xqd3XJIr82gvRzmosthzA8qLtWXl9vyXGFObqA=", - "ref": "pointlio", - "rev": "fc979456837be26dd4d4e8fe0a53a7de3f9baf33", - "revCount": 66, - "type": "git", - "url": "ssh://git@github.com/dimensionalOS/dimos-module-fastlio2" + "narHash": "sha256-DPoyTQbYlVi6qwwtLJVqgpdDHa2bQ+2W3v71QtIIPNU=", + "path": "/home/dimos/repos/dimos-module-fastlio2", + "type": "path" }, "original": { - "ref": "pointlio", - "type": "git", - "url": "ssh://git@github.com/dimensionalOS/dimos-module-fastlio2" + "path": "/home/dimos/repos/dimos-module-fastlio2", + "type": "path" } }, "flake-utils": { diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix index 04601d815e..cdcf4bb32b 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix @@ -13,7 +13,7 @@ flake = false; }; fast-lio = { - url = "git+ssh://git@github.com/dimensionalOS/dimos-module-fastlio2?ref=pointlio"; + url = "path:/home/dimos/repos/dimos-module-fastlio2"; flake = false; }; lcm-extended = { diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 8d645031cf..90f2f9df6d 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -4,14 +4,14 @@ // FAST-LIO2 + Livox Mid-360 native module for dimos NativeModule framework. // // Binds Livox SDK2 directly into FAST-LIO-NON-ROS: SDK callbacks feed -// CustomMsg/Imu to FastLio, which performs EKF-LOAM SLAM. Sensor-frame -// (mid360_link) point clouds and odometry are published on LCM. +// CustomMsg/Imu to FastLio, which performs EKF-LOAM SLAM. Registered +// (world-frame) point clouds and odometry are published on LCM. // // Usage: // ./fastlio2_native \ // --lidar '/lidar#sensor_msgs.PointCloud2' \ // --odometry '/odometry#nav_msgs.Odometry' \ -// --config_path /path/to/default.yaml \ +// --config_path /path/to/mid360.yaml \ // --host_ip 192.168.1.5 --lidar_ip 192.168.1.155 #include @@ -33,7 +33,9 @@ #include "cloud_filter.hpp" #include "dimos_native_module.hpp" +#include "pcap_replay.hpp" #include "timing.hpp" +#include "voxel_map.hpp" // dimos LCM message headers #include "geometry_msgs/Quaternion.hpp" @@ -60,27 +62,104 @@ static std::atomic g_running{true}; static lcm::LCM* g_lcm = nullptr; static FastLio* g_fastlio = nullptr; +// Virtual clock: in replay mode, tracks the pcap timestamp of the packet +// currently being fed so publish_*() reports the original capture time +// instead of replay wall time. Live mode leaves it at 0 and publish_*() +// falls back to system_clock::now(). +static std::atomic g_replay_mode{false}; +static std::atomic g_virtual_clock_ns{0}; + +// Deterministic clock mode. When set, both live and replay drive +// g_virtual_clock_ns from the packet's sensor-clock pkt->timestamp (which +// is identical bit-for-bit between SDK delivery and pcap), and use it as +// the source for scan-boundary rate limits and publish timestamps. This +// removes wall-clock jitter from scan boundaries → live and replay produce +// the same algorithm state. Trade-off: published header.stamp values +// become sensor-boot-relative seconds instead of unix wall time, so this +// is off by default and only flipped on by the record/replay demos. +static std::atomic g_deterministic_clock{false}; + +// First packet's sensor ts (deterministic mode only). Used to seed the +// main loop's rate-limit bookmarks at exactly the first delivered packet, +// independent of when the main loop's first iteration happens to run. +static std::atomic g_first_packet_clock_ns{0}; + +// First-packet marker. Used by record/replay tooling to align the SDK's +// warmup-induced packet drop with replay. The C++ binary writes the wall +// clock of the first on_point_cloud / on_imu_data callback (live mode +// only) to this file; demo_replay reads it back and passes the value as +// --replay_skip_until_ns so pcap_replay drops the same SDK-eaten prefix. +static std::string g_first_packet_marker_path; +static std::atomic g_first_packet_marker_written{false}; + +// The packet's sensor-clock timestamp (pkt->timestamp) is identical bit-for-bit +// between the live SDK delivery path and the recorded pcap, so writing it from +// the first SDK callback gives replay an exact boundary to skip on. Wall clock +// would only let us match within delivery latency (sub-ms). +static void mark_first_packet(uint64_t pkt_timestamp_ns) { + if (g_first_packet_marker_path.empty()) { + return; + } + bool expected = false; + if (!g_first_packet_marker_written.compare_exchange_strong(expected, true)) { + return; + } + FILE* f = std::fopen(g_first_packet_marker_path.c_str(), "w"); + if (f) { + std::fprintf(f, "%lu\n", static_cast(pkt_timestamp_ns)); + std::fclose(f); + } +} + static double get_publish_ts() { + if (g_deterministic_clock.load() || g_replay_mode.load()) { + return static_cast(g_virtual_clock_ns.load()) / 1e9; + } return std::chrono::duration( std::chrono::system_clock::now().time_since_epoch()).count(); } +// Virtualized clock for the main loop's frame/publish rate limiters. In +// replay mode this returns a time_point derived from g_virtual_clock_ns so +// scan boundaries are determined by packet arrival, not by wall-clock thread +// scheduling jitter. Returns nullopt if replay hasn't yet seen a packet +// (caller should skip rate-limit checks in that case). +static std::optional virtual_now() { + // Only the deterministic-clock flag drives the virtual (sensor-paced) + // clock. Replay with deterministic_clock=false must use the real wall + // clock so scan boundaries are cut on thread-scheduling timing — that + // restores the live feeder/main scan-composition race that the + // deterministic path deliberately removes (needed to reproduce the live + // divergence offline). + if (g_deterministic_clock.load()) { + uint64_t ns = g_virtual_clock_ns.load(); + if (ns == 0) { + return std::nullopt; + } + return std::chrono::steady_clock::time_point(std::chrono::nanoseconds(ns)); + } + return std::chrono::steady_clock::now(); +} + static std::string g_lidar_topic; static std::string g_odometry_topic; +static std::string g_map_topic; static std::string g_frame_id; // required via --frame_id static std::string g_child_frame_id; // required via --child_frame_id static float g_frequency = 10.0f; // Initial pose offset (applied to all SLAM outputs) +// Position offset static double g_init_x = 0.0; static double g_init_y = 0.0; static double g_init_z = 0.0; +// Orientation offset as quaternion (identity = no rotation) static double g_init_qx = 0.0; static double g_init_qy = 0.0; static double g_init_qz = 0.0; static double g_init_qw = 1.0; -// Hamilton product: q_out = q1 * q2 +// Helper: quaternion multiply (Hamilton product) q_out = q1 * q2 static void quat_mul(double ax, double ay, double az, double aw, double bx, double by, double bz, double bw, double& ox, double& oy, double& oz, double& ow) { @@ -90,18 +169,21 @@ static void quat_mul(double ax, double ay, double az, double aw, oz = aw*bz + ax*by - ay*bx + az*bw; } -// Rotate vector by quaternion: v_out = q * v * q_inv +// Helper: rotate a vector by a quaternion v_out = q * v * q_inv static void quat_rotate(double qx, double qy, double qz, double qw, double vx, double vy, double vz, double& ox, double& oy, double& oz) { + // t = 2 * cross(q_xyz, v) double tx = 2.0 * (qy*vz - qz*vy); double ty = 2.0 * (qz*vx - qx*vz); double tz = 2.0 * (qx*vy - qy*vx); + // v_out = v + qw*t + cross(q_xyz, t) ox = vx + qw*tx + (qy*tz - qz*ty); oy = vy + qw*ty + (qz*tx - qx*tz); oz = vz + qw*tz + (qx*ty - qy*tx); } +// Check if initial pose is non-identity static bool has_init_pose() { return g_init_x != 0.0 || g_init_y != 0.0 || g_init_z != 0.0 || g_init_qx != 0.0 || g_init_qy != 0.0 || g_init_qz != 0.0 || g_init_qw != 1.0; @@ -127,11 +209,9 @@ using dimos::time_from_seconds; using dimos::make_header; // --------------------------------------------------------------------------- -// Publish lidar point cloud in the sensor body frame (g_frame_id / mid360_link) +// Publish lidar (world-frame point cloud) // --------------------------------------------------------------------------- -// -// `cloud` is FAST-LIO's undistorted scan in the sensor's own frame -// (get_body_cloud), so points are published as-is with no world registration. + static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, const std::string& topic = "") { const std::string& chan = topic.empty() ? g_lidar_topic : topic; @@ -146,7 +226,7 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, pc.is_bigendian = 0; pc.is_dense = 1; - // x, y, z, intensity (float32 each) + // Fields: x, y, z, intensity (float32 each) pc.fields_length = 4; pc.fields.resize(4); @@ -170,11 +250,28 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, pc.data_length = pc.row_step; pc.data.resize(pc.data_length); + // Apply the full init_pose transform (rotation + translation) to point clouds. + // FAST-LIO's map origin is at the sensor's initial position. The rotation + // corrects axis direction (e.g. 180° X for upside-down mount) and the + // translation shifts the origin so that ground sits at z≈0 (e.g. z=1.2 + // for a sensor mounted 1.2m above ground). This matches the odometry + // frame, which also gets the full init_pose applied. + const bool apply_init_pose = has_init_pose(); for (int i = 0; i < num_points; ++i) { float* dst = reinterpret_cast(pc.data.data() + i * 16); - dst[0] = cloud->points[i].x; - dst[1] = cloud->points[i].y; - dst[2] = cloud->points[i].z; + if (apply_init_pose) { + double rx, ry, rz; + quat_rotate(g_init_qx, g_init_qy, g_init_qz, g_init_qw, + cloud->points[i].x, cloud->points[i].y, cloud->points[i].z, + rx, ry, rz); + dst[0] = static_cast(rx + g_init_x); + dst[1] = static_cast(ry + g_init_y); + dst[2] = static_cast(rz + g_init_z); + } else { + dst[0] = cloud->points[i].x; + dst[1] = cloud->points[i].y; + dst[2] = cloud->points[i].z; + } dst[3] = cloud->points[i].intensity; } @@ -192,7 +289,7 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times msg.header = make_header(g_frame_id, timestamp); msg.child_frame_id = g_child_frame_id; - // p_out = R_init * p_slam + t_init + // Pose (apply initial pose offset: p_out = R_init * p_slam + t_init) if (has_init_pose()) { double rx, ry, rz; quat_rotate(g_init_qx, g_init_qy, g_init_qz, g_init_qw, @@ -225,11 +322,12 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times msg.pose.pose.orientation.w = odom.pose.pose.orientation.w; } + // Covariance (fixed-size double[36]) for (int i = 0; i < 36; ++i) { msg.pose.covariance[i] = odom.pose.covariance[i]; } - // Twist zeroed — FAST-LIO doesn't output velocity. + // Twist (zero — FAST-LIO doesn't output velocity directly) msg.twist.twist.linear.x = 0; msg.twist.twist.linear.y = 0; msg.twist.twist.linear.z = 0; @@ -253,14 +351,25 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ uint64_t ts_ns = get_timestamp_ns(data); uint16_t dot_num = data->dot_num; - // Per-point intra-packet offset (matches livox_ros_driver2). Without it all - // points share one timestamp and per-point deskew is lost. time_interval - // unit is 0.1us, so *100 → ns. - const uint64_t point_interval_ns = - dot_num > 0 ? static_cast(data->time_interval) * 100 / dot_num : 0; + if (!g_replay_mode.load()) { + mark_first_packet(ts_ns); + } std::lock_guard lock(g_pc_mutex); + // Update the deterministic-mode virtual clock INSIDE the accumulator + // mutex so the main loop can never observe a clock advance without + // also seeing the matching points (race that drifts scan composition). + // Monotonic update: SDK threads can deliver packets slightly out of + // sensor-ts order, and we must not let a later store(lower_ts) undo + // a previous store(higher_ts). + if (g_deterministic_clock.load()) { + uint64_t expected = 0; + g_first_packet_clock_ns.compare_exchange_strong(expected, ts_ns); + uint64_t cur = g_virtual_clock_ns.load(); + while (cur < ts_ns && !g_virtual_clock_ns.compare_exchange_weak(cur, ts_ns)) {} + } + if (!g_frame_has_timestamp) { g_frame_start_ns = ts_ns; g_frame_has_timestamp = true; @@ -275,8 +384,8 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ cp.z = static_cast(pts[i].z) / 1000.0; cp.reflectivity = pts[i].reflectivity; cp.tag = pts[i].tag; - cp.line = 0; // Mid-360: single line - cp.offset_time = static_cast((ts_ns - g_frame_start_ns) + i * point_interval_ns); + cp.line = 0; // Mid-360: non-repetitive, single "line" + cp.offset_time = static_cast(ts_ns - g_frame_start_ns); g_accumulated_points.push_back(cp); } } else if (data->data_type == DATA_TYPE_CARTESIAN_LOW) { @@ -289,7 +398,7 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ cp.reflectivity = pts[i].reflectivity; cp.tag = pts[i].tag; cp.line = 0; - cp.offset_time = static_cast((ts_ns - g_frame_start_ns) + i * point_interval_ns); + cp.offset_time = static_cast(ts_ns - g_frame_start_ns); g_accumulated_points.push_back(cp); } } @@ -300,9 +409,12 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, if (!g_running.load() || data == nullptr || !g_fastlio) return; uint64_t pkt_ts_ns = get_timestamp_ns(data); - // Live IMU-drop instrumentation: a dropped datagram shows as a sensor-ts - // jump; wall gaps exceeding sensor gaps mean callback starvation. - { + if (!g_replay_mode.load()) { + mark_first_packet(pkt_ts_ns); + // Live IMU-drop instrumentation: a dropped UDP datagram shows up as a + // jump in the sensor timestamp (each pkt is ~5ms apart at 200Hz). Wall + // gaps that exceed sensor gaps mean callback starvation. Confirms (or + // refutes) the dropped-IMU divergence hypothesis on real hardware. static std::atomic last_pkt_ts_ns{0}; static std::atomic imu_pkt_count{0}; static std::atomic imu_gap_count{0}; @@ -358,8 +470,10 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, for (int j = 0; j < 9; ++j) imu_msg->angular_velocity_covariance[j] = 0.0; - // Point-LIO expects accel in g (EKF does its own scaling). SDK already - // reports g, so feed raw — scaling by GRAVITY_MS2 would double-scale and + // Point-LIO consumes accel in g (config acc_norm=1.0; the EKF applies + // its own G_m_s2/acc_norm scaling internally). The Livox SDK already + // reports acc in g, so feed it raw — multiplying by GRAVITY_MS2 here + // would double-scale, blow up the gravity estimate, and permanently // trip the satu_acc check at rest. imu_msg->linear_acceleration.x = static_cast(imu_pts[i].acc_x); imu_msg->linear_acceleration.y = static_cast(imu_pts[i].acc_y); @@ -369,6 +483,18 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, g_fastlio->feed_imu(imu_msg); } + + // Advance the deterministic-mode virtual clock AFTER feed_imu has + // queued the sample, holding g_pc_mutex so this is fully serialized + // with on_point_cloud / the main-loop scan swap. Monotonic CAS so + // out-of-order SDK delivery can't roll the clock backward. + if (g_deterministic_clock.load()) { + std::lock_guard lock(g_pc_mutex); + uint64_t expected = 0; + g_first_packet_clock_ns.compare_exchange_strong(expected, pkt_ts_ns); + uint64_t cur = g_virtual_clock_ns.load(); + while (cur < pkt_ts_ns && !g_virtual_clock_ns.compare_exchange_weak(cur, pkt_ts_ns)) {} + } } static void on_info_change(const uint32_t handle, const LivoxLidarInfo* info, @@ -407,6 +533,7 @@ int main(int argc, char** argv) { // Required: LCM topics for output ports g_lidar_topic = mod.has("lidar") ? mod.topic("lidar") : ""; g_odometry_topic = mod.has("odometry") ? mod.topic("odometry") : ""; + g_map_topic = mod.has("global_map") ? mod.topic("global_map") : ""; if (g_lidar_topic.empty() && g_odometry_topic.empty()) { fprintf(stderr, "Error: at least one of --lidar or --odometry is required\n"); @@ -429,15 +556,19 @@ int main(int argc, char** argv) { std::string lidar_ip = mod.arg("lidar_ip", "192.168.1.155"); g_frequency = mod.arg_float("frequency", 10.0f); g_frame_id = mod.arg_required("frame_id"); - g_child_frame_id = mod.arg_required("odom_frame_id"); + g_child_frame_id = mod.arg_required("child_frame_id"); float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f); float odom_freq = mod.arg_float("odom_freq", 50.0f); CloudFilterConfig filter_cfg; filter_cfg.voxel_size = mod.arg_float("voxel_size", 0.1f); filter_cfg.sor_mean_k = mod.arg_int("sor_mean_k", 50); filter_cfg.sor_stddev = mod.arg_float("sor_stddev", 1.0f); + float map_voxel_size = mod.arg_float("map_voxel_size", 0.1f); + float map_max_range = mod.arg_float("map_max_range", 100.0f); + float map_freq = mod.arg_float("map_freq", 0.0f); - // Propagates to the FAST-LIO core via the `fastlio_debug` global. + // Verbose logging — propagates to the FAST-LIO C++ core via the + // `fastlio_debug` global. Default false → only real errors print. bool debug = mod.arg_bool("debug", false); fastlio_debug = debug; @@ -455,6 +586,39 @@ int main(int argc, char** argv) { ports.host_imu_data = mod.arg_int("host_imu_data_port", port_defaults.host_imu_data); ports.host_log_data = mod.arg_int("host_log_data_port", port_defaults.host_log_data); + // Replay mode (offline). When --replay_pcap is given the SDK is not + // initialized; a feeder thread reads the pcap and calls the existing + // on_point_cloud / on_imu_data callbacks directly. publish_*() uses + // the pcap timestamps as the clock so outputs match the original run. + std::string replay_pcap = mod.arg("replay_pcap", ""); + g_replay_mode.store(!replay_pcap.empty()); + + // Drop pcap packets with pcap_ts < this value. Used in replay to mimic + // the SDK warmup discard that the live run experienced — so the + // algorithm starts from the same first packet in both modes. + uint64_t replay_skip_until_ns = 0; + { + std::string s = mod.arg("replay_skip_until_ns", "0"); + if (!s.empty()) { + replay_skip_until_ns = std::stoull(s); + } + } + + // Live mode: write the wall_clock_ns of the first SDK callback to this + // file. Pair with replay's --replay_skip_until_ns to align packet sets. + g_first_packet_marker_path = mod.arg("first_packet_marker", ""); + + // Replay: feed point and IMU on two separate threads (mimics the live + // Livox SDK's concurrent delivery threads). Only meaningful with + // deterministic_clock=false. + const bool replay_dual_thread = mod.arg_bool("replay_dual_thread", false); + + // Drive virtual_now() and get_publish_ts() off the packet's sensor + // clock in both live and replay. Eliminates wall-clock jitter from + // scan boundaries and makes outputs bit-comparable across modes. + // Changes header.stamp from unix wall time to sensor-boot seconds. + g_deterministic_clock.store(mod.arg_bool("deterministic_clock", false)); + // Initial pose offset [x, y, z, qx, qy, qz, qw] { std::string init_str = mod.arg("init_pose", ""); @@ -483,6 +647,8 @@ int main(int argc, char** argv) { g_lidar_topic.empty() ? "(disabled)" : g_lidar_topic.c_str()); printf("[fastlio2] odometry topic: %s\n", g_odometry_topic.empty() ? "(disabled)" : g_odometry_topic.c_str()); + printf("[fastlio2] global_map topic: %s\n", + g_map_topic.empty() ? "(disabled)" : g_map_topic.c_str()); printf("[fastlio2] config: %s\n", config_path.c_str()); printf("[fastlio2] host_ip: %s lidar_ip: %s frequency: %.1f Hz\n", host_ip.c_str(), lidar_ip.c_str(), g_frequency); @@ -490,6 +656,9 @@ int main(int argc, char** argv) { pointcloud_freq, odom_freq); printf("[fastlio2] voxel_size: %.3f sor_mean_k: %d sor_stddev: %.1f\n", filter_cfg.voxel_size, filter_cfg.sor_mean_k, filter_cfg.sor_stddev); + if (!g_map_topic.empty()) + printf("[fastlio2] map_voxel_size: %.3f map_max_range: %.1f map_freq: %.1f Hz\n", + map_voxel_size, map_max_range, map_freq); } // Signal handlers @@ -504,13 +673,16 @@ int main(int argc, char** argv) { } g_lcm = &lcm; + // Init FAST-LIO with config if (debug) printf("[fastlio2] Initializing FAST-LIO...\n"); FastLio fast_lio(config_path, msr_freq, main_freq); g_fastlio = &fast_lio; if (debug) printf("[fastlio2] FAST-LIO initialized.\n"); - // Main-loop state. Body lives in `run_main_iter`, driven by the wall-paced - // main thread. + // Main-loop state. The body lives in `run_main_iter` below so it can be + // invoked from either the wall-clock-paced main thread (live) or the + // pcap-paced feeder thread (replay), with no race on accumulator + // contents in the replay case. auto frame_interval = std::chrono::microseconds( static_cast(1e6 / g_frequency)); std::optional last_emit; @@ -523,50 +695,91 @@ int main(int argc, char** argv) { std::optional last_pc_publish; std::optional last_odom_publish; + std::unique_ptr global_map; + std::chrono::microseconds map_interval{0}; + std::optional last_map_publish; + if (!g_map_topic.empty() && map_freq > 0.0f) { + global_map = std::make_unique(map_voxel_size, map_max_range); + map_interval = std::chrono::microseconds( + static_cast(1e6 / map_freq)); + } - // Per-section timing for `run_main_iter`, active only with --debug. - // maybe_flush() below prints a summary every second. + // Per-section timing counters for `run_main_iter`. Active only when + // --debug is on (Scope's constructor reads `fastlio_debug` and no-ops + // otherwise). `timing::maybe_flush(now)` at the bottom prints a per- + // section summary every second of wall clock so we can see both how + // often each part fires and how long each call takes. static timing::Section t_iter{"run_main_iter"}; static timing::Section t_emit_check{"emit.lock+swap"}; static timing::Section t_feed_lidar{"fast_lio.feed_lidar"}; static timing::Section t_process{"fast_lio.process"}; - static timing::Section t_get_world_cloud{"fast_lio.get_body_cloud"}; + static timing::Section t_get_world_cloud{"fast_lio.get_world_cloud"}; static timing::Section t_filter_cloud{"filter_cloud"}; static timing::Section t_publish_lidar{"publish_lidar"}; + static timing::Section t_map_insert{"global_map.insert"}; + static timing::Section t_map_publish{"global_map.publish"}; static timing::Section t_publish_odom{"publish_odometry"}; auto run_main_iter = [&](std::chrono::steady_clock::time_point now) { timing::Scope iter_scope(t_iter); - // Lazy-seed rate-limit bookmarks on the first iteration so they align - // with the wall clock. + // Lazy-seed all rate-limit bookmarks on the first iteration so they + // line up with the chosen clock (wall in live, pcap in replay) and + // don't fire immediately based on an arbitrary "since program start" + // delta. In deterministic mode we seed from the FIRST packet's + // sensor ts (not the current ts) so both live and replay anchor + // their first scan boundary at the same packet — required for + // bit-for-bit live↔replay parity. + auto seed = now; + if (g_deterministic_clock.load()) { + uint64_t first = g_first_packet_clock_ns.load(); + if (first != 0) { + seed = std::chrono::steady_clock::time_point( + std::chrono::nanoseconds(first)); + } + } if (!last_emit.has_value()) { - last_emit = now; + last_emit = seed; } if (!last_pc_publish.has_value()) { - last_pc_publish = now; + last_pc_publish = seed; } if (!last_odom_publish.has_value()) { - last_odom_publish = now; + last_odom_publish = seed; + } + if (global_map && !last_map_publish.has_value()) { + last_map_publish = seed; } - // At frame rate: drain accumulated points into a CustomMsg and feed - // FAST-LIO. Hold g_pc_mutex across the rate-limit check AND swap so the - // clock + accumulator are observed atomically (no packet slips between). + // At frame rate: drain accumulated raw points into a CustomMsg + // and feed to FAST-LIO. Hold g_pc_mutex across the rate-limit + // CHECK as well as the swap, so in deterministic mode the + // virtual clock + accumulator are observed atomically with no + // other thread able to slip a packet in between the decision + // and the swap. std::vector points; uint64_t frame_start = 0; { timing::Scope s(t_emit_check); std::lock_guard lock(g_pc_mutex); - if (now - *last_emit >= frame_interval) { + auto check_now = now; + if (g_deterministic_clock.load()) { + uint64_t ns = g_virtual_clock_ns.load(); + if (ns != 0) { + check_now = std::chrono::steady_clock::time_point( + std::chrono::nanoseconds(ns)); + } + } + if (check_now - *last_emit >= frame_interval) { if (!g_accumulated_points.empty()) { points.swap(g_accumulated_points); frame_start = g_frame_start_ns; g_frame_has_timestamp = false; } - last_emit = now; + last_emit = check_now; } } if (!points.empty()) { + // Build CustomMsg auto lidar_msg = boost::make_shared(); lidar_msg->header.seq = 0; lidar_msg->header.stamp = custom_messages::Time().fromSec( @@ -581,38 +794,71 @@ int main(int argc, char** argv) { fast_lio.feed_lidar(lidar_msg); } - // One FAST-LIO IESKF step (cheap when queues empty). + // Run one FAST-LIO IESKF step. Cheap when the IMU/lidar queues + // are empty; the heavy work happens after a feed_lidar above. { timing::Scope s(t_process); fast_lio.process(); } + // Check for new SLAM results and publish (rate-limited). auto pose = fast_lio.get_pose(); if (!pose.empty() && (pose[0] != 0.0 || pose[1] != 0.0 || pose[2] != 0.0)) { double ts = get_publish_ts(); const bool lidar_due = !g_lidar_topic.empty() && now - *last_pc_publish >= pc_interval; - - // get_body_cloud + filter_cloud (SOR) is the loop's costliest step, - // so build it only when a publish is due. - if (lidar_due) { - auto body_cloud = ([&]() { + const bool map_due = + global_map && now - *last_map_publish >= map_interval; + + // get_world_cloud() + filter_cloud (SOR, mean_k=50) are by far the + // most expensive step in the loop, so build them ONLY when a lidar + // or map publish is actually due rather than every main_freq + // iteration. (Note: this is a CPU optimization, not the divergence + // fix — the live runaway was a std::deque race in the fastlio core's + // sync_packages/IESKF loop, since fixed by locking mtx_buffer there. + // Dropped IMU datagrams were ruled out: live diverged with zero + // dropped packets.) + if (lidar_due || map_due) { + auto world_cloud = ([&]() { timing::Scope s(t_get_world_cloud); - return fast_lio.get_body_cloud(); + return fast_lio.get_world_cloud(); })(); - if (body_cloud && !body_cloud->empty()) { + if (world_cloud && !world_cloud->empty()) { auto filtered = ([&]() { timing::Scope s(t_filter_cloud); - return filter_cloud(body_cloud, filter_cfg); + return filter_cloud(world_cloud, filter_cfg); })(); - timing::Scope s(t_publish_lidar); - publish_lidar(filtered, ts); - last_pc_publish = now; + + // Per-scan world-frame cloud at pointcloud_freq. + if (lidar_due) { + timing::Scope s(t_publish_lidar); + publish_lidar(filtered, ts); + last_pc_publish = now; + } + + // Global voxel map: insert this scan, prune, then publish + // a snapshot at map_freq. + if (global_map) { + { + timing::Scope s(t_map_insert); + global_map->insert(filtered); + } + if (map_due) { + timing::Scope s(t_map_publish); + global_map->prune( + static_cast(pose[0]), + static_cast(pose[1]), + static_cast(pose[2])); + auto map_cloud = global_map->to_cloud(); + publish_lidar(map_cloud, ts, g_map_topic); + last_map_publish = now; + } + } } } - // Pose + covariance at odom_freq. + // Pose + covariance, rate-limited to odom_freq. if (!g_odometry_topic.empty() && now - *last_odom_publish >= odom_interval) { timing::Scope s(t_publish_odom); publish_odometry(fast_lio.get_odometry(), ts); @@ -620,28 +866,87 @@ int main(int argc, char** argv) { } } + // Periodic per-section summary to stderr (no-op when --debug off). timing::maybe_flush(std::chrono::steady_clock::now()); }; - // Packet source: Livox SDK callbacks from its own threads feed the - // accumulator/EKF; the main thread below owns run_main_iter. - if (!livox_common::init_livox_sdk(host_ip, lidar_ip, ports, debug)) { - return 1; - } - SetLivoxLidarPointCloudCallBack(on_point_cloud, nullptr); - SetLivoxLidarImuDataCallback(on_imu_data, nullptr); - SetLivoxLidarInfoChangeCallback(on_info_change, nullptr); - if (!LivoxLidarSdkStart()) { - fprintf(stderr, "Error: LivoxLidarSdkStart failed\n"); - LivoxLidarSdkUninit(); - return 1; + // Source of point/IMU packets: + // live mode -> Livox SDK opens UDP sockets + dispatches via callbacks + // from its own threads. Main thread drives run_main_iter + // at main_freq, consuming whatever the SDK queued. + // replay mode -> the feeder thread reads the pcap and pushes packets + // through the same on_point/on_imu callbacks (paced at + // realtime via sleep_until). The MAIN thread — same + // one that runs in live mode — owns run_main_iter and + // drains the accumulator. Two threads in both modes, + // matching architectures, so the only difference is + // the source of packets (SDK vs pcap). + std::thread replay_thread; + if (g_replay_mode.load()) { + if (debug) printf("[fastlio2] REPLAY mode, pcap=%s\n", replay_pcap.c_str()); + replay_thread = std::thread([&]() { + pcap_replay::Replayer rep; + rep.path = replay_pcap; + rep.host_point_port = static_cast(ports.host_point_data); + rep.host_imu_port = static_cast(ports.host_imu_data); + rep.on_point = [](LivoxLidarEthernetPacket* p) { + on_point_cloud(0, 0, p, nullptr); + }; + rep.on_imu = [](LivoxLidarEthernetPacket* p) { + on_imu_data(0, 0, p, nullptr); + }; + rep.on_clock = [](uint64_t pcap_ts_ns) { + // In deterministic mode the callbacks already pushed the + // sensor pkt->timestamp into g_virtual_clock_ns — don't + // overwrite with the pcap (wall) ts. + if (g_deterministic_clock.load()) { + return; + } + g_virtual_clock_ns.store(pcap_ts_ns); + }; + // No rep.on_iter — the main thread drives run_main_iter in + // replay mode now, same as in live. This decouples packet + // ingestion from per-iter filter_cloud cost and lets replay + // run at the same wall throughput as live. + rep.running = &g_running; + // Pace the replay feeder at live wall-clock rate. sleep_until + // throttles the feeder so packets land in the accumulator at + // the same wall cadence as the SDK delivers in live mode. + rep.realtime = true; + rep.skip_until_ns = replay_skip_until_ns; + rep.dual_thread = replay_dual_thread; + rep.run(); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + g_running.store(false); + }); + } else { + if (!livox_common::init_livox_sdk(host_ip, lidar_ip, ports, debug)) { + return 1; + } + SetLivoxLidarPointCloudCallBack(on_point_cloud, nullptr); + SetLivoxLidarImuDataCallback(on_imu_data, nullptr); + SetLivoxLidarInfoChangeCallback(on_info_change, nullptr); + if (!LivoxLidarSdkStart()) { + fprintf(stderr, "Error: LivoxLidarSdkStart failed\n"); + LivoxLidarSdkUninit(); + return 1; + } + if (debug) printf("[fastlio2] SDK started, waiting for device...\n"); } - if (debug) printf("[fastlio2] SDK started, waiting for device...\n"); while (g_running.load()) { auto loop_start = std::chrono::high_resolution_clock::now(); - run_main_iter(std::chrono::steady_clock::now()); + auto now_opt = virtual_now(); + if (!now_opt.has_value()) { + // No clock yet — in replay this means the feeder hasn't read + // its first packet; in live it shouldn't happen because + // virtual_now falls back to steady_clock::now(). + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + continue; + } + run_main_iter(*now_opt); + // Drain LCM messages. lcm.handleTimeout(0); // Rate control (~main_freq, 5kHz default). @@ -656,7 +961,12 @@ int main(int argc, char** argv) { // Cleanup if (debug) printf("[fastlio2] Shutting down...\n"); g_fastlio = nullptr; - LivoxLidarSdkUninit(); + if (replay_thread.joinable()) { + replay_thread.join(); + } + if (!g_replay_mode.load()) { + LivoxLidarSdkUninit(); + } g_lcm = nullptr; if (debug) printf("[fastlio2] Done.\n"); diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/pcap_replay.hpp b/dimos/hardware/sensors/lidar/pointlio/cpp/pcap_replay.hpp new file mode 100644 index 0000000000..b1e4b4b930 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/pcap_replay.hpp @@ -0,0 +1,364 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Read a pcap of recorded Mid-360 UDP traffic and feed each point/imu +// payload to the existing SDK callbacks. Used by `--replay_pcap` to bypass +// the Livox SDK for deterministic offline regression testing. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "livox_lidar_def.h" + +namespace pcap_replay { + +constexpr uint32_t PCAP_MAGIC_LE_US = 0xa1b2c3d4u; +constexpr uint32_t PCAP_MAGIC_LE_NS = 0xa1b23c4du; +constexpr uint32_t LINKTYPE_ETHERNET = 1u; +constexpr uint16_t ETHERTYPE_IPV4 = 0x0800u; +constexpr uint8_t IPPROTO_UDP = 17u; +constexpr size_t ETH_HDR_LEN = 14; +constexpr size_t IP_MIN_HDR_LEN = 20; +constexpr size_t UDP_HDR_LEN = 8; +constexpr size_t LIVOX_ETH_HDR_LEN = 36; + +using PacketCb = std::function; +using ClockCb = std::function; +using IterCb = std::function; + +struct Replayer { + std::string path; + uint16_t host_point_port = 0; + uint16_t host_imu_port = 0; + PacketCb on_point; + PacketCb on_imu; + ClockCb on_clock; + // Called synchronously after every packet, once the payload has been + // appended and the virtual clock advanced. The replay path runs the + // main-loop body here so feeding + processing happen on a single + // thread — eliminates the feeder-vs-main-loop race on accumulator + // contents. + IterCb on_iter; + std::atomic* running = nullptr; + bool realtime = true; + // Drop Livox packets whose sensor timestamp (pkt->timestamp) is + // strictly less than this. Used to mimic the SDK warmup window from a + // paired live run so the algorithm starts from the same first packet + // in both modes. Comparing on sensor ts (which is identical bit-for-bit + // between live SDK delivery and pcap replay) is exact; comparing on + // wall pcap_ts would be off by SDK delivery latency. + uint64_t skip_until_ns = 0; + // When true, point and IMU packets are fed from TWO separate threads + // (each paced realtime against a shared wall anchor) instead of one + // serial feeder. This reproduces the live Livox SDK, which delivers + // point and IMU on independent threads — so on_point_cloud and + // on_imu_data actually overlap, exposing concurrency the single-feeder + // path can never hit. Requires deterministic_clock=false (wall clock). + bool dual_thread = false; + + // One parsed Livox UDP payload plus its pcap (wall) and sensor timestamps. + struct Pkt { + uint64_t pcap_ts_ns = 0; + bool is_point = false; + std::vector payload; + }; + + // Parse the whole pcap into point and IMU payload streams (applying the + // sensor-ts skip window). Returns false on a malformed/unsupported file. + bool prebuffer(std::vector& point_pkts, std::vector& imu_pkts) { + std::ifstream f(path, std::ios::binary); + if (!f) { + fprintf(stderr, "[replay] cannot open %s\n", path.c_str()); + return false; + } + uint8_t global_hdr[24]; + f.read(reinterpret_cast(global_hdr), 24); + if (!f) { + fprintf(stderr, "[replay] short read on pcap global header\n"); + return false; + } + uint32_t magic; + std::memcpy(&magic, global_hdr, 4); + const bool nanos = (magic == PCAP_MAGIC_LE_NS); + if (magic != PCAP_MAGIC_LE_US && magic != PCAP_MAGIC_LE_NS) { + fprintf(stderr, "[replay] unsupported pcap magic 0x%08x\n", magic); + return false; + } + uint32_t linktype; + std::memcpy(&linktype, global_hdr + 20, 4); + if (linktype != LINKTYPE_ETHERNET) { + fprintf(stderr, "[replay] unsupported linktype %u (need ETHERNET=1)\n", linktype); + return false; + } + uint8_t rec_hdr[16]; + std::vector buf; + while (true) { + f.read(reinterpret_cast(rec_hdr), 16); + if (!f) break; + uint32_t ts_sec, ts_sub, incl_len, orig_len; + std::memcpy(&ts_sec, rec_hdr + 0, 4); + std::memcpy(&ts_sub, rec_hdr + 4, 4); + std::memcpy(&incl_len, rec_hdr + 8, 4); + std::memcpy(&orig_len, rec_hdr + 12, 4); + (void)orig_len; + const uint64_t pcap_ts_ns = + static_cast(ts_sec) * 1'000'000'000ULL + + (nanos ? static_cast(ts_sub) : static_cast(ts_sub) * 1000ULL); + buf.resize(incl_len); + f.read(reinterpret_cast(buf.data()), incl_len); + if (!f) break; + if (buf.size() < ETH_HDR_LEN) continue; + uint16_t ethertype = (static_cast(buf[12]) << 8) | buf[13]; + if (ethertype != ETHERTYPE_IPV4) continue; + size_t ip_off = ETH_HDR_LEN; + if (buf.size() < ip_off + IP_MIN_HDR_LEN) continue; + uint8_t vihl = buf[ip_off]; + if ((vihl >> 4) != 4) continue; + int ihl = (vihl & 0x0f) * 4; + if (ihl < static_cast(IP_MIN_HDR_LEN)) continue; + if (buf[ip_off + 9] != IPPROTO_UDP) continue; + size_t udp_off = ip_off + ihl; + if (buf.size() < udp_off + UDP_HDR_LEN) continue; + uint16_t dst_port = (static_cast(buf[udp_off + 2]) << 8) | buf[udp_off + 3]; + uint16_t udp_len = (static_cast(buf[udp_off + 4]) << 8) | buf[udp_off + 5]; + size_t payload_off = udp_off + UDP_HDR_LEN; + size_t payload_end = std::min(buf.size(), static_cast(udp_off + udp_len)); + if (payload_end <= payload_off) continue; + size_t payload_len = payload_end - payload_off; + if (payload_len < LIVOX_ETH_HDR_LEN) continue; + const bool is_point = (dst_port == host_point_port); + const bool is_imu = (dst_port == host_imu_port); + if (!is_point && !is_imu) continue; + if (skip_until_ns > 0) { + auto* lp = reinterpret_cast(buf.data() + payload_off); + uint64_t pkt_ts; + std::memcpy(&pkt_ts, lp->timestamp, sizeof(uint64_t)); + if (pkt_ts < skip_until_ns) continue; + } + Pkt p; + p.pcap_ts_ns = pcap_ts_ns; + p.is_point = is_point; + p.payload.assign(buf.begin() + static_cast(payload_off), + buf.begin() + static_cast(payload_end)); + (is_point ? point_pkts : imu_pkts).emplace_back(std::move(p)); + } + return true; + } + + // Pace one pre-buffered stream against a shared wall anchor and dispatch + // each payload to its callback. Runs on its own thread in dual mode. + void feed_stream(const std::vector& pkts, const PacketCb& cb, + std::chrono::steady_clock::time_point start_wall, + uint64_t first_pcap_ts_ns) { + for (const auto& p : pkts) { + if (running != nullptr && !running->load()) return; + if (realtime) { + auto target = start_wall + + std::chrono::nanoseconds(p.pcap_ts_ns - first_pcap_ts_ns); + auto now = std::chrono::steady_clock::now(); + if (target > now) std::this_thread::sleep_until(target); + } + auto* livox_pkt = reinterpret_cast( + const_cast(p.payload.data())); + if (cb) cb(livox_pkt); + } + } + + // Two-thread feeder: reproduces the live SDK's concurrent point/IMU + // delivery. The main loop (run_main_iter) drains the accumulator as in + // live; no on_clock/on_iter (wall-clock mode only). + bool run_dual() { + std::vector point_pkts, imu_pkts; + if (!prebuffer(point_pkts, imu_pkts)) return false; + printf("[replay] dual-thread: point=%zu imu=%zu (port=%u imu=%u)\n", + point_pkts.size(), imu_pkts.size(), host_point_port, host_imu_port); + uint64_t first_ts = UINT64_MAX; + if (!point_pkts.empty()) first_ts = std::min(first_ts, point_pkts.front().pcap_ts_ns); + if (!imu_pkts.empty()) first_ts = std::min(first_ts, imu_pkts.front().pcap_ts_ns); + if (first_ts == UINT64_MAX) { + printf("[replay] dual-thread: no packets\n"); + return true; + } + auto start_wall = std::chrono::steady_clock::now(); + std::thread pt([&]() { feed_stream(point_pkts, on_point, start_wall, first_ts); }); + std::thread it([&]() { feed_stream(imu_pkts, on_imu, start_wall, first_ts); }); + pt.join(); + it.join(); + printf("[replay] dual-thread done\n"); + return true; + } + + bool run() { + if (dual_thread) { + return run_dual(); + } + std::ifstream f(path, std::ios::binary); + if (!f) { + fprintf(stderr, "[replay] cannot open %s\n", path.c_str()); + return false; + } + + uint8_t global_hdr[24]; + f.read(reinterpret_cast(global_hdr), 24); + if (!f) { + fprintf(stderr, "[replay] short read on pcap global header\n"); + return false; + } + uint32_t magic; + std::memcpy(&magic, global_hdr, 4); + const bool nanos = (magic == PCAP_MAGIC_LE_NS); + if (magic != PCAP_MAGIC_LE_US && magic != PCAP_MAGIC_LE_NS) { + fprintf(stderr, "[replay] unsupported pcap magic 0x%08x\n", magic); + return false; + } + uint32_t linktype; + std::memcpy(&linktype, global_hdr + 20, 4); + if (linktype != LINKTYPE_ETHERNET) { + fprintf(stderr, "[replay] unsupported linktype %u (need ETHERNET=1)\n", linktype); + return false; + } + + printf("[replay] reading %s (port=%u imu=%u realtime=%d)\n", + path.c_str(), host_point_port, host_imu_port, realtime ? 1 : 0); + + uint64_t first_pcap_ts_ns = 0; + std::chrono::steady_clock::time_point start_wall; + bool seeded = false; + + size_t pkts = 0, pts = 0, imu = 0, other = 0; + uint8_t rec_hdr[16]; + std::vector buf; + + while (running == nullptr || running->load()) { + f.read(reinterpret_cast(rec_hdr), 16); + if (!f) { + break; + } + + uint32_t ts_sec, ts_sub, incl_len, orig_len; + std::memcpy(&ts_sec, rec_hdr + 0, 4); + std::memcpy(&ts_sub, rec_hdr + 4, 4); + std::memcpy(&incl_len, rec_hdr + 8, 4); + std::memcpy(&orig_len, rec_hdr + 12, 4); + (void)orig_len; + + const uint64_t pcap_ts_ns = + static_cast(ts_sec) * 1'000'000'000ULL + + (nanos ? static_cast(ts_sub) : static_cast(ts_sub) * 1000ULL); + + buf.resize(incl_len); + f.read(reinterpret_cast(buf.data()), incl_len); + if (!f) { + break; + } + pkts++; + + if (buf.size() < ETH_HDR_LEN) { + continue; + } + uint16_t ethertype = (static_cast(buf[12]) << 8) | buf[13]; + if (ethertype != ETHERTYPE_IPV4) { + continue; + } + size_t ip_off = ETH_HDR_LEN; + if (buf.size() < ip_off + IP_MIN_HDR_LEN) { + continue; + } + uint8_t vihl = buf[ip_off]; + if ((vihl >> 4) != 4) { + continue; + } + int ihl = (vihl & 0x0f) * 4; + if (ihl < static_cast(IP_MIN_HDR_LEN)) { + continue; + } + if (buf[ip_off + 9] != IPPROTO_UDP) { + continue; + } + size_t udp_off = ip_off + ihl; + if (buf.size() < udp_off + UDP_HDR_LEN) { + continue; + } + uint16_t dst_port = (static_cast(buf[udp_off + 2]) << 8) | buf[udp_off + 3]; + uint16_t udp_len = (static_cast(buf[udp_off + 4]) << 8) | buf[udp_off + 5]; + size_t payload_off = udp_off + UDP_HDR_LEN; + size_t payload_end = std::min(buf.size(), static_cast(udp_off + udp_len)); + if (payload_end <= payload_off) { + continue; + } + size_t payload_len = payload_end - payload_off; + if (payload_len < LIVOX_ETH_HDR_LEN) { + continue; + } + + auto* livox_pkt = + reinterpret_cast(buf.data() + payload_off); + + // Sensor-clock skip: drop packets the live SDK wouldn't have + // seen (those before its first delivered callback) so the + // algorithm processes the same input set in both modes. + if (skip_until_ns > 0) { + uint64_t pkt_ts; + std::memcpy(&pkt_ts, livox_pkt->timestamp, sizeof(uint64_t)); + if (pkt_ts < skip_until_ns) { + continue; + } + } + + if (realtime) { + if (!seeded) { + first_pcap_ts_ns = pcap_ts_ns; + start_wall = std::chrono::steady_clock::now(); + seeded = true; + } else { + auto target = start_wall + std::chrono::nanoseconds(pcap_ts_ns - first_pcap_ts_ns); + auto now = std::chrono::steady_clock::now(); + if (target > now) { + std::this_thread::sleep_until(target); + } + } + } + + if (dst_port == host_point_port) { + if (on_point) { + on_point(livox_pkt); + } + pts++; + } else if (dst_port == host_imu_port) { + if (on_imu) { + on_imu(livox_pkt); + } + imu++; + } else { + other++; + } + // Advance the virtual clock AFTER the payload has been added to + // accumulators. Reverse order would let the main-loop thread see + // the clock advance and emit a scan that's missing this packet. + if (on_clock) { + on_clock(pcap_ts_ns); + } + + // Run one main-loop iteration synchronously so feeding and + // processing are strictly serialized in replay mode. + if (on_iter) { + on_iter(); + } + } + + printf("[replay] done: %zu pcap records (point=%zu imu=%zu other=%zu)\n", + pkts, pts, imu, other); + return true; + } +}; + +} // namespace pcap_replay diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp b/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp index ddd72eac47..d1fbe8ded4 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp @@ -1,14 +1,24 @@ // Copyright 2026 Dimensional Inc. // SPDX-License-Identifier: Apache-2.0 // -// Lightweight per-section timing for `run_main_iter`. Active only when the -// global `fastlio_debug` flag is set, so non-debug runs pay one branch per -// scope. +// Lightweight per-section timing for diagnosing where wall time goes in +// `run_main_iter`. Active only when --debug is on (i.e. the global +// `fastlio_debug` flag is true) so non-debug runs pay only a single +// branch per scope. // // Usage: +// // static timing::Section sec{"filter_cloud"}; -// { timing::Scope s(sec); /* work */ } -// timing::maybe_flush(now); // periodically +// { +// timing::Scope s(sec); +// // ...do work... +// } +// // and periodically: +// timing::maybe_flush(now); +// +// The flush prints one line per section to stderr every flush interval +// (1 second of wall clock) summarising count / mean / max / total, then +// resets the accumulators. The flush is cheap when nothing was recorded. #pragma once @@ -19,7 +29,7 @@ #include #include -#include "fast_lio_debug.hpp" +#include "fast_lio_debug.hpp" // for the global `fastlio_debug` flag namespace timing { @@ -37,6 +47,7 @@ struct Section { uint64_t prev = max_ns.load(std::memory_order_relaxed); while (ns > prev && !max_ns.compare_exchange_weak(prev, ns, std::memory_order_relaxed)) { + // prev is updated on failure by compare_exchange_weak. } } }; @@ -71,8 +82,11 @@ struct Scope { } }; -// Print one line per section to stderr every FLUSH_INTERVAL, then reset. -// Mutex serialises flushes across threads (SDK callbacks vs main loop). +// Print one summary line per section to stderr every FLUSH_INTERVAL wall +// seconds, then reset accumulators. The check is cheap: a single time +// comparison guarded by the fastlio_debug flag. The mutex serialises the +// flush between threads (replay's feeder vs live's main loop) so we +// never see torn output. inline void maybe_flush(std::chrono::steady_clock::time_point now) { if (!fastlio_debug) { return; diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index 65be5dee6f..02b670cfa5 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -12,14 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Python NativeModule wrapper for the Point-LIO + Livox Mid-360 binary. +"""Python NativeModule wrapper for the FAST-LIO2 + Livox Mid-360 binary. -Binds Livox SDK2 directly into Point-LIO for real-time LiDAR SLAM. +Binds Livox SDK2 directly into FAST-LIO-NON-ROS for real-time LiDAR SLAM. Outputs registered (world-frame) point clouds and odometry with covariance. Usage:: - from dimos.hardware.sensors.lidar.pointlio.module import PointLio + from dimos.hardware.sensors.lidar.fastlio2.module import PointLio from dimos.core.coordination.blueprints import autoconnect from dimos.core.coordination.module_coordinator import ModuleCoordinator @@ -61,8 +61,8 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.navigation.nav_stack.frames import FRAME_ODOM -from dimos.spec import perception +from dimos.navigation.nav_stack.frames import FRAME_BODY, FRAME_ODOM +from dimos.spec import mapping, perception from dimos.utils.generic import get_local_ips from dimos.utils.logging_config import setup_logger @@ -83,14 +83,11 @@ class PointLioConfig(NativeModuleConfig): # Converted to init_pose CLI arg [x, y, z, qx, qy, qz, qw] in model_post_init. mount: Pose = Pose() - # frame_id is the header frame for BOTH the point cloud and the odometry - # message (the Mid-360 sensor frame). The TF published by the module is a - # separate odom_parent_frame_id -> odom_frame_id transform. - frame_id: str = "mid360_link" - # TF publish frames (odom -> base_link): the sensor pose expressed as the - # base_link pose in the odom frame. - odom_parent_frame_id: str = FRAME_ODOM - odom_frame_id: str = "base_link" + # Frame IDs for output messages. "odom" reflects that PointLio provides + # locally-smooth, continuous odometry (no loop-closure jumps). PGO + # publishes the map→odom correction via TF. + frame_id: str = FRAME_ODOM + child_frame_id: str = FRAME_BODY # FAST-LIO internal processing rates msr_freq: float = 50.0 @@ -105,11 +102,16 @@ class PointLioConfig(NativeModuleConfig): sor_mean_k: int = 50 sor_stddev: float = 1.0 + # Global voxel map (disabled when map_freq <= 0) + map_freq: float = 0.0 + map_voxel_size: float = 0.1 + map_max_range: float = 100.0 + # FAST-LIO YAML config (relative to config/ dir, or absolute path) # C++ binary reads YAML directly via yaml-cpp config: Annotated[ Path, validate_as(...).transform(lambda p: p if p.is_absolute() else _CONFIG_DIR / p) - ] = Path("default.yaml") + ] = Path("mid360.yaml") debug: bool = False @@ -128,9 +130,24 @@ class PointLioConfig(NativeModuleConfig): # Resolved in __post_init__, passed as --config_path to the binary config_path: str | None = None + # Offline replay. When set, the C++ binary skips SDK init and feeds + # packets from this pcap into the same callbacks the SDK would. + replay_pcap: Path | None = None + # Replay-only: drop pcap records with sensor ts < this. + replay_skip_until_ns: int | None = None + # Live-only: path where the binary writes the first-callback wall_ns. + first_packet_marker: Path | None = None + # Drive scan boundaries + publish ts off the sensor packet timestamp + # for bit-reproducible offline replay. + deterministic_clock: bool = False + # Replay-only: feed point and IMU packets on two separate threads to + # mimic the live Livox SDK's concurrent delivery. Use with + # deterministic_clock=False to reproduce live thread-interleaving. + replay_dual_thread: bool = False + # init_pose is computed from mount; config is resolved to config_path init_pose: list[float] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] - cli_exclude: frozenset[str] = frozenset({"config", "mount", "odom_parent_frame_id"}) + cli_exclude: frozenset[str] = frozenset({"config", "mount"}) def model_post_init(self, __context: object) -> None: """Resolve config_path and compute init_pose from mount.""" @@ -151,15 +168,17 @@ def model_post_init(self, __context: object) -> None: ] -class PointLio(NativeModule, perception.Lidar, perception.Odometry): +class PointLio(NativeModule, perception.Lidar, perception.Odometry, mapping.GlobalPointcloud): config: PointLioConfig lidar: Out[PointCloud2] odometry: Out[Odometry] + global_map: Out[PointCloud2] @rpc def start(self) -> None: - self._validate_network() + if self.config.replay_pcap is None: + self._validate_network() super().start() self.register_disposable( Disposable(self.odometry.transport.subscribe(self._on_odom_for_tf, self.odometry)) @@ -168,8 +187,8 @@ def start(self) -> None: def _on_odom_for_tf(self, msg: Odometry) -> None: self.tf.publish( Transform( - frame_id=self.config.odom_parent_frame_id, - child_frame_id=self.config.odom_frame_id, + frame_id=FRAME_ODOM, + child_frame_id=FRAME_BODY, translation=Vector3( msg.pose.position.x, msg.pose.position.y, diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/__init__.py b/dimos/hardware/sensors/lidar/pointlio/tools/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/demo_live_test.py b/dimos/hardware/sensors/lidar/pointlio/tools/demo_live_test.py new file mode 100644 index 0000000000..4bba36ec2a --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/tools/demo_live_test.py @@ -0,0 +1,121 @@ +# 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. + +"""Live Mid-360 smoke test for the Point-LIO module. + +Runs the PointLio module against the physically-connected lidar for a short +window, mirrors odometry into a sqlite db, then prints a summary so we can +confirm the EKF stays bounded (stationary ⇒ near-origin, no satu_acc runaway). + + cd ~/repos/dimos6 && source .venv/bin/activate + python -m dimos.hardware.sensors.lidar.pointlio.tools.demo_live_test +""" + +from __future__ import annotations + +from collections.abc import AsyncIterator +import os +from pathlib import Path +import sqlite3 +import sys +import time + +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In +from dimos.msgs.nav_msgs.Odometry import Odometry + +RUN_SEC = float(os.environ.get("RUN_SEC", "25.0")) +DB_PATH = Path("/tmp/pointlio_live.db") + +_EPS = 1e-9 + + +class RecConfig(ModuleConfig): + db_path: str = "" + + +class Rec(Module): + config: RecConfig + pointlio_odometry: In[Odometry] + _last_o: float = 0.0 + + async def main(self) -> AsyncIterator[None]: + from dimos.memory2.store.sqlite import SqliteStore + + self._store = SqliteStore(path=self.config.db_path) + self._os = self._store.stream("pointlio_odometry", Odometry) + yield + self._store.stop() + + async def handle_pointlio_odometry(self, v: Odometry) -> None: + ts = max(getattr(v, "ts", None) or time.time(), self._last_o + _EPS) + self._last_o = ts + pose = getattr(v, "pose", None) + pose_inner = getattr(pose, "pose", None) if pose is not None else None + self._os.append(v, ts=ts, pose=pose_inner) + + +def main() -> int: + if DB_PATH.exists(): + DB_PATH.unlink() + + from dimos.core.coordination.blueprints import autoconnect + from dimos.core.coordination.module_coordinator import ModuleCoordinator + from dimos.hardware.sensors.lidar.pointlio.module import PointLio + + bp = autoconnect( + PointLio.blueprint(frame_id="world", map_freq=-1, debug=False).remappings( + [(PointLio, "odometry", "pointlio_odometry")] + ), + Rec.blueprint(db_path=str(DB_PATH)), + ).global_config(n_workers=2, robot_model="mid360_pointlio_live_test") + coord = ModuleCoordinator.build(bp) + + t0 = time.time() + try: + while time.time() - t0 < RUN_SEC: + time.sleep(1.0) + finally: + coord.stop() + + if not DB_PATH.exists(): + print("[live_test] NO DB — module never produced odometry", file=sys.stderr) + return 1 + con = sqlite3.connect(f"file:{DB_PATH}?mode=ro", uri=True) + rows = list(con.execute("SELECT ts,pose_x,pose_y,pose_z FROM pointlio_odometry ORDER BY ts")) + con.close() + print(f"[live_test] odometry rows: {len(rows)}") + if not rows: + print("[live_test] NO ROWS — lidar likely not streaming", file=sys.stderr) + return 1 + import math + + t0r = rows[0][0] + xs = [r[1] for r in rows] + ys = [r[2] for r in rows] + zs = [r[3] for r in rows] + dmax = max(math.sqrt(x * x + y * y + z * z) for x, y, z in zip(xs, ys, zs, strict=False)) + print( + f"[live_test] dur={rows[-1][0] - t0r:.1f}s rate≈{len(rows) / max(rows[-1][0] - t0r, 1e-3):.1f}Hz" + ) + print( + f"[live_test] x=({min(xs):.3f},{max(xs):.3f}) y=({min(ys):.3f},{max(ys):.3f}) z=({min(zs):.3f},{max(zs):.3f})" + ) + print(f"[live_test] max |pos| from origin: {dmax:.3f} m") + print(f"[live_test] final pos: ({xs[-1]:.3f},{ys[-1]:.3f},{zs[-1]:.3f})") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py new file mode 100644 index 0000000000..1313e3b8c1 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py @@ -0,0 +1,271 @@ +# 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. + +"""Run Point-LIO over a .pcap and append the odometry into an existing .db. + +Given a Livox Mid-360 pcap capture and a memory2 SQLite database, this streams +the pcap through the Point-LIO native module (deterministic clock, single feeder +-> never loads the whole pcap into memory) and writes a ``pointlio_odometry`` +stream into the database at the native publish rate (~30 Hz). + +Timing conversion +----------------- +Point-LIO's deterministic output timestamps are in *sensor-boot seconds* (the +Livox packet clock, small values like 1588.x). The target db may use a different +clock for its existing streams: + +* db already on the sensor clock (e.g. a fastlio replay db, min ts < 1e8): + offset 0 -- both replay the same pcap packet clock, so they line up exactly. +* db on wall-clock unix time (min ts > 1e9): start-align by shifting Point-LIO's + first odom ts onto the db's earliest existing ts. +* db has no existing timestamped rows: offset 0. + +Pass ``--time-offset`` to override the auto choice. + +Usage (from the dimos6 venv):: + + source .venv/bin/activate + python -m dimos.hardware.sensors.lidar.pointlio.tools.pcap_to_db \ + --pcap /path/to/capture.pcap --db /path/to/memory.db +""" + +from __future__ import annotations + +import argparse +from collections.abc import AsyncIterator +import math +from pathlib import Path +import sqlite3 +import sys +import time + +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In +from dimos.msgs.nav_msgs.Odometry import Odometry + +# Below this the db's existing timestamps are sensor-boot seconds, not unix time. +_SENSOR_CLOCK_MAX = 1e8 +# Strictly-increasing tie-breaker so two odom samples never collide on ts. +_EPS = 1e-9 +# Poll the db on this cadence while the replay drains the pcap. +_POLL_SEC = 1.0 +# Stop after the odom stream has been stagnant this long (pcap fully drained). +_STAGNANT_SEC = 4.0 + + +class RecConfig(ModuleConfig): + """Configures the recorder with the target db and timing conversion.""" + + db_path: str = "" + # Earliest existing ts in the db, or -1.0 if the db has no timestamped rows. + ref_start_ts: float = -1.0 + # Explicit offset override; NaN means auto-derive from ref_start_ts. + time_offset: float = float("nan") + + +class Rec(Module): + """Append Point-LIO odometry into an existing SQLite db with ts conversion.""" + + config: RecConfig + pointlio_odometry: In[Odometry] + _offset: float | None = None + _last_ts: float = 0.0 + _count: int = 0 + + async def main(self) -> AsyncIterator[None]: + from dimos.memory2.store.sqlite import SqliteStore + + self._store = SqliteStore(path=self.config.db_path) + self._os = self._store.stream("pointlio_odometry", Odometry) + yield + self._store.stop() + + def _resolve_offset(self, first_ts: float) -> float: + override = self.config.time_offset + if not math.isnan(override): + return override + ref = self.config.ref_start_ts + if ref < 0.0 or ref < _SENSOR_CLOCK_MAX: + # Empty db, or db already on the sensor clock -> exact alignment. + return 0.0 + # db on wall-clock time -> start-align Point-LIO onto the db's earliest ts. + return ref - first_ts + + async def handle_pointlio_odometry(self, v: Odometry) -> None: + raw_ts = getattr(v, "ts", None) or time.time() + if self._offset is None: + self._offset = self._resolve_offset(raw_ts) + ts = max(raw_ts + self._offset, self._last_ts + _EPS) + self._last_ts = ts + pose = getattr(v, "pose", None) + pose_inner = getattr(pose, "pose", None) if pose is not None else None + self._os.append(v, ts=ts, pose=pose_inner) + self._count += 1 + + +def _db_ref_start_ts(db_path: Path) -> float: + """Min ts across the db's existing streams, or -1.0 if none/absent.""" + if not db_path.exists(): + return -1.0 + con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) + try: + tables = [ + row[0] + for row in con.execute("SELECT name FROM sqlite_master WHERE type='table'").fetchall() + ] + best: float | None = None + for table in tables: + if table.startswith("_") or table.startswith("sqlite_"): + continue + cols = [c[1] for c in con.execute(f"PRAGMA table_info('{table}')").fetchall()] + if "ts" not in cols: + continue + row = con.execute(f"SELECT MIN(ts) FROM '{table}'").fetchone() + if row and row[0] is not None: + best = row[0] if best is None else min(best, row[0]) + return best if best is not None else -1.0 + finally: + con.close() + + +def _odom_stats(db_path: Path) -> tuple[int, float, float]: + """(count, min_ts, max_ts) for the pointlio_odometry table; zeros if absent.""" + if not db_path.exists(): + return 0, 0.0, 0.0 + con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) + try: + try: + row = con.execute("SELECT COUNT(*), MIN(ts), MAX(ts) FROM pointlio_odometry").fetchone() + except sqlite3.OperationalError: + return 0, 0.0, 0.0 + cnt = row[0] or 0 + return cnt, row[1] or 0.0, row[2] or 0.0 + finally: + con.close() + + +def _run(args: argparse.Namespace) -> int: + pcap_path = Path(args.pcap).expanduser().resolve() + db_path = Path(args.db).expanduser().resolve() + if not pcap_path.exists(): + print(f"[pcap_to_db] missing pcap: {pcap_path}", file=sys.stderr) + return 2 + if args.max_sensor_sec < 0: + print("[pcap_to_db] --max-sensor-sec must be >= 0", file=sys.stderr) + return 2 + + from dimos.core.coordination.blueprints import autoconnect + from dimos.core.coordination.module_coordinator import ModuleCoordinator + from dimos.hardware.sensors.lidar.pointlio.module import PointLio + + ref_start_ts = _db_ref_start_ts(db_path) + time_offset = float("nan") if args.time_offset is None else args.time_offset + if not math.isnan(time_offset): + offset_desc = f"explicit {time_offset:+.3f}s" + elif ref_start_ts < 0.0: + offset_desc = "auto: db empty -> 0" + elif ref_start_ts < _SENSOR_CLOCK_MAX: + offset_desc = f"auto: db sensor-clock (R0={ref_start_ts:.2f}) -> 0" + else: + offset_desc = f"auto: db wall-clock (R0={ref_start_ts:.2f}) -> start-align" + print( + f"[pcap_to_db] pcap={pcap_path.name} db={db_path.name} " + f"odom_freq={args.odom_freq}Hz offset={offset_desc}", + flush=True, + ) + + blueprint = autoconnect( + PointLio.blueprint( + frame_id="world", + map_freq=-1, + odom_freq=args.odom_freq, + replay_pcap=pcap_path, + deterministic_clock=True, + replay_dual_thread=False, + debug=False, + ).remappings([(PointLio, "odometry", "pointlio_odometry")]), + Rec.blueprint( + db_path=str(db_path), + ref_start_ts=ref_start_ts, + time_offset=time_offset, + ), + ).global_config(n_workers=4, robot_model="mid360_pointlio_pcap_to_db") + coord = ModuleCoordinator.build(blueprint) + + t0 = time.time() + last_max = 0.0 + first_max: float | None = None + stagnant_since: float | None = None + try: + while True: + time.sleep(_POLL_SEC) + cnt, min_ts, max_ts = _odom_stats(db_path) + if cnt == 0: + continue + if first_max is None: + first_max = min_ts + if args.max_sensor_sec > 0 and (max_ts - first_max) >= args.max_sensor_sec: + print( + f"[pcap_to_db] reached --max-sensor-sec={args.max_sensor_sec:.1f}s", + flush=True, + ) + break + if max_ts == last_max: + if stagnant_since is None: + stagnant_since = time.time() + elif time.time() - stagnant_since > _STAGNANT_SEC: + break + else: + last_max = max_ts + stagnant_since = None + finally: + coord.stop() + + cnt, min_ts, max_ts = _odom_stats(db_path) + span = max_ts - min_ts + print( + f"[pcap_to_db] done rows={cnt} ts=[{min_ts:.3f}, {max_ts:.3f}] " + f"span={span:.1f}s wall={time.time() - t0:.1f}s", + flush=True, + ) + return 0 + + +def main(argv: list[str]) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--pcap", required=True, help="Livox Mid-360 pcap capture") + parser.add_argument("--db", required=True, help="target memory2 SQLite db (appended to)") + parser.add_argument( + "--odom-freq", + type=float, + default=30.0, + help="Point-LIO odometry publish rate in Hz (default 30)", + ) + parser.add_argument( + "--max-sensor-sec", + type=float, + default=0.0, + help="stop after this many seconds of sensor time (0 = whole pcap)", + ) + parser.add_argument( + "--time-offset", + type=float, + default=None, + help="seconds added to every odom ts; omit to auto-derive from the db clock", + ) + return _run(parser.parse_args(argv)) + + +if __name__ == "__main__": + raise SystemExit(main(sys.argv[1:])) diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/replay_ruwik2_pt3.py b/dimos/hardware/sensors/lidar/pointlio/tools/replay_ruwik2_pt3.py new file mode 100644 index 0000000000..bf20ec36e1 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/tools/replay_ruwik2_pt3.py @@ -0,0 +1,261 @@ +# 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. + +"""Replay the ruwik2_pt3 5/29 pcap through the Point-LIO native module. + +Mirror of the fastlio2 replay_ruwik2_pt3 harness, but driving the PointLio +module. Same orchestrator+worker shape, same pcap, same deterministic clock. +Captures PointLio odometry into a per-attempt sqlite db so the trajectory can +be compared against the fastlio replay (which diverges to ~2257 m/s on this +same wire data). + +Run from the dimos6 venv: + + cd ~/repos/dimos6 + source .venv/bin/activate + python -m dimos.hardware.sensors.lidar.pointlio.tools.replay_ruwik2_pt3 +""" + +from __future__ import annotations + +from collections.abc import AsyncIterator +import json +import os +from pathlib import Path +import subprocess +import sys +import time + +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In +from dimos.msgs.nav_msgs.Odometry import Odometry + +PCAP_PATH = Path( + os.environ.get( + "REPLAY_PCAP_PATH", + "/home/dimos/repos/dimos/fastlio2_pcap/mid360_20260529_225346.pcap", + ) +) + +RUNS_ROOT = Path("/home/dimos/repos/dimos6/pointlio_ruwik2_pt3_replays") + +_ATTEMPT_DIR_ENV = "_REPLAY_POINTLIO_RUWIK2_PT3_ATTEMPT_DIR" + +MAX_WALL_SEC = 480.0 + +REPLAY_MAX_SENSOR_SEC = float(os.environ.get("REPLAY_MAX_SENSOR_SEC", "60.0")) + +# deterministic_clock=true cuts scan boundaries on the sensor virtual clock read +# atomically under g_pc_mutex, which removes the feeder/main scan-composition race +# and makes replay reproducibly bounded. Set REPLAY_DETERMINISTIC_CLOCK=0 to cut +# scans on wall time (realtime feeder) — that restores the live threading race and +# is the only way replay reproduces the live divergence offline. +REPLAY_DETERMINISTIC_CLOCK = os.environ.get("REPLAY_DETERMINISTIC_CLOCK", "1") != "0" + +# Feed point + IMU on two separate threads (mimics the live Livox SDK's +# concurrent delivery). Set REPLAY_DUAL_THREAD=1 with REPLAY_DETERMINISTIC_CLOCK=0 +# to reproduce live thread-interleaving offline. +REPLAY_DUAL_THREAD = os.environ.get("REPLAY_DUAL_THREAD", "0") == "1" + + +def _next_attempt_dir() -> Path: + RUNS_ROOT.mkdir(parents=True, exist_ok=True) + existing = sorted(p.name for p in RUNS_ROOT.iterdir() if p.name.startswith("attempt_")) + n = 0 + for name in existing: + try: + n = max(n, int(name.split("_", 1)[1]) + 1) + except ValueError: + continue + attempt = RUNS_ROOT / f"attempt_{n:03d}" + attempt.mkdir() + return attempt + + +def _commit_hash() -> str: + try: + return subprocess.check_output( + ["git", "-C", str(Path(__file__).resolve().parents[6]), "rev-parse", "HEAD"], + text=True, + ).strip() + except subprocess.CalledProcessError: + return "unknown" + + +class RecConfig(ModuleConfig): + """Configures Rec with the per-attempt sqlite db path.""" + + db_path: str = "" + + +_EPS = 1e-9 + + +class Rec(Module): + """Mirror replay PointLio odometry into a SqliteStore.""" + + config: RecConfig + pointlio_odometry: In[Odometry] + _last_o: float = 0.0 + + async def main(self) -> AsyncIterator[None]: + from dimos.memory2.store.sqlite import SqliteStore + + self._store = SqliteStore(path=self.config.db_path) + self._os = self._store.stream("pointlio_odometry", Odometry) + yield + self._store.stop() + + async def handle_pointlio_odometry(self, v: Odometry) -> None: + ts = max(getattr(v, "ts", None) or time.time(), self._last_o + _EPS) + self._last_o = ts + pose = getattr(v, "pose", None) + pose_inner = getattr(pose, "pose", None) if pose is not None else None + self._os.append(v, ts=ts, pose=pose_inner) + + +def _orchestrate() -> int: + if not PCAP_PATH.exists(): + print(f"[replay_pointlio] missing pcap: {PCAP_PATH}", file=sys.stderr) + return 2 + attempt_dir = _next_attempt_dir() + stdout_path = attempt_dir / "stdout.txt" + stderr_path = attempt_dir / "stderr.txt" + meta = { + "attempt_dir": str(attempt_dir), + "pcap_path": str(PCAP_PATH), + "commit": _commit_hash(), + "started_at": time.time(), + } + print(f"[replay_pointlio] attempt {attempt_dir.name} commit {meta['commit'][:8]}", flush=True) + t0 = time.time() + env = {**os.environ, _ATTEMPT_DIR_ENV: str(attempt_dir)} + with stdout_path.open("w") as out, stderr_path.open("w") as err: + rc = subprocess.run( + [sys.executable, "-m", __spec__.name, "--worker"], + stdout=out, + stderr=err, + env=env, + check=False, + ).returncode + meta["finished_at"] = time.time() + meta["wall_sec"] = meta["finished_at"] - t0 + meta["worker_rc"] = rc + (attempt_dir / "meta.json").write_text(json.dumps(meta, indent=2)) + print( + f"[replay_pointlio] done attempt {attempt_dir.name} rc={rc} wall={meta['wall_sec']:.1f}s", + flush=True, + ) + return rc + + +def _worker() -> int: + attempt_dir = Path(os.environ[_ATTEMPT_DIR_ENV]) + db_path = attempt_dir / "pointlio.db" + if db_path.exists(): + db_path.unlink() + db_path_str = str(db_path) + + from dimos.core.coordination.blueprints import autoconnect + from dimos.core.coordination.module_coordinator import ModuleCoordinator + from dimos.hardware.sensors.lidar.pointlio.module import PointLio + + subprocess.run(["kd"], check=False) + time.sleep(1.0) + + bp = autoconnect( + PointLio.blueprint( + frame_id="world", + map_freq=-1, + replay_pcap=PCAP_PATH, + deterministic_clock=REPLAY_DETERMINISTIC_CLOCK, + replay_dual_thread=REPLAY_DUAL_THREAD, + debug=False, + ).remappings( + [ + (PointLio, "odometry", "pointlio_odometry"), + ] + ), + Rec.blueprint(db_path=db_path_str), + ).global_config(n_workers=4, robot_model="mid360_pointlio_replay_ruwik2") + coord = ModuleCoordinator.build(bp) + + import sqlite3 + + t0 = time.time() + last_ts_seen = 0.0 + first_ts_seen = 0.0 + stagnant_since: float | None = None + saw_first_row = False + try: + while time.time() - t0 < MAX_WALL_SEC: + time.sleep(1.0) + if not db_path.exists(): + continue + try: + con = sqlite3.connect(f"file:{db_path_str}?mode=ro", uri=True, timeout=0.5) + row = con.execute( + "SELECT MIN(ts), MAX(ts), COUNT(*) FROM pointlio_odometry" + ).fetchone() + con.close() + except Exception: + continue + first_ts = row[0] if row and row[0] else 0.0 + last_ts = row[1] if row and row[1] else 0.0 + cnt = row[2] if row else 0 + if cnt > 0: + saw_first_row = True + if first_ts_seen == 0.0: + first_ts_seen = first_ts + if not saw_first_row: + continue + if ( + REPLAY_MAX_SENSOR_SEC > 0 + and first_ts_seen > 0 + and (last_ts - first_ts_seen) >= REPLAY_MAX_SENSOR_SEC + ): + print( + f"[replay_pointlio.worker] reached REPLAY_MAX_SENSOR_SEC=" + f"{REPLAY_MAX_SENSOR_SEC:.1f} s — stopping", + flush=True, + ) + break + if last_ts == last_ts_seen: + if stagnant_since is None: + stagnant_since = time.time() + elif time.time() - stagnant_since > 3.0: + break + else: + last_ts_seen = last_ts + stagnant_since = None + finally: + coord.stop() + + if db_path.exists(): + size_mb = db_path.stat().st_size / 1e6 + print( + f"[replay_pointlio.worker] db_size={size_mb:.2f}MB wall={time.time() - t0:.1f}s", + flush=True, + ) + return 0 + + +def main(argv: list[str]) -> int: + if len(argv) >= 2 and argv[1] == "--worker": + return _worker() + return _orchestrate() + + +if __name__ == "__main__": + raise SystemExit(main(sys.argv)) From 8d518aa5bbb29992f2cc4cf73f45083cccee3ece Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 4 Jun 2026 21:05:52 -0700 Subject: [PATCH 032/185] fixup the aggregation steps --- dimos/mapping/recording/multi_map_anchor.py | 304 ++++++++++++++++++ dimos/mapping/recording/utils/build_rrd.py | 3 + dimos/mapping/recording/utils/gtsam_gt.py | 63 +++- .../recording/utils/lidar_loop_closure.py | 299 +++++++++++++++++ .../mapping/recording/utils/lidar_reanchor.py | 2 +- dimos/mapping/recording/utils/post_process.py | 22 +- 6 files changed, 681 insertions(+), 12 deletions(-) create mode 100644 dimos/mapping/recording/multi_map_anchor.py create mode 100644 dimos/mapping/recording/utils/lidar_loop_closure.py diff --git a/dimos/mapping/recording/multi_map_anchor.py b/dimos/mapping/recording/multi_map_anchor.py new file mode 100644 index 0000000000..28e9bbd066 --- /dev/null +++ b/dimos/mapping/recording/multi_map_anchor.py @@ -0,0 +1,304 @@ +#!/usr/bin/env python +# 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. + +"""Anchor one recording's trajectory onto another via shared AprilTags. + +Takes two recording dirs of the same route -- a stable mid360_realsense map (the +anchor) and a shakier go2_mid360 map (the target). It: + + 1. solves the anchor's drift-corrected trajectory (AprilTag landmark SLAM), + ignoring the dog-mounted tag, and keeps the optimized world poses of the + static environment tags as a tag map (this defines the world frame), + 2. solves the target the same way but pins the *shared* tags to the anchor's + tag map, so the target lands in the anchor's world frame, drift-corrected, + 3. writes one .rrd overlaying the original FAST-LIO odom and the corrected + odom from both recordings (and, by default, the re-anchored lidar maps so + you can see whether the two corrected maps line up). + + uv run --no-sync python \ + dimos/mapping/recording/multi_map_anchor.py DIR_A DIR_B [--out PATH] + +DIR_A/DIR_B may be in any order; the mid360_realsense recording (detected by its +`realsense_camera_info` stream) is always used as the anchor. +""" + +from __future__ import annotations + +import argparse +from pathlib import Path +from typing import cast + +import numpy as np +import rerun as rr +import rerun.blueprint as rrb +from scipy.spatial.transform import Rotation + +from dimos.mapping.recording.go2_mid360.post_process import load_camera as load_go2_camera +from dimos.mapping.recording.mid360_realsense.post_process import ( + load_camera as load_realsense_camera, +) +from dimos.mapping.recording.utils.apriltags import detect_apriltags +from dimos.mapping.recording.utils.build_rrd import _log_map, _log_path_gradient +from dimos.mapping.recording.utils.gtsam_gt import build_gtsam_gt, write_gtsam_odom +from dimos.mapping.recording.utils.lidar_reanchor import reanchor_stream +from dimos.mapping.recording.utils.post_process import CameraParams +from dimos.memory2.store.sqlite import SqliteStore + +TagMap = dict[int, list[float]] +Trajectory = list[tuple[float, list[float]]] +MIN_SHARED_TAGS = 3 # rigid SE3 needs >=3 non-collinear correspondences + +DB_NAME = "mem2.db" +DOG_TAG_ID = 17 # mounted on the robot dog -> not a static landmark, ignored +GTSAM_STREAM = "gtsam_odom" +FASTLIO_LIDAR = "fastlio_lidar" +FASTLIO_ODOM = "fastlio_odometry" +LOOP_LIDAR = "livox_lidar" # raw sensor-frame cloud (loop closure needs sensor, not world, frame) +CORRECTED_LIDAR = "fastlio_lidar_corrected" +REALSENSE_INFO_STREAM = "realsense_camera_info" + + +def _resolve_db(arg: str) -> Path: + path = Path(arg) + if path.name == DB_NAME: + return path + if (path / DB_NAME).exists(): + return path / DB_NAME + raise SystemExit(f"no {DB_NAME} found at {arg}") + + +def _is_realsense(db: Path) -> bool: + # recordings drop a `type.` marker file; prefer it over opening the + # (multi-GB) db, falling back to a stream check if absent. + if (db.parent / "type.mid360_realsense").exists(): + return True + if (db.parent / "type.go2_mid360").exists(): + return False + with SqliteStore(path=str(db)) as store: + return REALSENSE_INFO_STREAM in store.list_streams() + + +def _load_camera(db: Path) -> CameraParams: + return load_realsense_camera(db) if _is_realsense(db) else load_go2_camera(db) + + +def _mat_from_pose7(pose7: list[float]) -> np.ndarray: + matrix = np.eye(4) + matrix[:3, :3] = Rotation.from_quat(pose7[3:7]).as_matrix() + matrix[:3, 3] = pose7[:3] + return matrix + + +def _pose7_from_mat(matrix: np.ndarray) -> list[float]: + quaternion = Rotation.from_matrix(matrix[:3, :3]).as_quat() + return [*matrix[:3, 3].tolist(), *quaternion.tolist()] + + +def _rigid_align(source_points: np.ndarray, target_points: np.ndarray) -> np.ndarray: + """Kabsch: the rigid SE3 (4x4) that best maps `source_points` onto + `target_points` (no scale). Position-only — tag orientation is untrusted.""" + source_centroid = source_points.mean(axis=0) + target_centroid = target_points.mean(axis=0) + covariance = (source_points - source_centroid).T @ (target_points - target_centroid) + u_matrix, _singular, vt_matrix = np.linalg.svd(covariance) + reflection = np.diag([1.0, 1.0, np.sign(np.linalg.det(vt_matrix.T @ u_matrix.T))]) + rotation = vt_matrix.T @ reflection @ u_matrix.T + transform = np.eye(4) + transform[:3, :3] = rotation + transform[:3, 3] = target_centroid - rotation @ source_centroid + return transform + + +def _align_to_anchor(target_map: TagMap, anchor_map: TagMap) -> np.ndarray: + """Rigid SE3 placing the target's frame into the anchor's frame, fit from the + tags both maps share.""" + shared = sorted(set(target_map) & set(anchor_map)) + if len(shared) < MIN_SHARED_TAGS: + raise SystemExit( + f"only {len(shared)} shared tags {shared} (need >={MIN_SHARED_TAGS}) — cannot anchor" + ) + source_points = np.array([target_map[marker_id][:3] for marker_id in shared]) + target_points = np.array([anchor_map[marker_id][:3] for marker_id in shared]) + transform = _rigid_align(source_points, target_points) + residuals = np.linalg.norm( + (source_points @ transform[:3, :3].T + transform[:3, 3]) - target_points, axis=1 + ) + print( + f" align: {len(shared)} shared tags {shared} | residual " + f"mean {residuals.mean():.3f} m, max {residuals.max():.3f} m" + ) + return transform + + +def _solve( + db: Path, + *, + image_stream: str, + marker_length: float, + dictionary: str, + add_loop_closures: bool, +) -> tuple[Trajectory, TagMap]: + """Detect tags and run the GTSAM solve (ignoring the dog tag), each map in its + own frame. Returns its corrected trajectory and optimized static tag map.""" + intrinsics, distortion, optical_in_base, _resolution = _load_camera(db) + with SqliteStore(path=str(db)) as store: + detections = detect_apriltags( + store, intrinsics, distortion, image_stream, "april_tags", marker_length, dictionary + ) + if not detections: + raise SystemExit(f"no AprilTags detected in {db} -- cannot anchor") + + return cast( + "tuple[Trajectory, TagMap]", + build_gtsam_gt( + str(db), + detections, + optical_in_base, + exclude_marker_ids=(DOG_TAG_ID,), + pose_stream=FASTLIO_ODOM, + loop_lidar_stream=LOOP_LIDAR, + add_loop_closures=add_loop_closures, + return_landmarks=True, + ), + ) + + +def _write_corrected(db: Path, trajectory: Trajectory) -> None: + """Write `gtsam_odom` (+ .tum) and re-anchor the fastlio lidar onto it.""" + with SqliteStore(path=str(db)) as store: + write_gtsam_odom(store, trajectory, GTSAM_STREAM, db.parent / "gtsam_odom.tum") + stream_names = store.list_streams() + if FASTLIO_LIDAR in stream_names and FASTLIO_ODOM in stream_names: + try: + reanchor_stream( + store, + str(db), + lidar_stream=FASTLIO_LIDAR, + odom_stream=FASTLIO_ODOM, + gtsam_stream=GTSAM_STREAM, + out_stream=CORRECTED_LIDAR, + ) + except Exception as error: + print(f" re-anchor {CORRECTED_LIDAR} failed: {error}") + + +def build_combined_rrd( + out_path: str, anchor_db: Path, target_db: Path, tag_map: TagMap, *, with_maps: bool +) -> None: + """One Rerun recording overlaying both recordings in the shared world frame: + raw FAST-LIO odom + corrected odom for each, the anchor tag map, and + (optionally) the re-anchored lidar maps.""" + rr.init("multi_map_anchor", recording_id=str(out_path)) + rr.save(str(out_path)) + + map_entities = ["anchor/map", "target/map"] + hide = {f"/world/{entity}": rrb.EntityBehavior(visible=False) for entity in map_entities} + rr.send_blueprint( + rrb.Blueprint( + rrb.Spatial3DView(origin="/world", name="3D", overrides=hide), + rrb.BlueprintPanel(state=rrb.PanelState.Expanded), + rrb.TimePanel(state=rrb.PanelState.Collapsed), + ) + ) + + # raw odom dim, corrected odom bright; anchor in greens/cyan, target in warms. + _log_path_gradient(str(anchor_db), FASTLIO_ODOM, "world/anchor/fastlio_raw", (0, 110, 150)) + _log_path_gradient(str(anchor_db), GTSAM_STREAM, "world/anchor/corrected", (0, 220, 120)) + _log_path_gradient(str(target_db), FASTLIO_ODOM, "world/target/fastlio_raw", (150, 110, 0)) + _log_path_gradient(str(target_db), GTSAM_STREAM, "world/target/corrected", (255, 150, 60)) + + for marker_id, pose7 in sorted(tag_map.items()): + rr.log( + f"world/tags/marker_{marker_id}", + rr.Points3D( + [pose7[:3]], + colors=(255, 220, 60), + radii=0.25, + labels=[f"tag {marker_id}"], + show_labels=True, + ), + static=True, + ) + + if with_maps: + with SqliteStore(path=str(anchor_db)) as store: + _log_map(store, CORRECTED_LIDAR, "world/anchor/map", 0.1, (0, 180, 170)) + with SqliteStore(path=str(target_db)) as store: + _log_map(store, CORRECTED_LIDAR, "world/target/map", 0.1, (240, 160, 40)) + + print(f" wrote {out_path}") + + +def main() -> None: + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument("dir_a", help="a recording dir or mem2.db") + parser.add_argument("dir_b", help="a recording dir or mem2.db") + parser.add_argument("--out", default=None, help="output .rrd (default: next to the anchor)") + parser.add_argument("--image-stream", default="color_image") + parser.add_argument("--marker-length", type=float, default=0.10) + parser.add_argument("--dictionary", default="DICT_APRILTAG_36h11") + parser.add_argument( + "--loop", + action="store_true", + help="add lidar loop-closure constraints (off by default: with fastlio_odometry as the " + "pose chain the livox_lidar revisits are unreliable and currently degrade the solve)", + ) + parser.add_argument("--no-maps", action="store_true", help="skip the re-anchored lidar maps") + args = parser.parse_args() + + db_a, db_b = _resolve_db(args.dir_a), _resolve_db(args.dir_b) + if _is_realsense(db_a): + anchor_db, target_db = db_a, db_b + elif _is_realsense(db_b): + anchor_db, target_db = db_b, db_a + else: + raise SystemExit("neither recording is a mid360_realsense (no realsense_camera_info)") + print(f"anchor (realsense): {anchor_db.parent}") + print(f"target (go2): {target_db.parent}") + + common = { + "image_stream": args.image_stream, + "marker_length": args.marker_length, + "dictionary": args.dictionary, + "add_loop_closures": args.loop, + } + + # The anchor (realsense) defines the world frame: solve it, write it as-is. + print(">> solving anchor (defines the world frame)") + anchor_trajectory, anchor_map = _solve(anchor_db, **common) + _write_corrected(anchor_db, anchor_trajectory) + + # The target (go2) is solved in its own frame, then rigidly placed into the + # anchor frame via the tags both maps share. + print(">> solving target") + target_trajectory, target_map = _solve(target_db, **common) + print(">> aligning target onto the anchor frame") + transform = _align_to_anchor(target_map, anchor_map) + target_trajectory = [ + (timestamp, _pose7_from_mat(transform @ _mat_from_pose7(pose7))) + for timestamp, pose7 in target_trajectory + ] + _write_corrected(target_db, target_trajectory) + + out_path = args.out or str(anchor_db.parent / "multi_map_anchor.rrd") + print(">> building combined .rrd") + build_combined_rrd(out_path, anchor_db, target_db, anchor_map, with_maps=not args.no_maps) + print("done") + + +if __name__ == "__main__": + main() diff --git a/dimos/mapping/recording/utils/build_rrd.py b/dimos/mapping/recording/utils/build_rrd.py index ddccfdce33..b80680ad35 100644 --- a/dimos/mapping/recording/utils/build_rrd.py +++ b/dimos/mapping/recording/utils/build_rrd.py @@ -103,6 +103,7 @@ def _shaded(pts: np.ndarray, base) -> np.ndarray: def _log_frames(store, stream, entity, stride, voxel, base): if stream not in store.list_streams(): return + print(f" rrd: logging {stream} frames -> {entity} (stride {stride}) ...", flush=True) n = 0 for k, obs in enumerate(store.stream(stream, PointCloud2)): if k % stride: @@ -119,6 +120,7 @@ def _log_frames(store, stream, entity, stride, voxel, base): def _log_map(store, stream, entity, voxel, base): if stream not in store.list_streams(): return + print(f" rrd: aggregating {stream} map -> {entity} (this is the slow one) ...", flush=True) chunks = [ c for c in (obs.data.points_f32() for obs in store.stream(stream, PointCloud2)) if len(c) ] @@ -216,6 +218,7 @@ def _log_apriltags(store, db_path, cam_xform, intrinsics, resolution, max_views_ a pinhole frustum at the camera pose (3D only — see the blueprint).""" if "april_tags" not in store.list_streams(): return + print(" rrd: placing april_tags in 3D ...", flush=True) connection = sqlite3.connect(db_path) traj_stream = "gtsam_odom" if _has_rows(connection, "gtsam_odom") else "fastlio_odometry" pose_rows = connection.execute( diff --git a/dimos/mapping/recording/utils/gtsam_gt.py b/dimos/mapping/recording/utils/gtsam_gt.py index 1de1b5eb77..649d8a7076 100644 --- a/dimos/mapping/recording/utils/gtsam_gt.py +++ b/dimos/mapping/recording/utils/gtsam_gt.py @@ -27,6 +27,7 @@ import numpy as np +from dimos.mapping.recording.utils.lidar_loop_closure import find_loop_closures from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -71,7 +72,7 @@ def pick_pose_stream(connection) -> str: continue if populated > 0: return name - raise SystemExit(f"no odom stream with populated pose columns among {candidates}") + raise ValueError(f"no odom stream with populated pose columns among {candidates}") def build_gtsam_gt( @@ -85,14 +86,36 @@ def build_gtsam_gt( tag_rot_sig=1.0, tag_trans_sig=0.1, tag_huber=0.5, + add_loop_closures=True, + loop_lidar_stream="livox_lidar", + loop_rot_sig=0.01, + loop_trans_sig=1.0, + loop_huber=1.0, + exclude_marker_ids=(), + pose_stream=None, + return_landmarks=False, ): - """Landmark-SLAM the odom chain + AprilTag landmarks. Returns [(ts, pose7), ...].""" + """Landmark-SLAM the odom chain + AprilTag landmarks + lidar loop closures. + + Per-DOF weighting plays each source to its strength: AprilTags trust POSITION + (tight tag_trans_sig) and ignore their own ORIENTATION (loose tag_rot_sig); + lidar loop closures trust ORIENTATION (tight loop_rot_sig, fixes accumulated + pitch/yaw drift) and stay out of translation (loose loop_trans_sig). + + `exclude_marker_ids` drops those tags entirely (e.g. a tag mounted on a moving + robot is not a static landmark). `pose_stream` forces which odom stream is the + pose chain (default: auto-pick) — set it to match the stream the lidar is + re-anchored through, so trajectory and clouds share a frame. + + Returns [(ts, pose7), ...], or ([(ts, pose7), ...], {marker_id: pose7_world}) + of the optimized static-tag world poses when `return_landmarks` is set.""" import gtsam from gtsam import BetweenFactorPose3, PriorFactorPose3 from gtsam.symbol_shorthand import L, X connection = sqlite3.connect(db_path) - pose_stream = pick_pose_stream(connection) + if pose_stream is None: + pose_stream = pick_pose_stream(connection) pose_rows = connection.execute( f"SELECT ts,pose_x,pose_y,pose_z,pose_qx,pose_qy,pose_qz,pose_qw " f'FROM "{pose_stream}" WHERE pose_qw IS NOT NULL ORDER BY ts' @@ -100,7 +123,8 @@ def build_gtsam_gt( connection.close() pose_rows = pose_rows[::node_stride] node_timestamps = np.array([row[0] for row in pose_rows]) - node_poses = [_pose_from7(row[1:8]) for row in pose_rows] + node_poses7 = [list(row[1:8]) for row in pose_rows] + node_poses = [_pose_from7(pose7) for pose7 in node_poses7] num_nodes = len(pose_rows) print( f" gtsam: pose stream '{pose_stream}', {num_nodes} nodes (stride {node_stride}), " @@ -118,6 +142,10 @@ def nearest_node(timestamp): node_index -= 1 return node_index + exclude = {int(marker_id) for marker_id in exclude_marker_ids} + if exclude: + markers = [marker for marker in markers if int(marker["marker_id"]) not in exclude] + graph = gtsam.NonlinearFactorGraph() initial = gtsam.Values() prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.full(6, 1e-4)) @@ -148,6 +176,22 @@ def nearest_node(timestamp): landmark_ids.add(marker_id) graph.add(BetweenFactorPose3(X(node_index), L(marker_id), tag_in_body, tag_noise)) + loops = [] + if add_loop_closures: + loops = find_loop_closures( + db_path, node_timestamps, node_poses7, lidar_stream=loop_lidar_stream + ) + loop_noise = gtsam.noiseModel.Robust.Create( + gtsam.noiseModel.mEstimator.Huber.Create(loop_huber), + gtsam.noiseModel.Diagonal.Sigmas(np.array([loop_rot_sig] * 3 + [loop_trans_sig] * 3)), + ) + for loop in loops: + graph.add( + BetweenFactorPose3( + X(loop.i), X(loop.j), _pose_from7(loop.relative_pose7), loop_noise + ) + ) + params = gtsam.LevenbergMarquardtParams() params.setMaxIterations(100) optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) @@ -159,13 +203,20 @@ def nearest_node(timestamp): for node_index in range(num_nodes) ] print( - f" gtsam: landmarks {sorted(landmark_ids)} | correction max {max(corrections):.2f} m, " + f" gtsam: landmarks {sorted(landmark_ids)} | {len(loops)} loop closures | " + f"correction max {max(corrections):.2f} m, " f"mean {np.mean(corrections):.2f} m ({optimizer.iterations()} iters)" ) - return [ + trajectory = [ (float(node_timestamps[node_index]), _pose_to7(result.atPose3(X(node_index)))) for node_index in range(num_nodes) ] + if return_landmarks: + landmarks = { + marker_id: _pose_to7(result.atPose3(L(marker_id))) for marker_id in sorted(landmark_ids) + } + return trajectory, landmarks + return trajectory def write_gtsam_odom(store, trajectory, stream_name, tum_path): diff --git a/dimos/mapping/recording/utils/lidar_loop_closure.py b/dimos/mapping/recording/utils/lidar_loop_closure.py new file mode 100644 index 0000000000..97b2eb4d11 --- /dev/null +++ b/dimos/mapping/recording/utils/lidar_loop_closure.py @@ -0,0 +1,299 @@ +# 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. + +"""LiDAR loop closure for the AprilTag groundtruth solve. + +AprilTags only pin drift where a tag is re-seen; between tags the FAST-LIO chain +drifts uncorrected. This module finds *revisits* from the lidar alone and turns +each into a relative-pose constraint the GTSAM solve can add as a loop closure: + + 1. sample keyframes along the odom chain (one per `keyframe_gap_m` of travel), + 2. describe each keyframe cloud with a rotation-invariant Scan Context + descriptor (Kim & Kim, IROS'18) so revisits match by appearance, not by the + drifted odom position, + 3. shortlist candidates by ring-key nearest neighbour, confirm with the + column-aligned Scan Context distance, + 4. verify each candidate with Generalized-ICP and keep only well-aligned pairs. + +Returns `LoopClosure` measurements indexed against the same node array the solve +uses. The keyframe clouds and the odom backbone are both in the lidar (mid360) +frame, so the GICP transform *is* the relative-pose measurement — no extrinsics. +""" + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np +import open3d as o3d +from scipy.spatial.transform import Rotation +from sklearn.neighbors import NearestNeighbors + +from dimos.memory2.store.sqlite import SqliteStore +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + + +@dataclass +class LoopClosure: + """A revisit constraint: node `j` (later) seen again at node `i` (earlier). + + `relative_pose7` is the pose of frame `j` expressed in frame `i` + ([x y z qx qy qz qw]); `fitness`/`rmse` are the GICP alignment quality. + """ + + i: int + j: int + relative_pose7: list[float] + fitness: float + rmse: float + + +def scan_context(points: np.ndarray, n_ring: int, n_sector: int, max_radius: float) -> np.ndarray: + """Bird's-eye Scan Context: an `n_ring` x `n_sector` grid whose cells hold the + max height of the points falling in each (range, azimuth) bin.""" + descriptor = np.zeros((n_ring, n_sector), dtype=np.float64) + xy_range = np.linalg.norm(points[:, :2], axis=1) + in_range = (xy_range > 1e-3) & (xy_range < max_radius) + if not np.any(in_range): + return descriptor + points = points[in_range] + xy_range = xy_range[in_range] + azimuth = np.arctan2(points[:, 1], points[:, 0]) + ring_index = np.clip((xy_range / max_radius * n_ring).astype(int), 0, n_ring - 1) + sector_index = np.clip( + ((azimuth + np.pi) / (2 * np.pi) * n_sector).astype(int), 0, n_sector - 1 + ) + flat_index = ring_index * n_sector + sector_index + np.maximum.at(descriptor.reshape(-1), flat_index, points[:, 2]) + return descriptor + + +def ring_key(descriptor: np.ndarray) -> np.ndarray: + """Rotation-invariant summary of a descriptor (mean height per ring).""" + return descriptor.mean(axis=1) + + +def scan_context_distance(query: np.ndarray, candidate: np.ndarray) -> tuple[float, int]: + """Minimum column-aligned cosine distance over all azimuth shifts. + + Returns (distance in [0, 1], best column shift). The shift recovers the yaw + between the two views and seeds GICP.""" + n_sector = query.shape[1] + query_norm = np.linalg.norm(query, axis=0) + best_distance, best_shift = 2.0, 0 + for shift in range(n_sector): + shifted = np.roll(candidate, shift, axis=1) + shifted_norm = np.linalg.norm(shifted, axis=0) + denominator = query_norm * shifted_norm + valid = denominator > 1e-9 + if not np.any(valid): + continue + cosine = (query * shifted).sum(axis=0)[valid] / denominator[valid] + distance = 1.0 - float(cosine.mean()) + if distance < best_distance: + best_distance, best_shift = distance, shift + return best_distance, best_shift + + +def _yaw_transform(yaw_radians: float) -> np.ndarray: + transform = np.eye(4) + transform[:3, :3] = Rotation.from_euler("z", yaw_radians).as_matrix() + return transform + + +def _world_rotation(pose7: list[float]) -> np.ndarray: + """Rotation-only 4x4 of a [x y z qx qy qz qw] pose (its world orientation).""" + rotation = np.eye(4) + rotation[:3, :3] = Rotation.from_quat(pose7[3:7]).as_matrix() + return rotation + + +def _to_open3d(points: np.ndarray, voxel: float) -> o3d.geometry.PointCloud: + cloud = o3d.geometry.PointCloud() + cloud.points = o3d.utility.Vector3dVector(points.astype(np.float64)) + return cloud.voxel_down_sample(voxel) if voxel > 0 else cloud + + +def _gicp( + source: np.ndarray, + target: np.ndarray, + init: np.ndarray, + max_correspondence: float, + voxel: float, +) -> tuple[np.ndarray, float, float]: + """Generalized-ICP of `source` onto `target`. Returns (target_from_source 4x4, + fitness, inlier_rmse). The transform maps a source-frame point into target.""" + source_cloud = _to_open3d(source, voxel) + target_cloud = _to_open3d(target, voxel) + result = o3d.pipelines.registration.registration_generalized_icp( + source_cloud, + target_cloud, + max_correspondence, + init, + o3d.pipelines.registration.TransformationEstimationForGeneralizedICP(), + o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=60), + ) + return result.transformation, result.fitness, result.inlier_rmse + + +def _matrix_to_pose7(transform: np.ndarray) -> list[float]: + quaternion = Rotation.from_matrix(transform[:3, :3]).as_quat() # x, y, z, w + translation = transform[:3, 3] + return [float(value) for value in (*translation, *quaternion)] + + +def _load_keyframe_clouds( + db_path: str, lidar_stream: str, keyframe_timestamps: np.ndarray +) -> dict[int, np.ndarray]: + """Stream `lidar_stream` once, keeping the nearest-in-time cloud per keyframe.""" + best_delta = {index: float("inf") for index in range(len(keyframe_timestamps))} + clouds: dict[int, np.ndarray] = {} + with SqliteStore(path=db_path) as store: + if lidar_stream not in store.list_streams(): + return clouds + for observation in store.stream(lidar_stream, PointCloud2): + keyframe_index = int(np.argmin(np.abs(keyframe_timestamps - observation.ts))) + delta = abs(float(observation.ts) - keyframe_timestamps[keyframe_index]) + if delta < best_delta[keyframe_index]: + points = observation.data.points_f32() + if len(points): + best_delta[keyframe_index] = delta + clouds[keyframe_index] = np.asarray(points, dtype=np.float64) + return clouds + + +def _select_keyframes(node_poses7: list[list[float]], keyframe_gap_m: float) -> list[int]: + """Node indices spaced ~`keyframe_gap_m` apart along the travelled path.""" + keyframe_nodes = [0] + last_position = np.array(node_poses7[0][:3]) + for node_index in range(1, len(node_poses7)): + position = np.array(node_poses7[node_index][:3]) + if np.linalg.norm(position - last_position) >= keyframe_gap_m: + keyframe_nodes.append(node_index) + last_position = position + return keyframe_nodes + + +def find_loop_closures( + db_path: str, + node_timestamps: np.ndarray, + node_poses7: list[list[float]], + *, + lidar_stream: str = "livox_lidar", + keyframe_gap_m: float = 1.0, + n_ring: int = 20, + n_sector: int = 60, + max_radius: float = 20.0, + num_candidates: int = 10, + sc_distance_max: float = 0.5, + min_time_gap_s: float = 15.0, + gicp_max_correspondence: float = 2.0, + gicp_voxel: float = 0.3, + gicp_fitness_min: float = 0.7, + gicp_rmse_max: float = 0.55, + max_translation_m: float = 2.5, + max_vertical_m: float = 1.0, +) -> list[LoopClosure]: + """Detect lidar revisits among the solve's odom nodes and verify them with GICP.""" + keyframe_nodes = _select_keyframes(node_poses7, keyframe_gap_m) + if len(keyframe_nodes) < 3: + print(f" loop: too few keyframes ({len(keyframe_nodes)}) — skipping") + return [] + keyframe_timestamps = node_timestamps[keyframe_nodes] + clouds = _load_keyframe_clouds(db_path, lidar_stream, keyframe_timestamps) + usable = [index for index in range(len(keyframe_nodes)) if index in clouds] + if len(usable) < 3: + print(f" loop: no usable '{lidar_stream}' clouds at keyframes — skipping") + return [] + + # The mid360 is mounted pitched, so raw scans aren't a level bird's-eye. Rotate + # each keyframe into its world orientation (gravity up) for Scan Context and + # GICP, so a revisit from any heading differs only by a yaw column-shift. The + # GICP transform is converted back to the sensor frame at the end. + world_rotation = { + index: _world_rotation(node_poses7[keyframe_nodes[index]]) for index in usable + } + aligned = {index: clouds[index] @ world_rotation[index][:3, :3].T for index in usable} + + descriptors = { + index: scan_context(aligned[index], n_ring, n_sector, max_radius) for index in usable + } + ring_keys = np.array([ring_key(descriptors[index]) for index in usable]) + neighbours = NearestNeighbors(n_neighbors=min(num_candidates + 1, len(usable))).fit(ring_keys) + + loops: list[LoopClosure] = [] + for position, query_index in enumerate(usable): + _, candidate_positions = neighbours.kneighbors(ring_keys[position : position + 1]) + best: LoopClosure | None = None + best_distance = sc_distance_max + for candidate_position in candidate_positions[0]: + candidate_index = usable[candidate_position] + if ( + keyframe_timestamps[query_index] - keyframe_timestamps[candidate_index] + < min_time_gap_s + ): + continue # not a revisit: too close in time (or in the future) + distance, shift = scan_context_distance( + descriptors[query_index], descriptors[candidate_index] + ) + if distance >= best_distance: + continue + # Scan Context recovers the yaw magnitude but not its sign — seed GICP + # from both and keep the better-aligned result. + yaw = shift * (2 * np.pi / n_sector) + transform, fitness, rmse = max( + ( + _gicp( + aligned[query_index], + aligned[candidate_index], + _yaw_transform(signed_yaw), + gicp_max_correspondence, + gicp_voxel, + ) + for signed_yaw in (yaw, -yaw) + ), + key=lambda result: result[1], + ) + if fitness < gicp_fitness_min or rmse > gicp_rmse_max: + continue + # A genuine revisit puts the two scans at nearly the same place; a large + # (especially vertical) offset is perceptual aliasing — drop it. GT is + # corrupted far worse by one bad loop than helped by a marginal one. + if ( + abs(transform[2, 3]) > max_vertical_m + or np.linalg.norm(transform[:3, 3]) > max_translation_m + ): + continue + # convert aligned-frame transform back to the sensor (mid360) node frame + sensor_transform = ( + np.linalg.inv(world_rotation[candidate_index]) + @ transform + @ world_rotation[query_index] + ) + best_distance = distance + best = LoopClosure( + i=keyframe_nodes[candidate_index], + j=keyframe_nodes[query_index], + relative_pose7=_matrix_to_pose7(sensor_transform), + fitness=float(fitness), + rmse=float(rmse), + ) + if best is not None: + loops.append(best) + + print( + f" loop: {len(keyframe_nodes)} keyframes ({len(usable)} with clouds) " + f"-> {len(loops)} lidar loop closures (stream '{lidar_stream}')" + ) + return loops diff --git a/dimos/mapping/recording/utils/lidar_reanchor.py b/dimos/mapping/recording/utils/lidar_reanchor.py index 1d9cc559c2..e1b1fd3e6e 100644 --- a/dimos/mapping/recording/utils/lidar_reanchor.py +++ b/dimos/mapping/recording/utils/lidar_reanchor.py @@ -48,7 +48,7 @@ def _load_poses(db_path: str, stream: str): ).fetchall() conn.close() if not rows: - raise SystemExit(f"no populated poses in stream '{stream}'") + raise ValueError(f"no populated poses in stream '{stream}'") return np.array([r[0] for r in rows]), np.array([r[1:8] for r in rows], dtype=np.float64) diff --git a/dimos/mapping/recording/utils/post_process.py b/dimos/mapping/recording/utils/post_process.py index 2657051dd0..415e820bba 100644 --- a/dimos/mapping/recording/utils/post_process.py +++ b/dimos/mapping/recording/utils/post_process.py @@ -98,6 +98,7 @@ def correct_db( gtsam_stream, marker_length, dictionary, + add_loop_closures=True, ): """AprilTag detection -> GTSAM trajectory -> re-anchor lidar. Returns True if a corrected trajectory was written.""" @@ -108,7 +109,9 @@ def correct_db( if not detections: print(" no AprilTags detected — skipping gtsam_odom (no landmark constraints)") return False - trajectory = build_gtsam_gt(str(db), detections, optical_in_base) + trajectory = build_gtsam_gt( + str(db), detections, optical_in_base, add_loop_closures=add_loop_closures + ) with SqliteStore(path=str(db)) as store: write_gtsam_odom(store, trajectory, gtsam_stream, db.parent / "gtsam_odom.tum") stream_names = store.list_streams() @@ -143,6 +146,7 @@ def process_db( dictionary, force, no_gtsam=False, + no_loop=False, make_rrd=True, camera_freq=30, map_voxel=0.1, @@ -156,11 +160,12 @@ def process_db( except Exception as error: print(f" rec_check skipped: {error}") + try: + print(f" wrote {rec_check.write_summary(db.parent)}") + except Exception as error: + print(f" summary failed: {error}") + if check_only: - try: - print(f" wrote {rec_check.write_summary(db.parent)}") - except Exception as error: - print(f" summary failed: {error}") return with SqliteStore(path=str(db)) as store: @@ -182,6 +187,7 @@ def process_db( gtsam_stream=gtsam_stream, marker_length=marker_length, dictionary=dictionary, + add_loop_closures=not no_loop, ) if make_rrd: @@ -240,6 +246,11 @@ def run( action="store_true", help="skip AprilTag/GTSAM/re-anchor (e.g. rebuild only the .rrd)", ) + parser.add_argument( + "--no-loop", + action="store_true", + help="skip lidar loop-closure detection (AprilTags-only drift correction)", + ) parser.add_argument("--no-rrd", action="store_true", help="skip the .rrd visualization step") parser.add_argument( "--camera-freq", @@ -286,6 +297,7 @@ def run( dictionary=args.dictionary, force=args.force, no_gtsam=args.no_gtsam, + no_loop=args.no_loop, make_rrd=not args.no_rrd, camera_freq=args.camera_freq, map_voxel=args.map_voxel, From fa6325d524df954e01b47266bad792fdf7a87aef Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 4 Jun 2026 21:18:04 -0700 Subject: [PATCH 033/185] pointlio native: consume fast-lio from github instead of local path Switches the flake's fast-lio input from path:/home/dimos/repos/dimos-module-fastlio2 to github:dimensionalOS/dimos-module-fastlio2/pointlio so the module builds on any machine without a local fastlio2 clone. Relocked onto rev 7e5d88f (the mtx_buffer race fix); verified pointlio_native builds from the github source. --- .../sensors/lidar/pointlio/cpp/flake.lock | 15 ++++++++++----- .../hardware/sensors/lidar/pointlio/cpp/flake.nix | 2 +- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock index 6472a83e88..670a10a9aa 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock @@ -37,13 +37,18 @@ "fast-lio": { "flake": false, "locked": { - "narHash": "sha256-DPoyTQbYlVi6qwwtLJVqgpdDHa2bQ+2W3v71QtIIPNU=", - "path": "/home/dimos/repos/dimos-module-fastlio2", - "type": "path" + "lastModified": 1780625608, + "narHash": "sha256-r21B0goiQr8VnxcF9RCxhLaZBihfWQSpaVLAY0K18Xw=", + "owner": "dimensionalOS", + "repo": "dimos-module-fastlio2", + "rev": "7e5d88fcb3c22e56f426bc07ade1bd8afa02129c", + "type": "github" }, "original": { - "path": "/home/dimos/repos/dimos-module-fastlio2", - "type": "path" + "owner": "dimensionalOS", + "ref": "pointlio", + "repo": "dimos-module-fastlio2", + "type": "github" } }, "flake-utils": { diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix index cdcf4bb32b..52247fc10f 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix @@ -13,7 +13,7 @@ flake = false; }; fast-lio = { - url = "path:/home/dimos/repos/dimos-module-fastlio2"; + url = "github:dimensionalOS/dimos-module-fastlio2/pointlio"; flake = false; }; lcm-extended = { From c3a65b2ed3b187ab3d44d4c8a1ed5bca1f457a66 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sat, 6 Jun 2026 19:42:39 -0700 Subject: [PATCH 034/185] add ssh tele op --- dimos/mapping/recording/go2_mid360/record.py | 56 +-- dimos/robot/unitree/keyboard_teleop_tui.py | 344 +++++++++++++++++++ 2 files changed, 378 insertions(+), 22 deletions(-) create mode 100644 dimos/robot/unitree/keyboard_teleop_tui.py diff --git a/dimos/mapping/recording/go2_mid360/record.py b/dimos/mapping/recording/go2_mid360/record.py index 15fc601e60..9717a66720 100644 --- a/dimos/mapping/recording/go2_mid360/record.py +++ b/dimos/mapping/recording/go2_mid360/record.py @@ -23,6 +23,7 @@ from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.core.global_config import global_config from dimos.core.stream import In +from dimos.core.transport import LCMTransport from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 from dimos.hardware.sensors.lidar.fastlio2.recorder import FastLio2Recorder, _default_recording_dir from dimos.hardware.sensors.lidar.fastlio2.speed_warner import SpeedWarner @@ -34,18 +35,20 @@ from dimos.memory2.stream import Stream from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.Imu import Imu from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.robot.unitree.go2.connection import GO2Connection -from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop +from dimos.robot.unitree.keyboard_teleop_tui import KeyboardTeleopTUI 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") +_LIDAR_IP = os.getenv("LIDAR_IP", "192.168.1.171") +_LIDAR_HOST_IP = os.getenv("LIDAR_HOST_IP", "192.168.1.100") class Go2TfHackRecorder(FastLio2Recorder): @@ -73,9 +76,7 @@ class Go2TfHackRecorder(FastLio2Recorder): # empty `lidar`/`odom` streams; the go2-prefixed ports above take their place. lidar: None = None # type: ignore[assignment] odom: None = None # type: ignore[assignment] - # sanity check - fastlio_lidar_no_cap: In[PointCloud2] - fastlio_odometry_no_cap: In[Odometry] + tf: In[Transform] _latest_fastlio_odom: Odometry | None = None _warning_names: set[str] = set() @@ -128,7 +129,6 @@ class FastLio2NoCap(FastLio2): unitree_go2_record = autoconnect( - KeyboardTeleop.blueprint(), MovementManager.blueprint(), GO2Connection.blueprint().remappings( [ @@ -138,6 +138,7 @@ class FastLio2NoCap(FastLio2): ), Mid360.blueprint( lidar_ip=_LIDAR_IP, + host_ip=_LIDAR_HOST_IP, ).remappings( [ (Mid360, "lidar", "livox_lidar"), @@ -155,24 +156,10 @@ class FastLio2NoCap(FastLio2): (FastLio2, "odometry", "fastlio_odometry"), ] ), - # FastLio2NoCap.blueprint( - # frame_id="world", - # map_freq=-1, - # lidar_ip=_LIDAR_IP, - # max_velocity_norm_ms=100, - # # Absolute path to FastLio2's cpp build dir; passed to FastLio2NoCap so the - # # trivial subclass doesn't try to resolve `cpp` next to this file. - # cwd=str(Path(_fastlio2_module.__file__).resolve().parent / "cpp"), - # ).remappings( - # [ - # (FastLio2, "lidar", "fastlio_lidar_no_cap"), - # (FastLio2, "odometry", "fastlio_odometry_no_cap"), - # ] - # ), Go2TfHackRecorder.blueprint(lidar_ip=_LIDAR_IP, record_pcap=True), SpeedWarner.blueprint().remappings( [ - (SpeedWarner, "odometry", "fastlio_odometry_no_cap"), + (SpeedWarner, "odometry", "fastlio_odometry"), ] ), ).global_config(n_workers=10, robot_model="unitree_go2") @@ -187,4 +174,29 @@ class FastLio2NoCap(FastLio2): unitree_go2_record, {Go2TfHackRecorder.name: {"recording_dir": recording_dir}}, ) - coordinator.loop() + + # Sit/stand drive the Go2 directly via its RPCs. After standing the dog must + # re-enter BalanceStand before it will walk again, so on_stand chains + # standup -> balance_stand (mirrors GO2Connection.start()). + go2 = coordinator.get_instance(GO2Connection) + + def stand_and_ready() -> None: + go2.standup() + time.sleep(3.0) + go2.balance_stand() + + # Launch the TUI teleop in THIS process (not via the coordinator) so it owns + # the terminal's stdin and can read WASD keypresses. Its output is wired onto + # the same /tele_cmd_vel topic MovementManager subscribes to. + teleop = KeyboardTeleopTUI( + linear_speed=0.3, + angular_speed=0.6, + on_sit=go2.liedown, + on_stand=stand_and_ready, + ) + teleop.tele_cmd_vel.transport = LCMTransport("/tele_cmd_vel", Twist) + teleop.start() + try: + coordinator.loop() + finally: + teleop.stop() diff --git a/dimos/robot/unitree/keyboard_teleop_tui.py b/dimos/robot/unitree/keyboard_teleop_tui.py new file mode 100644 index 0000000000..4affbe708b --- /dev/null +++ b/dimos/robot/unitree/keyboard_teleop_tui.py @@ -0,0 +1,344 @@ +#!/usr/bin/env python3 +# Copyright 2025-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. + +"""Terminal (TUI) keyboard teleop. Drop-in alternative to ``KeyboardTeleop``. + +Unlike the pygame version this needs no display/window: it reads keypresses +straight from the controlling terminal and renders a status panel with ANSI +escapes. Same ``tele_cmd_vel`` Twist output and the same constructor knobs, so it +can be swapped into a blueprint wherever ``KeyboardTeleop`` is used. + +Terminals can't report key-release, only key-repeat, so "hold to move" is +emulated: a movement key stays active for ``key_timeout`` seconds after its +last repeat. Holding a key refreshes it continuously (smooth motion); on +release the robot coasts to a stop within ``key_timeout``. + +Controls: + W/S: forward / back + A/D: turn left / right + Q/E: strafe left / right + Shift+key (tap): toggle turbo on/off + 1: sit (lie down) 2: stand (and re-enter walk mode) + SPACE: emergency stop + ESC: quit + +Sit/stand are driven through optional ``on_sit`` / ``on_stand`` callbacks so the +module stays robot-agnostic; the caller wires them to the robot's RPCs. +""" + +from collections.abc import Callable +import os +import select +import sys +import termios +import threading +import time +import tty +from typing import Any + +from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT +from dimos.core.core import rpc +from dimos.core.module import Module +from dimos.core.stream import Out +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +DEFAULT_LINEAR_SPEED: float = 0.3 # m/s +DEFAULT_ANGULAR_SPEED: float = 0.6 # rad/s +DEFAULT_BOOST_MULTIPLIER: float = 2.0 +DEFAULT_SLOW_MULTIPLIER: float = 0.5 +DEFAULT_KEY_TIMEOUT: float = 0.4 # s a key stays "held" after its last repeat + +_CONTROL_RATE_HZ = 50 +# Min seconds between turbo toggles; debounces terminal key-repeat so holding +# Shift+key flips turbo once, not every repeat. +_TURBO_TOGGLE_COOLDOWN = 0.5 + +# ANSI helpers. +_ENTER_ALT = "\033[?1049h" # switch to alternate screen buffer +_EXIT_ALT = "\033[?1049l" # restore the normal screen (and its scrollback) +_HIDE_CURSOR = "\033[?25l" +_SHOW_CURSOR = "\033[?25h" +_CLEAR_SCREEN = "\033[2J" +_CLEAR_BELOW = "\033[J" # clear from cursor to end of screen +_CURSOR_HOME = "\033[H" +_CLEAR_LINE = "\033[K" +_RESET = "\033[0m" +_CYAN = "\033[36m" +_DIM = "\033[2m" +_GREEN = "\033[32m" +_RED = "\033[31m" + +_BOX_WIDTH = 46 # visible width of the panel's inner content area + +# Movement keys -> (linear_x, linear_y, angular_z) unit contribution. +_KEY_MOTION: dict[str, tuple[float, float, float]] = { + "w": (1.0, 0.0, 0.0), + "s": (-1.0, 0.0, 0.0), + "q": (0.0, 1.0, 0.0), + "e": (0.0, -1.0, 0.0), + "a": (0.0, 0.0, 1.0), + "d": (0.0, 0.0, -1.0), +} + + +class KeyboardTeleopTUI(Module): + """Terminal-based keyboard control. Outputs Twist on tele_cmd_vel.""" + + tele_cmd_vel: Out[Twist] + + _stop_event: threading.Event + _thread: threading.Thread | None = None + + def __init__( + self, + linear_speed: float = DEFAULT_LINEAR_SPEED, + angular_speed: float = DEFAULT_ANGULAR_SPEED, + boost_multiplier: float = DEFAULT_BOOST_MULTIPLIER, + slow_multiplier: float = DEFAULT_SLOW_MULTIPLIER, + key_timeout: float = DEFAULT_KEY_TIMEOUT, + publish_only_when_active: bool = False, + on_sit: Callable[[], Any] | None = None, + on_stand: Callable[[], Any] | None = None, + **kwargs: Any, + ) -> None: + super().__init__(**kwargs) + self._stop_event = threading.Event() + self.linear_speed = linear_speed + self.angular_speed = angular_speed + self.boost_multiplier = boost_multiplier + self.slow_multiplier = slow_multiplier + self.key_timeout = key_timeout + # Discrete robot actions, run on a background thread so the control loop + # stays responsive. on_stand should also re-enter the locomotion mode so + # the robot can walk again (e.g. standup() then balance_stand()). + self.on_sit = on_sit + self.on_stand = on_stand + # When True, only publish while a movement key is held; on + # release publish a single zero Twist (stop) then go silent. + # Lets the teleop coexist with another /tele_cmd_vel publisher + # instead of flooding zeros. + self.publish_only_when_active = publish_only_when_active + self._was_active = False + # key char -> monotonic timestamp of its most recent press. + self._last_press: dict[str, float] = {} + # Turbo (boost) is a toggle now, not press-and-hold. + self._turbo = False + self._last_turbo_toggle = 0.0 + # Discrete-action state: name of the in-flight action ("sitting"/ + # "standing") or None. Guarded so only one runs at a time. + self._action_lock = threading.Lock() + self._action_status: str | None = None + + @rpc + def start(self) -> None: + super().start() + self._stop_event.clear() + self._last_press.clear() + self._turbo = False + self._last_turbo_toggle = 0.0 + self._thread = threading.Thread(target=self._tui_loop, daemon=True) + self._thread.start() + + @rpc + def stop(self) -> None: + self._publish_stop() + self._stop_event.set() + if self._thread is None: + raise RuntimeError("Cannot stop: thread was never started") + self._thread.join(DEFAULT_THREAD_JOIN_TIMEOUT) + super().stop() + + def _publish_stop(self) -> None: + stop_twist = Twist() + stop_twist.linear = Vector3(0, 0, 0) + stop_twist.angular = Vector3(0, 0, 0) + self.tele_cmd_vel.publish(stop_twist) + + def _tui_loop(self) -> None: + if not sys.stdin.isatty(): + logger.warning( + "KeyboardTeleopTUI: stdin is not a TTY; no keyboard input will be " + "read. Run this module in the foreground of an interactive terminal." + ) + self._stop_event.wait() + return + + fd = sys.stdin.fileno() + old_attrs = termios.tcgetattr(fd) + period = 1.0 / _CONTROL_RATE_HZ + # Render on the alternate screen buffer so the panel owns the terminal, + # isolated from the coordinator/worker log spam (which keeps going to the + # log file). The full repaint each frame wipes any stray writes that leak + # through. Leaving the buffer on exit restores the user's normal screen. + sys.stdout.write(_ENTER_ALT + _HIDE_CURSOR + _CLEAR_SCREEN) + sys.stdout.flush() + try: + # cbreak leaves ISIG on, so Ctrl+C still works as usual. + tty.setcbreak(fd) + while not self._stop_event.is_set(): + ready, _, _ = select.select([sys.stdin], [], [], period) + if ready: + data = os.read(fd, 64).decode("utf-8", errors="ignore") + if not self._handle_input(data): + break + + twist = self._compute_twist() + self._maybe_publish(twist) + self._render(twist) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_attrs) + sys.stdout.write(_SHOW_CURSOR + _EXIT_ALT) + sys.stdout.flush() + + def _handle_input(self, data: str) -> bool: + """Process a raw read. Returns False to request quit.""" + now = time.monotonic() + for ch in data: + if ch == " ": + # Emergency stop: drop all held keys and send a zero Twist. + self._last_press.clear() + self._publish_stop() + logger.warning("EMERGENCY STOP!") + elif ch == "\x1b": + # Lone ESC quits; ESC-sequences (arrow keys) are ignored here. + if len(data) == 1: + return False + elif ch == "1": + self._trigger_action("sitting", self.on_sit) + elif ch == "2": + self._trigger_action("standing", self.on_stand) + else: + lower = ch.lower() + if lower in _KEY_MOTION: + self._last_press[lower] = now + # Uppercase (Shift) taps toggle turbo; cooldown debounces + # terminal key-repeat so a held key flips it only once. + if ch.isupper() and now - self._last_turbo_toggle > _TURBO_TOGGLE_COOLDOWN: + self._turbo = not self._turbo + self._last_turbo_toggle = now + return True + + def _trigger_action(self, name: str, fn: Callable[[], Any] | None) -> None: + """Run a discrete robot action (sit/stand) on a background thread. + + Movement is suppressed while it runs; re-triggers are ignored until it + finishes so key-repeat or impatient taps can't stack commands. + """ + if fn is None: + return + with self._action_lock: + if self._action_status is not None: + return + self._action_status = name + self._last_press.clear() # stop driving while the robot changes posture + + def run() -> None: + try: + fn() + except Exception: + logger.exception(f"KeyboardTeleopTUI: {name} action failed") + finally: + with self._action_lock: + self._action_status = None + + threading.Thread(target=run, daemon=True).start() + + def _compute_twist(self) -> Twist: + now = time.monotonic() + twist = Twist() + twist.linear = Vector3(0, 0, 0) + twist.angular = Vector3(0, 0, 0) + + # Hold still while a sit/stand action is in flight. + if self._action_status is not None: + return twist + + for key, (fx, fy, fz) in _KEY_MOTION.items(): + if now - self._last_press.get(key, float("-inf")) <= self.key_timeout: + twist.linear.x += fx * self.linear_speed + twist.linear.y += fy * self.linear_speed + twist.angular.z += fz * self.angular_speed + + multiplier = self.boost_multiplier if self._turbo else 1.0 + twist.linear.x *= multiplier + twist.linear.y *= multiplier + twist.angular.z *= multiplier + return twist + + def _maybe_publish(self, twist: Twist) -> None: + if self.publish_only_when_active: + active = twist.linear.x != 0 or twist.linear.y != 0 or twist.angular.z != 0 + # Publish while active; publish exactly one zero on the + # active->idle transition (clean stop); then stay silent. + if active or self._was_active: + self.tele_cmd_vel.publish(twist) + self._was_active = active + else: + self.tele_cmd_vel.publish(twist) + + def _render(self, twist: Twist) -> None: + now = time.monotonic() + moving = twist.linear.x != 0 or twist.linear.y != 0 or twist.angular.z != 0 + action = self._action_status + held = sorted( + key.upper() + for key in _KEY_MOTION + if now - self._last_press.get(key, float("-inf")) <= self.key_timeout + ) + status_plain = "● MOVING" if moving else "● IDLE" + status_color = _RED if moving else _GREEN + turbo_txt = f" [TURBO {self.boost_multiplier:g}x]" if self._turbo else "" + action_line = f"▶ {action}…" if action else "" + + # (plain_text, color) rows. Padding is computed from the *plain* text so + # invisible ANSI color codes don't throw off the box alignment. + rows: list[tuple[str, str]] = [ + (f"{status_plain}{turbo_txt}", status_color), + (action_line, _CYAN if action else ""), + ("", ""), + (f"Linear X (Fwd/Back) : {twist.linear.x:+.2f} m/s", ""), + (f"Linear Y (Strafe) : {twist.linear.y:+.2f} m/s", ""), + (f"Angular Z (Turn) : {twist.angular.z:+.2f} rad/s", ""), + ("", ""), + (f"Held: {', '.join(held) if held else '-'}", ""), + ("", ""), + ("WS: move AD: turn QE: strafe", _DIM), + ("Shift+key (tap): toggle turbo", _DIM), + ("1: sit 2: stand", _DIM), + ("SPACE: e-stop ESC: quit", _DIM), + ] + + w = _BOX_WIDTH + title = " Keyboard Teleop (TUI) " + top = "┌" + title + "─" * (w + 2 - len(title)) + "┐" + bottom = "└" + "─" * (w + 2) + "┘" + + lines = [f"{_CYAN}{top}{_RESET}"] + for plain, color in rows: + text = plain[:w] + pad = " " * (w - len(text)) + inner = f"{color}{text}{_RESET}{pad}" if color else f"{text}{pad}" + lines.append(f"{_CYAN}│{_RESET} {inner} {_CYAN}│{_RESET}") + lines.append(f"{_CYAN}{bottom}{_RESET}") + + out = ( + _CURSOR_HOME + "\r\n".join(line + _CLEAR_LINE for line in lines) + "\r\n" + _CLEAR_BELOW + ) + sys.stdout.write(out) + sys.stdout.flush() From 89151a707eba16ed1d7f0b40f676ec402202344b Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 10 Jun 2026 17:07:33 -0700 Subject: [PATCH 035/185] pointlio_native: fix replay thread race + iVox glog dep + master-faithful config MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit OUTPUT-model replay of the Jun-7 hand-shake bag diverged to ~25km (vs hku-mars master's bounded ~5m). Two issues, both fixed here (paired with the dimos-module-fastlio2 pointlio-branch curvature/deque fixes): - main.cpp: heap-use-after-free thread race in bag-frame replay. The feeder thread drives run_main_iter via step() after each feed, but the main thread also drove run_main_iter, so both raced on the shared PCL measurement cloud (ASan: free in sync_packages, read-after-free in ImuProcess::Process). Force serial_replay in bag-frame mode so only the feeder thread touches the EKF. - CMakeLists.txt + flake.nix: the iVox map backend needs glog; add the find_package/link and the nix buildInput. Drop ikd_Tree.cpp from sources (iVox replaces ikd-Tree). - config/mid360.yaml (+ cpp/config duplicate): set satu_acc 5.5->3.0 (master value — accel >=3g saturated, residual zeroed, keeps velocity bounded) and add ivox_grid_resolution=2.0 / ivox_nearby_type=6 (NEARBY6) matching the master config that produced the 5.028m baseline. - replay_ruwik2_pt3.py: make MAX_WALL_SEC env-overridable. After the fix the OUTPUT model stays bounded (peak |pos| 3.898m) on the full bag; ASan reports zero memory errors. --- .../sensors/lidar/pointlio/config/mid360.yaml | 4 +- .../sensors/lidar/pointlio/cpp/CMakeLists.txt | 5 +- .../lidar/pointlio/cpp/config/mid360.yaml | 4 +- .../sensors/lidar/pointlio/cpp/flake.nix | 1 + .../sensors/lidar/pointlio/cpp/main.cpp | 160 ++++++++++++++++-- .../lidar/pointlio/tools/replay_ruwik2_pt3.py | 2 +- 6 files changed, 161 insertions(+), 15 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/config/mid360.yaml b/dimos/hardware/sensors/lidar/pointlio/config/mid360.yaml index 1839646d5a..a989359c0c 100644 --- a/dimos/hardware/sensors/lidar/pointlio/config/mid360.yaml +++ b/dimos/hardware/sensors/lidar/pointlio/config/mid360.yaml @@ -22,12 +22,14 @@ mapping: check_satu: true init_map_size: 10 space_down_sample: true - satu_acc: 5.5 # g (acc_norm=1.0). Just below the ruwik2 6.9g spikes. + satu_acc: 3.0 # g (acc_norm=1.0). hku-mars master value: accel >= 3g saturated → EKF residual zeroed, keeps velocity bounded. satu_gyro: 35.0 acc_norm: 1.0 # IMU accel in g plane_thr: 0.1 filter_size_surf: 0.5 filter_size_map: 0.5 + ivox_grid_resolution: 2.0 # iVox local-map grid (m). Matches hku-mars master avia/horizon Livox configs. + ivox_nearby_type: 6 # NEARBY6. Matches the hku_repro mapping_mid360.launch override (NOT the yaml default) that produced the bounded 5.028m baseline. cube_side_length: 1000.0 det_range: 100.0 fov_degree: 360.0 diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt index 1c5aed391c..28a43e6fc9 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt @@ -61,6 +61,9 @@ find_package(PCL 1.8 REQUIRED COMPONENTS common filters) # yaml-cpp (FAST-LIO config parsing — standard YAML format) find_package(yaml-cpp REQUIRED) +# glog (iVox map backend — include/ivox/ivox3d.h needs glog) +find_package(glog REQUIRED) + # Livox SDK2 (from nix or /usr/local fallback) find_library(LIVOX_SDK livox_lidar_sdk_shared) if(NOT LIVOX_SDK) @@ -75,7 +78,6 @@ add_executable(pointlio_native ${FASTLIO_DIR}/src/preprocess.cpp ${FASTLIO_DIR}/src/Estimator.cpp ${FASTLIO_DIR}/src/parameters.cpp - ${FASTLIO_DIR}/include/ikd-Tree/ikd_Tree.cpp ) # Shared Livox common headers (livox_sdk_config.hpp etc.) @@ -101,6 +103,7 @@ target_link_libraries(pointlio_native PRIVATE ${LIVOX_SDK} ${PCL_LIBRARIES} yaml-cpp::yaml-cpp + glog::glog ) if(OpenMP_CXX_FOUND) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/config/mid360.yaml b/dimos/hardware/sensors/lidar/pointlio/cpp/config/mid360.yaml index 1839646d5a..a989359c0c 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/config/mid360.yaml +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/config/mid360.yaml @@ -22,12 +22,14 @@ mapping: check_satu: true init_map_size: 10 space_down_sample: true - satu_acc: 5.5 # g (acc_norm=1.0). Just below the ruwik2 6.9g spikes. + satu_acc: 3.0 # g (acc_norm=1.0). hku-mars master value: accel >= 3g saturated → EKF residual zeroed, keeps velocity bounded. satu_gyro: 35.0 acc_norm: 1.0 # IMU accel in g plane_thr: 0.1 filter_size_surf: 0.5 filter_size_map: 0.5 + ivox_grid_resolution: 2.0 # iVox local-map grid (m). Matches hku-mars master avia/horizon Livox configs. + ivox_nearby_type: 6 # NEARBY6. Matches the hku_repro mapping_mid360.launch override (NOT the yaml default) that produced the bounded 5.028m baseline. cube_side_length: 1000.0 det_range: 100.0 fov_degree: 360.0 diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix index 52247fc10f..0d174f6e24 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix @@ -80,6 +80,7 @@ pkgs.eigen pkgs.pcl pkgs.yaml-cpp + pkgs.glog pkgs.boost pkgs.llvmPackages.openmp ]; diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 90f2f9df6d..865fe49585 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -351,6 +351,14 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ uint64_t ts_ns = get_timestamp_ns(data); uint16_t dot_num = data->dot_num; + // Per-point intra-packet time offset, matching livox_ros_driver2 + // (pub_handler.cpp: point_interval = time_interval*100/dot_num ns; + // offset_time = base + i*point_interval). Without this, every point in a + // packet collapses to one timestamp and per-point deskew is lost — fatal + // during aggressive motion. time_interval unit is 0.1us, so *100 → ns. + const uint64_t point_interval_ns = + dot_num > 0 ? static_cast(data->time_interval) * 100 / dot_num : 0; + if (!g_replay_mode.load()) { mark_first_packet(ts_ns); } @@ -385,7 +393,7 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ cp.reflectivity = pts[i].reflectivity; cp.tag = pts[i].tag; cp.line = 0; // Mid-360: non-repetitive, single "line" - cp.offset_time = static_cast(ts_ns - g_frame_start_ns); + cp.offset_time = static_cast((ts_ns - g_frame_start_ns) + i * point_interval_ns); g_accumulated_points.push_back(cp); } } else if (data->data_type == DATA_TYPE_CARTESIAN_LOW) { @@ -398,7 +406,7 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ cp.reflectivity = pts[i].reflectivity; cp.tag = pts[i].tag; cp.line = 0; - cp.offset_time = static_cast(ts_ns - g_frame_start_ns); + cp.offset_time = static_cast((ts_ns - g_frame_start_ns) + i * point_interval_ns); g_accumulated_points.push_back(cp); } } @@ -591,7 +599,12 @@ int main(int argc, char** argv) { // on_point_cloud / on_imu_data callbacks directly. publish_*() uses // the pcap timestamps as the clock so outputs match the original run. std::string replay_pcap = mod.arg("replay_pcap", ""); - g_replay_mode.store(!replay_pcap.empty()); + // Alternative replay source: a flat binary of driver CustomMsg/Imu frames + // dumped from a ROS bag (tools/dump_bag_frames.py). Feeds the port the EXACT + // driver output the ROS reference consumes, bypassing UDP->CustomMsg + // reconstruction — isolates port faithfulness from reconstruction fidelity. + std::string replay_bagframes = mod.arg("replay_bagframes", ""); + g_replay_mode.store(!replay_pcap.empty() || !replay_bagframes.empty()); // Drop pcap packets with pcap_ts < this value. Used in replay to mimic // the SDK warmup discard that the live run experienced — so the @@ -885,6 +898,106 @@ int main(int argc, char** argv) { if (g_replay_mode.load()) { if (debug) printf("[fastlio2] REPLAY mode, pcap=%s\n", replay_pcap.c_str()); replay_thread = std::thread([&]() { + if (!replay_bagframes.empty()) { + // Bag-frame replay: read driver CustomMsg/Imu records from the + // flat dump and feed them straight into the port, serialized + // with the EKF (feed -> run_main_iter) on this one thread. No + // UDP reconstruction, no accumulator. Deterministic by design. + std::ifstream bf(replay_bagframes, std::ios::binary); + if (!bf) { + fprintf(stderr, "[bagframes] cannot open %s\n", replay_bagframes.c_str()); + g_running.store(false); + return; + } + auto advance_clock = [](uint64_t ts_ns) { + uint64_t expected = 0; + g_first_packet_clock_ns.compare_exchange_strong(expected, ts_ns); + uint64_t cur = g_virtual_clock_ns.load(); + while (cur < ts_ns && + !g_virtual_clock_ns.compare_exchange_weak(cur, ts_ns)) {} + }; + auto step = [&]() { + auto now_opt = virtual_now(); + if (now_opt.has_value()) run_main_iter(*now_opt); + }; + size_t n_imu = 0, n_lid = 0; + uint8_t type = 0; + while (g_running.load() && bf.read(reinterpret_cast(&type), 1)) { + if (type == 0) { + double rec[7]; + if (!bf.read(reinterpret_cast(rec), sizeof(rec))) break; + auto imu_msg = boost::make_shared(); + imu_msg->header.seq = 0; + imu_msg->header.stamp = custom_messages::Time().fromSec(rec[0]); + imu_msg->header.frame_id = "livox_frame"; + imu_msg->orientation.x = 0.0; + imu_msg->orientation.y = 0.0; + imu_msg->orientation.z = 0.0; + imu_msg->orientation.w = 1.0; + imu_msg->linear_acceleration.x = rec[1]; + imu_msg->linear_acceleration.y = rec[2]; + imu_msg->linear_acceleration.z = rec[3]; + imu_msg->angular_velocity.x = rec[4]; + imu_msg->angular_velocity.y = rec[5]; + imu_msg->angular_velocity.z = rec[6]; + for (int j = 0; j < 9; ++j) { + imu_msg->orientation_covariance[j] = 0.0; + imu_msg->angular_velocity_covariance[j] = 0.0; + imu_msg->linear_acceleration_covariance[j] = 0.0; + } + advance_clock(static_cast(rec[0] * 1e9)); + g_fastlio->feed_imu(imu_msg); + step(); + ++n_imu; + } else if (type == 1) { + double stamp_sec = 0.0; + uint64_t timebase = 0; + uint32_t point_num = 0; + if (!bf.read(reinterpret_cast(&stamp_sec), 8)) break; + if (!bf.read(reinterpret_cast(&timebase), 8)) break; + if (!bf.read(reinterpret_cast(&point_num), 4)) break; + auto lidar_msg = boost::make_shared(); + lidar_msg->header.seq = 0; + lidar_msg->header.stamp = custom_messages::Time().fromSec(stamp_sec); + lidar_msg->header.frame_id = "livox_frame"; + lidar_msg->timebase = timebase; + lidar_msg->lidar_id = 0; + for (int j = 0; j < 3; ++j) lidar_msg->rsvd[j] = 0; + lidar_msg->point_num = point_num; + lidar_msg->points.resize(point_num); + for (uint32_t i = 0; i < point_num; ++i) { + uint32_t off = 0; + float xyz[3] = {0, 0, 0}; + uint8_t meta[3] = {0, 0, 0}; + if (!bf.read(reinterpret_cast(&off), 4) || + !bf.read(reinterpret_cast(xyz), 12) || + !bf.read(reinterpret_cast(meta), 3)) { + g_running.store(false); + break; + } + custom_messages::CustomPoint& cp = lidar_msg->points[i]; + cp.offset_time = off; + cp.x = static_cast(xyz[0]); + cp.y = static_cast(xyz[1]); + cp.z = static_cast(xyz[2]); + cp.reflectivity = meta[0]; + cp.tag = meta[1]; + cp.line = meta[2]; + } + advance_clock(static_cast(stamp_sec * 1e9)); + g_fastlio->feed_lidar(lidar_msg); + step(); + ++n_lid; + } else { + fprintf(stderr, "[bagframes] bad record type %u\n", type); + break; + } + } + printf("[bagframes] done: imu=%zu lidar=%zu\n", n_imu, n_lid); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + g_running.store(false); + return; + } pcap_replay::Replayer rep; rep.path = replay_pcap; rep.host_point_port = static_cast(ports.host_point_data); @@ -904,15 +1017,27 @@ int main(int argc, char** argv) { } g_virtual_clock_ns.store(pcap_ts_ns); }; - // No rep.on_iter — the main thread drives run_main_iter in - // replay mode now, same as in live. This decouples packet - // ingestion from per-iter filter_cloud cost and lets replay - // run at the same wall throughput as live. rep.running = &g_running; - // Pace the replay feeder at live wall-clock rate. sleep_until - // throttles the feeder so packets land in the accumulator at - // the same wall cadence as the SDK delivers in live mode. - rep.realtime = true; + if (g_deterministic_clock.load() && !replay_dual_thread) { + // Deterministic serial replay: the feeder thread drives the + // EKF synchronously after each packet (on_iter) with no + // wall-clock pacing. Feed+process are strictly serialized, so + // the run is reproducible and matches the reference Point-LIO's + // single-executor semantics. The two-thread realtime path + // (else branch) leaves which IMU samples are present at scan + // composition up to wall-clock scheduling — that interleaving + // race makes even clean data diverge nondeterministically. + rep.realtime = false; + rep.on_iter = [&]() { + auto now_opt = virtual_now(); + if (now_opt.has_value()) run_main_iter(*now_opt); + }; + } else { + // Live-throughput path: feeder paced at wall-clock rate, the + // main thread drives run_main_iter. Used for wall-clock replay + // and dual-thread live-race reproduction. + rep.realtime = true; + } rep.skip_until_ns = replay_skip_until_ns; rep.dual_thread = replay_dual_thread; rep.run(); @@ -934,7 +1059,20 @@ int main(int argc, char** argv) { if (debug) printf("[fastlio2] SDK started, waiting for device...\n"); } + // Bag-frame replay always drives run_main_iter from the feeder thread + // (step() after each feed), so the main thread must stay out of the EKF + // regardless of the deterministic_clock flag — otherwise both threads + // co-drive run_main_iter and race on the shared measurement cloud. + const bool serial_replay = + g_replay_mode.load() && !replay_dual_thread && + (g_deterministic_clock.load() || !replay_bagframes.empty()); while (g_running.load()) { + if (serial_replay) { + // The feeder thread's on_iter drives run_main_iter; the main + // thread only services LCM and waits for the feeder to finish. + lcm.handleTimeout(10); + continue; + } auto loop_start = std::chrono::high_resolution_clock::now(); auto now_opt = virtual_now(); if (!now_opt.has_value()) { diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/replay_ruwik2_pt3.py b/dimos/hardware/sensors/lidar/pointlio/tools/replay_ruwik2_pt3.py index bf20ec36e1..e4c6058cf3 100644 --- a/dimos/hardware/sensors/lidar/pointlio/tools/replay_ruwik2_pt3.py +++ b/dimos/hardware/sensors/lidar/pointlio/tools/replay_ruwik2_pt3.py @@ -52,7 +52,7 @@ _ATTEMPT_DIR_ENV = "_REPLAY_POINTLIO_RUWIK2_PT3_ATTEMPT_DIR" -MAX_WALL_SEC = 480.0 +MAX_WALL_SEC = float(os.environ.get("REPLAY_MAX_WALL_SEC", "480.0")) REPLAY_MAX_SENSOR_SEC = float(os.environ.get("REPLAY_MAX_SENSOR_SEC", "60.0")) From b35d0f2ef4a5b1a933f5194ae6e02dc628425abe Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 10 Jun 2026 17:16:55 -0700 Subject: [PATCH 036/185] pointlio_native: pin fast-lio to local path so production nix build consumes the iVox fix The iVox-port + divergence fixes live in dimos-module-fastlio2 @ pointlio commit 02d5066, which is committed locally but NOT pushed (per the task's no-push constraint). The flake's fast-lio input was pointing at github:dimensionalOS/dimos-module-fastlio2/pointlio, whose lock pinned the pre-fix rev 7e5d88f. Since CMakeLists.txt already drops ikd_Tree.cpp (iVox replaces ikd-Tree), building against that stale github source fails with KD_TREE undefined-reference linker errors. Repoint fast-lio at path:/home/dimos/repos/dimos-module-fastlio2 and bump the lock so `nix build .#pointlio_native` consumes the local 02d5066 source. Verified: build exits 0, no KD_TREE errors, 661176-byte binary. NOTE: this bakes a machine-specific absolute path into the lock. Once Repo A's pointlio branch is pushed, switch this input back to github:dimensionalOS/dimos-module-fastlio2/pointlio and bump the lock to 02d5066 for portability. --- .../sensors/lidar/pointlio/cpp/flake.lock | 15 +++++---------- .../hardware/sensors/lidar/pointlio/cpp/flake.nix | 2 +- 2 files changed, 6 insertions(+), 11 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock index 670a10a9aa..a6309246a4 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock @@ -37,18 +37,13 @@ "fast-lio": { "flake": false, "locked": { - "lastModified": 1780625608, - "narHash": "sha256-r21B0goiQr8VnxcF9RCxhLaZBihfWQSpaVLAY0K18Xw=", - "owner": "dimensionalOS", - "repo": "dimos-module-fastlio2", - "rev": "7e5d88fcb3c22e56f426bc07ade1bd8afa02129c", - "type": "github" + "narHash": "sha256-FJpt9RSnrkTdOJ35X8XIGDeOlwtm/KTNHQxCijGr10w=", + "path": "/home/dimos/repos/dimos-module-fastlio2", + "type": "path" }, "original": { - "owner": "dimensionalOS", - "ref": "pointlio", - "repo": "dimos-module-fastlio2", - "type": "github" + "path": "/home/dimos/repos/dimos-module-fastlio2", + "type": "path" } }, "flake-utils": { diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix index 0d174f6e24..3f66794108 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix @@ -13,7 +13,7 @@ flake = false; }; fast-lio = { - url = "github:dimensionalOS/dimos-module-fastlio2/pointlio"; + url = "path:/home/dimos/repos/dimos-module-fastlio2"; flake = false; }; lcm-extended = { From 4303fd6f413a3b12ee3a300e862a3fb07bcbe008 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 10 Jun 2026 20:44:05 -0700 Subject: [PATCH 037/185] pointlio_native: repoint fast-lio at pushed github pointlio (02d5066) Now that dimos-module-fastlio2 pointlio (02d5066, the iVox port + divergence fix) is on origin, switch the fast-lio flake input from the local path: pin back to github:dimensionalOS/dimos-module-fastlio2/pointlio and bump the lock to rev 02d5066. This makes jeff/feat/pointlio_native self-contained and portable (no machine-specific path in the lock). Verified: nix build .#pointlio_native exits 0, no KD_TREE errors. --- .../sensors/lidar/pointlio/cpp/flake.lock | 15 ++++++++++----- .../hardware/sensors/lidar/pointlio/cpp/flake.nix | 2 +- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock index a6309246a4..a26ebea325 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock @@ -37,13 +37,18 @@ "fast-lio": { "flake": false, "locked": { - "narHash": "sha256-FJpt9RSnrkTdOJ35X8XIGDeOlwtm/KTNHQxCijGr10w=", - "path": "/home/dimos/repos/dimos-module-fastlio2", - "type": "path" + "lastModified": 1781136428, + "narHash": "sha256-rqzroZAzhQ7A+wOGsN7Qzk64p7m3+4doyb92fyLAOZE=", + "owner": "dimensionalOS", + "repo": "dimos-module-fastlio2", + "rev": "02d506674b8e532d6fc094214ea76c958dbfcbcb", + "type": "github" }, "original": { - "path": "/home/dimos/repos/dimos-module-fastlio2", - "type": "path" + "owner": "dimensionalOS", + "ref": "pointlio", + "repo": "dimos-module-fastlio2", + "type": "github" } }, "flake-utils": { diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix index 3f66794108..0d174f6e24 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix @@ -13,7 +13,7 @@ flake = false; }; fast-lio = { - url = "path:/home/dimos/repos/dimos-module-fastlio2"; + url = "github:dimensionalOS/dimos-module-fastlio2/pointlio"; flake = false; }; lcm-extended = { From 360133c4a055ed72562f0c1a16b03adc834bb5ce Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 11 Jun 2026 22:34:27 -0700 Subject: [PATCH 038/185] pointlio: pcap_to_db time-aligns pointlio_lidar + odometry into a db, add --force Extends pcap_to_db.py to populate both pointlio_odometry and pointlio_lidar streams (start-aligned onto the db's earliest ts) so pointlio can be compared against fastlio in one recording. --force overwrites existing pointlio streams. Untracks the machine-specific fastlio_test/setup_network symlinks (enhance overlay) and drops the empty tools/__init__.py. --- .../sensors/lidar/pointlio/fastlio_test.py | 1 - .../sensors/lidar/pointlio/setup_network.py | 1 - .../sensors/lidar/pointlio/tools/__init__.py | 0 .../lidar/pointlio/tools/pcap_to_db.py | 107 ++++++++++++++---- 4 files changed, 83 insertions(+), 26 deletions(-) delete mode 120000 dimos/hardware/sensors/lidar/pointlio/fastlio_test.py delete mode 120000 dimos/hardware/sensors/lidar/pointlio/setup_network.py delete mode 100644 dimos/hardware/sensors/lidar/pointlio/tools/__init__.py diff --git a/dimos/hardware/sensors/lidar/pointlio/fastlio_test.py b/dimos/hardware/sensors/lidar/pointlio/fastlio_test.py deleted file mode 120000 index 89f5f294ea..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/fastlio_test.py +++ /dev/null @@ -1 +0,0 @@ -/Users/jeffhykin/repos/dimos6/.ignore.enhance/items/dimos/hardware/sensors/lidar/fastlio2/fastlio_test.py \ No newline at end of file diff --git a/dimos/hardware/sensors/lidar/pointlio/setup_network.py b/dimos/hardware/sensors/lidar/pointlio/setup_network.py deleted file mode 120000 index 60620716ac..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/setup_network.py +++ /dev/null @@ -1 +0,0 @@ -/Users/jeffhykin/repos/dimos6/.ignore.enhance/items/dimos/hardware/sensors/lidar/fastlio2/setup_network.py \ No newline at end of file diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/__init__.py b/dimos/hardware/sensors/lidar/pointlio/tools/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py index 1313e3b8c1..b5b72eb470 100644 --- a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py @@ -12,12 +12,20 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Run Point-LIO over a .pcap and append the odometry into an existing .db. +"""Run Point-LIO over a .pcap and append its outputs into an existing .db. Given a Livox Mid-360 pcap capture and a memory2 SQLite database, this streams the pcap through the Point-LIO native module (deterministic clock, single feeder --> never loads the whole pcap into memory) and writes a ``pointlio_odometry`` -stream into the database at the native publish rate (~30 Hz). +-> never loads the whole pcap into memory) and writes two streams into the +database, both time-aligned onto the db's existing clock: + +* ``pointlio_odometry`` -- the IESKF pose at the native odom rate (~30 Hz). +* ``pointlio_lidar`` -- the registered (deskewed, odom-frame) point cloud at the + native pointcloud rate (~10 Hz). + +If either stream already exists in the db the run aborts, unless ``--force`` is +given, in which case the existing ``pointlio_odometry`` and ``pointlio_lidar`` +streams are dropped before the new ones are written. Timing conversion ----------------- @@ -53,6 +61,7 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 # Below this the db's existing timestamps are sensor-boot seconds, not unix time. _SENSOR_CLOCK_MAX = 1e8 @@ -75,19 +84,23 @@ class RecConfig(ModuleConfig): class Rec(Module): - """Append Point-LIO odometry into an existing SQLite db with ts conversion.""" + """Append Point-LIO odometry + lidar into an existing SQLite db with ts conversion.""" config: RecConfig pointlio_odometry: In[Odometry] + pointlio_lidar: In[PointCloud2] _offset: float | None = None - _last_ts: float = 0.0 - _count: int = 0 + _last_odom_ts: float = 0.0 + _last_lidar_ts: float = 0.0 + _odom_count: int = 0 + _lidar_count: int = 0 async def main(self) -> AsyncIterator[None]: from dimos.memory2.store.sqlite import SqliteStore self._store = SqliteStore(path=self.config.db_path) self._os = self._store.stream("pointlio_odometry", Odometry) + self._ls = self._store.stream("pointlio_lidar", PointCloud2) yield self._store.stop() @@ -102,16 +115,27 @@ def _resolve_offset(self, first_ts: float) -> float: # db on wall-clock time -> start-align Point-LIO onto the db's earliest ts. return ref - first_ts - async def handle_pointlio_odometry(self, v: Odometry) -> None: - raw_ts = getattr(v, "ts", None) or time.time() + def _aligned_ts(self, raw_ts: float, last_ts: float) -> float: + """Convert a sensor ts onto the db clock, kept strictly above last_ts.""" if self._offset is None: self._offset = self._resolve_offset(raw_ts) - ts = max(raw_ts + self._offset, self._last_ts + _EPS) - self._last_ts = ts + return max(raw_ts + self._offset, last_ts + _EPS) + + async def handle_pointlio_odometry(self, v: Odometry) -> None: + raw_ts = getattr(v, "ts", None) or time.time() + ts = self._aligned_ts(raw_ts, self._last_odom_ts) + self._last_odom_ts = ts pose = getattr(v, "pose", None) pose_inner = getattr(pose, "pose", None) if pose is not None else None self._os.append(v, ts=ts, pose=pose_inner) - self._count += 1 + self._odom_count += 1 + + async def handle_pointlio_lidar(self, v: PointCloud2) -> None: + raw_ts = getattr(v, "ts", None) or time.time() + ts = self._aligned_ts(raw_ts, self._last_lidar_ts) + self._last_lidar_ts = ts + self._ls.append(v, ts=ts) + self._lidar_count += 1 def _db_ref_start_ts(db_path: Path) -> float: @@ -128,10 +152,15 @@ def _db_ref_start_ts(db_path: Path) -> float: for table in tables: if table.startswith("_") or table.startswith("sqlite_"): continue - cols = [c[1] for c in con.execute(f"PRAGMA table_info('{table}')").fetchall()] - if "ts" not in cols: + try: + # vec0/rtree virtual tables (sqlite-vec etc.) raise "no such + # module" here when the extension isn't loaded -- skip them. + cols = [c[1] for c in con.execute(f"PRAGMA table_info('{table}')").fetchall()] + if "ts" not in cols: + continue + row = con.execute(f"SELECT MIN(ts) FROM '{table}'").fetchone() + except sqlite3.OperationalError: continue - row = con.execute(f"SELECT MIN(ts) FROM '{table}'").fetchone() if row and row[0] is not None: best = row[0] if best is None else min(best, row[0]) return best if best is not None else -1.0 @@ -139,14 +168,14 @@ def _db_ref_start_ts(db_path: Path) -> float: con.close() -def _odom_stats(db_path: Path) -> tuple[int, float, float]: - """(count, min_ts, max_ts) for the pointlio_odometry table; zeros if absent.""" +def _table_stats(db_path: Path, table: str) -> tuple[int, float, float]: + """(count, min_ts, max_ts) for a stream table; zeros if absent.""" if not db_path.exists(): return 0, 0.0, 0.0 con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) try: try: - row = con.execute("SELECT COUNT(*), MIN(ts), MAX(ts) FROM pointlio_odometry").fetchone() + row = con.execute(f"SELECT COUNT(*), MIN(ts), MAX(ts) FROM '{table}'").fetchone() except sqlite3.OperationalError: return 0, 0.0, 0.0 cnt = row[0] or 0 @@ -168,6 +197,24 @@ def _run(args: argparse.Namespace) -> int: from dimos.core.coordination.blueprints import autoconnect from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.hardware.sensors.lidar.pointlio.module import PointLio + from dimos.memory2.store.sqlite import SqliteStore + + pointlio_streams = ("pointlio_odometry", "pointlio_lidar") + store = SqliteStore(path=str(db_path)) + try: + existing = sorted(set(store.list_streams()) & set(pointlio_streams)) + if existing and not args.force: + print( + f"[pcap_to_db] {db_path.name} already has {existing}; pass --force to overwrite", + file=sys.stderr, + ) + return 2 + for name in existing: + store.delete_stream(name) + if existing: + print(f"[pcap_to_db] --force: dropped existing {existing}", flush=True) + finally: + store.stop() ref_start_ts = _db_ref_start_ts(db_path) time_offset = float("nan") if args.time_offset is None else args.time_offset @@ -194,7 +241,12 @@ def _run(args: argparse.Namespace) -> int: deterministic_clock=True, replay_dual_thread=False, debug=False, - ).remappings([(PointLio, "odometry", "pointlio_odometry")]), + ).remappings( + [ + (PointLio, "odometry", "pointlio_odometry"), + (PointLio, "lidar", "pointlio_lidar"), + ] + ), Rec.blueprint( db_path=str(db_path), ref_start_ts=ref_start_ts, @@ -210,7 +262,7 @@ def _run(args: argparse.Namespace) -> int: try: while True: time.sleep(_POLL_SEC) - cnt, min_ts, max_ts = _odom_stats(db_path) + cnt, min_ts, max_ts = _table_stats(db_path, "pointlio_odometry") if cnt == 0: continue if first_max is None: @@ -232,11 +284,13 @@ def _run(args: argparse.Namespace) -> int: finally: coord.stop() - cnt, min_ts, max_ts = _odom_stats(db_path) - span = max_ts - min_ts + o_cnt, o_min, o_max = _table_stats(db_path, "pointlio_odometry") + l_cnt = _table_stats(db_path, "pointlio_lidar")[0] + span = o_max - o_min print( - f"[pcap_to_db] done rows={cnt} ts=[{min_ts:.3f}, {max_ts:.3f}] " - f"span={span:.1f}s wall={time.time() - t0:.1f}s", + f"[pcap_to_db] done odom={o_cnt} lidar={l_cnt} " + f"ts=[{o_min:.3f}, {o_max:.3f}] span={span:.1f}s " + f"wall={time.time() - t0:.1f}s", flush=True, ) return 0 @@ -262,7 +316,12 @@ def main(argv: list[str]) -> int: "--time-offset", type=float, default=None, - help="seconds added to every odom ts; omit to auto-derive from the db clock", + help="seconds added to every output ts; omit to auto-derive from the db clock", + ) + parser.add_argument( + "--force", + action="store_true", + help="overwrite existing pointlio_odometry/pointlio_lidar streams in the db", ) return _run(parser.parse_args(argv)) From 0f4cc4c995133e8dae497b4324ecfb4c10f288fe Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 12 Jun 2026 04:19:24 -0700 Subject: [PATCH 039/185] update --- .../sensors/lidar/fastlio2/recorder.py | 172 +++++++++++++++--- 1 file changed, 151 insertions(+), 21 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/recorder.py b/dimos/hardware/sensors/lidar/fastlio2/recorder.py index bfbd4edcdb..2289c74830 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/recorder.py +++ b/dimos/hardware/sensors/lidar/fastlio2/recorder.py @@ -12,9 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. +import asyncio from datetime import datetime import os from pathlib import Path +import re import shlex import shutil import signal @@ -36,28 +38,30 @@ def _stamp() -> str: now = datetime.now() - return now.strftime("%Y-%m-%d") + "_" + now.strftime("%-I-%M%p").lower() + "-PST" + return now.strftime("%Y-%m-%d") + "_" + now.strftime("%I-%M%p").lower() + "-PST" -def _stop_when_parent_dies(cmd: list[str]) -> list[str]: +def _stop_when_parent_dies(cmd: list[str], grace_sec: float) -> list[str]: + """Reap cmd if the recorder dies, including via SIGKILL (which it can't + intercept) — otherwise tcpdump's own session would outlive it.""" parent_pid = os.getpid() quoted = " ".join(shlex.quote(arg) for arg in cmd) - # script to work on macos and linux (theres a cleaner just-linux solution) + # Foreground waits on tcpdump so a startup failure propagates its exit code. script = textwrap.dedent(f""" {quoted} & child=$! - trap 'kill -INT "$child" 2>/dev/null' INT TERM ( while kill -0 {parent_pid} 2>/dev/null; do sleep 0.5 done kill -INT "$child" 2>/dev/null + sleep {grace_sec} + kill -KILL "$child" 2>/dev/null ) & watcher=$! wait "$child" code=$? kill "$watcher" 2>/dev/null - wait "$watcher" 2>/dev/null exit $code """).strip() return ["bash", "-c", script] @@ -83,6 +87,9 @@ class FastLio2RecorderConfig(RecorderConfig): record_pcap_iface: str = "enp2s0" record_pcap_snaplen: int = 2048 lidar_ip: str = "192.168.1.107" + # Grace period for each stop signal (SIGINT→SIGTERM→SIGKILL) when tearing + # down the tcpdump capture. + pcap_stop_timeout: float = 5.0 def model_post_init(self, __context: object) -> None: super().model_post_init(__context) @@ -111,6 +118,12 @@ class FastLio2Recorder(Recorder): # tcpdump fails fast (EPERM, bad iface) within a few ms; pause briefly so poll() catches that. _TCPDUMP_STARTUP_PROBE_SEC: float = 0.3 + # How long to let tcpdump run before declaring the capture dead if nothing landed. + _PCAP_WATCHDOG_SEC: float = 5.0 + # A libpcap file with zero packets is just its 24-byte global header. + _PCAP_GLOBAL_HEADER_BYTES: int = 24 + # How long the diagnostic sniff listens for *any* UDP source on the iface. + _PCAP_DIAGNOSTIC_SNIFF_SEC: float = 3.0 _pcap_proc: subprocess.Popen[bytes] | None = None @@ -120,11 +133,15 @@ def start(self) -> None: if self.config.record_pcap: self._start_pcap() super().start() + if self.config.record_pcap and self._pcap_proc is not None: + self.spawn(self._pcap_watchdog()) @rpc def stop(self) -> None: - super().stop() - self._stop_pcap() + try: + super().stop() + finally: + self._stop_pcap() def _start_pcap(self) -> None: cfg = self.config @@ -147,8 +164,10 @@ def _start_pcap(self) -> None: packet_filter_expression, ] + # Own session/group so _stop_pcap can signal the wrapper + tcpdump + # without touching the recorder, and Ctrl-C doesn't race shutdown. proc = subprocess.Popen( - _stop_when_parent_dies(cmd), + _stop_when_parent_dies(cmd, cfg.pcap_stop_timeout), stdout=subprocess.DEVNULL, stderr=subprocess.PIPE, start_new_session=True, @@ -177,6 +196,114 @@ def _start_pcap(self) -> None: ) self._pcap_proc = proc + async def _pcap_watchdog(self) -> None: + """If tcpdump captured nothing after a few seconds, dump everything we + know about why — almost always a wrong lidar_ip or interface.""" + await asyncio.sleep(self._PCAP_WATCHDOG_SEC) + proc = self._pcap_proc + if proc is None: + return + path = Path(self.config.pcap_path).expanduser() + try: + size = path.stat().st_size + except OSError: + size = -1 + if size > self._PCAP_GLOBAL_HEADER_BYTES: + logger.info( + f"FastLio2Recorder pcap healthy — {size} bytes captured in " + f"{self._PCAP_WATCHDOG_SEC:.0f}s path={path}" + ) + return + report = await asyncio.to_thread(self._build_empty_pcap_report, size, proc) + logger.error(report) + print(report, flush=True) + + def _build_empty_pcap_report(self, size: int, proc: subprocess.Popen[bytes]) -> str: + cfg = self.config + packet_filter_expression = f"src host {cfg.lidar_ip} and udp" + proc_alive = proc.poll() is None + stderr_text = "" + if not proc_alive and proc.stderr is not None: + try: + stderr_text = proc.stderr.read().decode(errors="replace").strip() + except (OSError, ValueError): + stderr_text = "" + + observed = self._observed_udp_sources() + if observed: + listing = "\n".join( + f" {source} ({count} pkts)" + for source, count in sorted(observed.items(), key=lambda kv: kv[1], reverse=True) + ) + diagnosis = ( + f" UDP traffic IS flowing on {cfg.record_pcap_iface}, but from other source(s):\n" + f"{listing}\n" + f" None matched 'src host {cfg.lidar_ip}'. The lidar_ip is almost certainly\n" + f" wrong — set LIDAR_IP to whichever address above is the lidar and restart." + ) + else: + diagnosis = ( + f" NO UDP traffic at all was seen on {cfg.record_pcap_iface} during a " + f"{self._PCAP_DIAGNOSTIC_SNIFF_SEC:.0f}s probe.\n" + f" Wrong interface, unplugged cable, or the lidar is powered off." + ) + + neigh = self._run_quiet(["ip", "neigh", "show", cfg.lidar_ip]).strip() + return textwrap.dedent(f""" + ============================================================================ + [go2_record] PCAP WATCHDOG: 0 packets captured after {self._PCAP_WATCHDOG_SEC:.0f}s + ============================================================================ + Recording is enabled but tcpdump wrote an EMPTY pcap (size={size} bytes; an + empty libpcap file is {self._PCAP_GLOBAL_HEADER_BYTES} bytes of global header). + + Capture config: + interface : {cfg.record_pcap_iface} + lidar_ip : {cfg.lidar_ip} + filter : {packet_filter_expression!r} + pcap_path : {cfg.pcap_path} + tcpdump : alive={proc_alive} pid={proc.pid}{f" stderr={stderr_text!r}" if stderr_text else ""} + + Diagnosis: + {diagnosis} + + arp/neigh for {cfg.lidar_ip}: {neigh or ""} + ============================================================================ + """).strip() + + def _observed_udp_sources(self) -> dict[str, int]: + """Sniff the interface briefly and tally which source IPs are sending UDP.""" + tcpdump = shutil.which("tcpdump") or "tcpdump" + cmd = [tcpdump, "-i", self.config.record_pcap_iface, "-nn", "-c", "60", "udp"] + try: + result = subprocess.run( + cmd, + capture_output=True, + text=True, + timeout=self._PCAP_DIAGNOSTIC_SNIFF_SEC, + ) + output = result.stdout + except subprocess.TimeoutExpired as expired: + stdout = expired.stdout + output = ( + stdout.decode(errors="replace") if isinstance(stdout, bytes) else (stdout or "") + ) + except OSError: + return {} + counts: dict[str, int] = {} + for line in output.splitlines(): + match = re.search(r"\bIP6?\s+(\S+?)\.\d+\s+>", line) + if match: + source = match.group(1) + counts[source] = counts.get(source, 0) + 1 + return counts + + @staticmethod + def _run_quiet(cmd: list[str]) -> str: + try: + return subprocess.run(cmd, capture_output=True, text=True, timeout=2.0).stdout + except (OSError, subprocess.TimeoutExpired): + return "" + def _stop_pcap(self) -> None: proc = self._pcap_proc if proc is None: @@ -184,22 +311,25 @@ def _stop_pcap(self) -> None: self._pcap_proc = None if proc.poll() is not None: return - # SIGINT is tcpdump's documented "stop cleanly" signal — it prints - # packet counts and flushes the pcap header. - proc.send_signal(signal.SIGINT) + # Signal the group so tcpdump gets it directly. SIGINT is its + # clean-stop signal (flushes the pcap); escalate if it hangs. try: - proc.wait(timeout=self.config.shutdown_timeout) - except subprocess.TimeoutExpired: + pgid = os.getpgid(proc.pid) + except ProcessLookupError: + return + timeout = self.config.pcap_stop_timeout + for sig in (signal.SIGINT, signal.SIGTERM, signal.SIGKILL): try: - os.killpg(os.getpgid(proc.pid), signal.SIGTERM) + os.killpg(pgid, sig) except ProcessLookupError: - pass + break try: - proc.wait(timeout=self.config.shutdown_timeout) + proc.wait(timeout=timeout) + break except subprocess.TimeoutExpired: - try: - os.killpg(os.getpgid(proc.pid), signal.SIGKILL) - except ProcessLookupError: - pass - proc.wait() + logger.warning( + f"tcpdump did not exit on {sig.name}; escalating path={self.config.pcap_path}" + ) + else: + proc.wait() logger.info(f"FastLio2Recorder pcap recording stopped path={self.config.pcap_path}") From d08f5550cf49dfd80a7e0c6dede688bbc4a2791d Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 12 Jun 2026 04:52:16 -0700 Subject: [PATCH 040/185] perf: stream point clouds/images during .rrd generation to bound memory Iterate lidar/image streams lazily instead of .to_list() in the rrd post-process pipeline (apriltags, lidar_reanchor, build_rrd map logging), which previously loaded the whole recording into RAM and got OOM-killed on large captures. build_rrd now downsamples per-frame and compacts the running map periodically. Also raise ValueError (not SystemExit) for an empty pose stream so correct_db skips that reanchor pair gracefully. --- dimos/mapping/recording/utils/apriltags.py | 7 ++-- dimos/mapping/recording/utils/build_rrd.py | 36 ++++++++++++++++--- .../mapping/recording/utils/lidar_reanchor.py | 4 +-- 3 files changed, 37 insertions(+), 10 deletions(-) diff --git a/dimos/mapping/recording/utils/apriltags.py b/dimos/mapping/recording/utils/apriltags.py index 306de707f8..723148c3ee 100644 --- a/dimos/mapping/recording/utils/apriltags.py +++ b/dimos/mapping/recording/utils/apriltags.py @@ -133,8 +133,9 @@ def detect_apriltags( medoid representative per cluster. Returns that list of representatives.""" detector = make_detector(dictionary) raw_detections: list[dict] = [] - images = store.stream(image_stream, Image).to_list() - for image_obs in images: + image_count = 0 + for image_obs in store.stream(image_stream, Image): + image_count += 1 image = image_obs.data bgr = image.numpy() if hasattr(image, "numpy") else np.asarray(image.data) all_corners, marker_ids, _ = detector.detectMarkers(bgr) @@ -197,6 +198,6 @@ def detect_apriltags( found_ids = sorted({detection["marker_id"] for detection in detections}) print( f" april_tags: {len(raw_detections)} raw -> {len(kept)} in-spec " - f"-> {len(detections)} clusters, markers {found_ids} (over {len(images)} images)" + f"-> {len(detections)} clusters, markers {found_ids} (over {image_count} images)" ) return detections diff --git a/dimos/mapping/recording/utils/build_rrd.py b/dimos/mapping/recording/utils/build_rrd.py index ddccfdce33..5965777a23 100644 --- a/dimos/mapping/recording/utils/build_rrd.py +++ b/dimos/mapping/recording/utils/build_rrd.py @@ -116,15 +116,41 @@ def _log_frames(store, stream, entity, stride, voxel, base): print(f" rrd: {entity} <- {stream} ({n} frames, stride {stride}, voxel {voxel}m)") +# Compact the running map once this many uncompacted points accumulate. Bounds +# peak memory to ~(final map + this) instead of every frame's full-res points. +_MAP_COMPACT_THRESHOLD = 5_000_000 + + def _log_map(store, stream, entity, voxel, base): if stream not in store.list_streams(): return - chunks = [ - c for c in (obs.data.points_f32() for obs in store.stream(stream, PointCloud2)) if len(c) - ] - if not chunks: + running: np.ndarray | None = None + pending: list[np.ndarray] = [] + pending_count = 0 + + def compact(parts: list[np.ndarray]) -> np.ndarray | None: + if running is not None: + parts = [running, *parts] + if not parts: + return running + merged = np.concatenate(parts) + return _down(merged, voxel) if voxel > 0 else merged + + for obs in store.stream(stream, PointCloud2): + cloud = obs.data.points_f32() + if len(cloud) == 0: + continue + # Per-frame downsample collapses each dense cloud to its voxel footprint, + # so we never hold all frames' raw points at once. + pending.append(_down(cloud, voxel) if voxel > 0 else cloud) + pending_count += len(pending[-1]) + if pending_count >= _MAP_COMPACT_THRESHOLD: + running = compact(pending) + pending, pending_count = [], 0 + + pts = compact(pending) + if pts is None or len(pts) == 0: return - pts = _down(np.concatenate(chunks), voxel) rr.log(entity, rr.Points3D(pts, colors=_shaded(pts, base)), static=True) print(f" rrd: {entity} <- {stream} ({len(pts):,} pts, voxel {voxel}m)") diff --git a/dimos/mapping/recording/utils/lidar_reanchor.py b/dimos/mapping/recording/utils/lidar_reanchor.py index 1d9cc559c2..8a2bd5253a 100644 --- a/dimos/mapping/recording/utils/lidar_reanchor.py +++ b/dimos/mapping/recording/utils/lidar_reanchor.py @@ -48,7 +48,7 @@ def _load_poses(db_path: str, stream: str): ).fetchall() conn.close() if not rows: - raise SystemExit(f"no populated poses in stream '{stream}'") + raise ValueError(f"no populated poses in stream '{stream}'") return np.array([r[0] for r in rows]), np.array([r[1:8] for r in rows], dtype=np.float64) @@ -87,7 +87,7 @@ def reanchor_stream( odom_M = [None] * len(odom_ts) gt_M = [None] * len(gt_ts) - src = store.stream(lidar_stream, PointCloud2).to_list() + src = store.stream(lidar_stream, PointCloud2) if out_stream in store.list_streams(): store.delete_stream(out_stream) out = store.stream(out_stream, PointCloud2) From f4b25c99d316c48513c067988d53ab937ec7bced Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 12 Jun 2026 04:57:18 -0700 Subject: [PATCH 041/185] refactor: drop lidar reanchor step from recording post-process Lidar clouds are stored in correct world-frame poses by default now, so re-anchoring each cloud onto the gtsam-corrected trajectory was redundant. Remove lidar_reanchor and its _corrected outputs, the reanchor_pairs plumbing through the post-process runner and both rig wrappers, and the corrected_* cloud/map rendering in build_rrd. The gtsam_odom groundtruth trajectory (AprilTag placement + green path) is unchanged. --- .../recording/go2_mid360/post_process.py | 7 +- .../mid360_realsense/post_process.py | 5 +- dimos/mapping/recording/utils/build_rrd.py | 32 +++-- .../mapping/recording/utils/lidar_reanchor.py | 114 ------------------ dimos/mapping/recording/utils/post_process.py | 40 ++---- 5 files changed, 25 insertions(+), 173 deletions(-) delete mode 100644 dimos/mapping/recording/utils/lidar_reanchor.py diff --git a/dimos/mapping/recording/go2_mid360/post_process.py b/dimos/mapping/recording/go2_mid360/post_process.py index d257eee8d9..19ac13e476 100755 --- a/dimos/mapping/recording/go2_mid360/post_process.py +++ b/dimos/mapping/recording/go2_mid360/post_process.py @@ -38,11 +38,6 @@ GO2_FRONT_CAMERA_RESOLUTION, ) -# Lidar/odom pairs that may be re-anchored onto gtsam_odom — only when their odom -# is the same frame family gtsam was built from. The legacy Go2 onboard -# `lidar`/`odom` is a different estimator frame -> left as-is. -REANCHOR_PAIRS = [("go2_lidar", "go2_odom"), ("fastlio_lidar", "fastlio_odometry")] - def load_camera(db: Path) -> CameraParams: return ( @@ -54,4 +49,4 @@ def load_camera(db: Path) -> CameraParams: if __name__ == "__main__": - run(description=__doc__, reanchor_pairs=REANCHOR_PAIRS, load_camera=load_camera) + run(description=__doc__, load_camera=load_camera) diff --git a/dimos/mapping/recording/mid360_realsense/post_process.py b/dimos/mapping/recording/mid360_realsense/post_process.py index 6c47d73612..78b2fc788d 100644 --- a/dimos/mapping/recording/mid360_realsense/post_process.py +++ b/dimos/mapping/recording/mid360_realsense/post_process.py @@ -37,9 +37,6 @@ from dimos.memory2.store.sqlite import SqliteStore from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -# The RealSense + Mid-360 recording only has the fastlio lidar/odom pair; its odom -# is the same frame family gtsam was built from, so the re-anchor composes. -REANCHOR_PAIRS = [("fastlio_lidar", "fastlio_odometry")] CAMERA_INFO_STREAM = "realsense_camera_info" # camera_optical pose in mid360_link (the frame fastlio odom is anchored to), @@ -61,4 +58,4 @@ def load_camera(db: Path) -> CameraParams: if __name__ == "__main__": - run(description=__doc__, reanchor_pairs=REANCHOR_PAIRS, load_camera=load_camera) + run(description=__doc__, load_camera=load_camera) diff --git a/dimos/mapping/recording/utils/build_rrd.py b/dimos/mapping/recording/utils/build_rrd.py index 5965777a23..9711e9d202 100644 --- a/dimos/mapping/recording/utils/build_rrd.py +++ b/dimos/mapping/recording/utils/build_rrd.py @@ -16,12 +16,11 @@ mapping/loop_closure/utils/map_rrd.py), focused on the post-processed result. For each lidar stream it logs both the per-frame clouds and a single aggregated, -voxel-downsampled "map" (corrected fastlio re-anchored on `gtsam_odom`; the Go2 -`lidar` stays in its own `odom` frame). Clouds get a slight height-color -gradient; trajectories get a start->end gradient. Each AprilTag is placed in 3D -(via the corrected trajectory) with its detections, a labeled marker, basis- -vector axes at the perceived pose, and the robot's-eye camera image at the -recognition moment. +voxel-downsampled "map" (each cloud in its own world frame). Clouds get a slight +height-color gradient; trajectories get a start->end gradient. Each AprilTag is +placed in 3D (via the gtsam-corrected trajectory) with its detections, a labeled +marker, basis-vector axes at the perceived pose, and the robot's-eye camera image +at the recognition moment. """ from __future__ import annotations @@ -443,9 +442,7 @@ def build_rrd( f"/world/{m}": rrb.EntityBehavior(visible=False) for m in ( "go2_map", - "corrected_go2_map", "fastlio_map", - "corrected_fastlio_map", "onboard_map", ) } @@ -475,17 +472,16 @@ def build_rrd( ) ci = 0 # rotate a distinct base color through each point cloud - for raw, corr, name in [ - ("go2_lidar", "go2_lidar_corrected", "go2"), # new: correct transforms - ("fastlio_lidar", "fastlio_lidar_corrected", "fastlio"), # legacy mid360 - ("lidar", None, "onboard"), # legacy Go2 onboard L1, own frame + for name, stream in [ + ("go2", "go2_lidar"), + ("fastlio", "fastlio_lidar"), + ("onboard", "lidar"), # legacy Go2 onboard L1, own frame ]: - for stream, ent in [(raw, name), (corr, f"corrected_{name}")]: - if stream and stream in streams: - base = _CLOUD_PALETTE[ci % len(_CLOUD_PALETTE)] - ci += 1 - _log_frames(store, stream, f"world/{ent}_lidar", cloud_stride, map_voxel, base) - _log_map(store, stream, f"world/{ent}_map", map_voxel, base) + if stream in streams: + base = _CLOUD_PALETTE[ci % len(_CLOUD_PALETTE)] + ci += 1 + _log_frames(store, stream, f"world/{name}_lidar", cloud_stride, map_voxel, base) + _log_map(store, stream, f"world/{name}_map", map_voxel, base) _log_apriltags(store, db_path, cam_xform, intrinsics, resolution) diff --git a/dimos/mapping/recording/utils/lidar_reanchor.py b/dimos/mapping/recording/utils/lidar_reanchor.py deleted file mode 100644 index 8a2bd5253a..0000000000 --- a/dimos/mapping/recording/utils/lidar_reanchor.py +++ /dev/null @@ -1,114 +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. - -"""Re-anchor a world-frame lidar stream onto a corrected trajectory. - -A lidar stream is stored in world frame *relative to some odometry* (e.g. -`fastlio_lidar` is relative to `fastlio_odometry`, `lidar` is relative to the -Go2 onboard `odom`). When a drift-corrected trajectory exists (`gtsam_odom`, -from AprilTag landmark SLAM), each cloud can be re-anchored onto it: - - P_corrected = T_gtsam(t) · T_odom(t)^-1 · P_world - -i.e. subtract the cloud's own odometry (back to body) and re-apply the corrected -pose at the nearest timestamp. The actual point transform runs through -open3d via ``PointCloud2.transform``. -""" - -from __future__ import annotations - -import sqlite3 - -import numpy as np -from scipy.spatial.transform import Rotation - -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.msgs.sensor_msgs.PointCloud2 import PointCloud2 - - -def _load_poses(db_path: str, stream: str): - """(ts (N,), pose (N,7) x y z qx qy qz qw) for a pose-bearing stream, via SQL.""" - conn = sqlite3.connect(db_path) - rows = conn.execute( - f"SELECT ts,pose_x,pose_y,pose_z,pose_qx,pose_qy,pose_qz,pose_qw " - f'FROM "{stream}" WHERE pose_qw IS NOT NULL ORDER BY ts' - ).fetchall() - conn.close() - if not rows: - raise ValueError(f"no populated poses in stream '{stream}'") - return np.array([r[0] for r in rows]), np.array([r[1:8] for r in rows], dtype=np.float64) - - -def _mat(p): - M = np.eye(4) - M[:3, :3] = Rotation.from_quat(p[3:7]).as_matrix() - M[:3, 3] = p[:3] - return M - - -def _to_transform(M): - q = Rotation.from_matrix(M[:3, :3]).as_quat() - return Transform( - translation=Vector3(float(M[0, 3]), float(M[1, 3]), float(M[2, 3])), - rotation=Quaternion(float(q[0]), float(q[1]), float(q[2]), float(q[3])), - ) - - -def _nearest(ts_arr: np.ndarray, t: float) -> int: - j = int(np.searchsorted(ts_arr, t)) - j = min(max(j, 0), len(ts_arr) - 1) - if j > 0 and abs(ts_arr[j - 1] - t) < abs(ts_arr[j] - t): - j -= 1 - return j - - -def reanchor_stream( - store, db_path: str, *, lidar_stream: str, odom_stream: str, gtsam_stream: str, out_stream: str -) -> int: - """Write `out_stream`: every cloud of `lidar_stream` re-anchored from - `odom_stream` onto `gtsam_stream`. Returns the number of clouds written.""" - odom_ts, odom_pose = _load_poses(db_path, odom_stream) - gt_ts, gt_pose = _load_poses(db_path, gtsam_stream) - # precompute per-odom-node the corrected relative transform isn't possible - # (it's per-cloud by timestamp), but caching odom/gtsam matrices is cheap. - odom_M = [None] * len(odom_ts) - gt_M = [None] * len(gt_ts) - - src = store.stream(lidar_stream, PointCloud2) - if out_stream in store.list_streams(): - store.delete_stream(out_stream) - out = store.stream(out_stream, PointCloud2) - n = 0 - for obs in src: - t = obs.ts - i = _nearest(odom_ts, t) - j = _nearest(gt_ts, t) - if odom_M[i] is None: - odom_M[i] = _mat(odom_pose[i]) - if gt_M[j] is None: - gt_M[j] = _mat(gt_pose[j]) - rel = gt_M[j] @ np.linalg.inv(odom_M[i]) - cloud = obs.data - corrected = cloud.transform(_to_transform(rel)) # open3d under the hood - nc = PointCloud2.from_numpy( - corrected.points_f32(), timestamp=t, intensities=cloud.intensities_f32() - ) - out.append(nc, ts=t, pose=tuple(gt_pose[j])) - n += 1 - print( - f" {out_stream}: {n} clouds re-anchored ({lidar_stream} via {odom_stream} -> {gtsam_stream})" - ) - return n diff --git a/dimos/mapping/recording/utils/post_process.py b/dimos/mapping/recording/utils/post_process.py index 2657051dd0..ecc5e30983 100644 --- a/dimos/mapping/recording/utils/post_process.py +++ b/dimos/mapping/recording/utils/post_process.py @@ -18,14 +18,13 @@ 1. prints a recording sanity check (rec_check), 2. detects AprilTags -> `april_tags` stream (apriltags), 3. solves a drift-corrected trajectory -> `gtsam_odom` (gtsam_gt), - 4. re-anchors each lidar onto it -> `_corrected` (lidar_reanchor), - 5. writes a Rerun `.rrd` visualization (build_rrd). + 4. writes a Rerun `.rrd` visualization (build_rrd). A tag seen at several times pins the odometry chain and removes accumulated drift. Also writes `gtsam_odom.tum` next to each db (relocalization groundtruth). -Each rig calls `run()` with its own re-anchor pairs and a `load_camera(db)` -returning `(intrinsics, distortion, optical_in_base, resolution)`. +Each rig calls `run()` with a `load_camera(db)` returning +`(intrinsics, distortion, optical_in_base, resolution)`. """ from __future__ import annotations @@ -40,14 +39,12 @@ from dimos.mapping.recording.utils.apriltags import detect_apriltags from dimos.mapping.recording.utils.build_rrd import build_rrd from dimos.mapping.recording.utils.gtsam_gt import build_gtsam_gt, write_gtsam_odom -from dimos.mapping.recording.utils.lidar_reanchor import reanchor_stream from dimos.memory2.store.sqlite import SqliteStore DB_NAME = "mem2.db" # (intrinsics 3x3, distortion, optical_in_base [x,y,z,qx,qy,qz,qw], (width, height)) CameraParams = tuple[np.ndarray, np.ndarray, list[float], tuple[int, int]] -ReanchorPairs = list[tuple[str, str]] def _created_time(path: Path) -> float: @@ -92,15 +89,14 @@ def correct_db( intrinsics, distortion, optical_in_base, - reanchor_pairs: ReanchorPairs, image_stream, apriltag_stream, gtsam_stream, marker_length, dictionary, ): - """AprilTag detection -> GTSAM trajectory -> re-anchor lidar. Returns True if - a corrected trajectory was written.""" + """AprilTag detection -> GTSAM trajectory. Returns True if a corrected + trajectory was written.""" with SqliteStore(path=str(db)) as store: detections = detect_apriltags( store, intrinsics, distortion, image_stream, apriltag_stream, marker_length, dictionary @@ -111,20 +107,6 @@ def correct_db( trajectory = build_gtsam_gt(str(db), detections, optical_in_base) with SqliteStore(path=str(db)) as store: write_gtsam_odom(store, trajectory, gtsam_stream, db.parent / "gtsam_odom.tum") - stream_names = store.list_streams() - for lidar_stream, odom_stream in reanchor_pairs: - if lidar_stream in stream_names and odom_stream in stream_names: - try: - reanchor_stream( - store, - str(db), - lidar_stream=lidar_stream, - odom_stream=odom_stream, - gtsam_stream=gtsam_stream, - out_stream=f"{lidar_stream}_corrected", - ) - except Exception as error: - print(f" re-anchor {lidar_stream}_corrected failed: {error}") return True @@ -135,7 +117,6 @@ def process_db( distortion, optical_in_base, resolution, - reanchor_pairs: ReanchorPairs, image_stream, apriltag_stream, gtsam_stream, @@ -167,16 +148,15 @@ def process_db( already_corrected = gtsam_stream in store.list_streams() if no_gtsam: - print(" --no-gtsam: skipping AprilTag/GTSAM/re-anchor") + print(" --no-gtsam: skipping AprilTag/GTSAM") elif already_corrected and not force: - print(f" already has '{gtsam_stream}' — skipping AprilTag/GTSAM/re-anchor (use --force)") + print(f" already has '{gtsam_stream}' — skipping AprilTag/GTSAM (use --force)") else: correct_db( db, intrinsics=intrinsics, distortion=distortion, optical_in_base=optical_in_base, - reanchor_pairs=reanchor_pairs, image_stream=image_stream, apriltag_stream=apriltag_stream, gtsam_stream=gtsam_stream, @@ -204,7 +184,6 @@ def process_db( def run( *, description: str | None, - reanchor_pairs: ReanchorPairs, load_camera: Callable[[Path], CameraParams], ) -> None: """Parse CLI args and post-process each resolved recording. `load_camera` @@ -233,12 +212,12 @@ def run( parser.add_argument( "--check", action="store_true", - help="only sanity-check each recording and write summary.json (no GTSAM/re-anchor/.rrd)", + help="only sanity-check each recording and write summary.json (no GTSAM/.rrd)", ) parser.add_argument( "--no-gtsam", action="store_true", - help="skip AprilTag/GTSAM/re-anchor (e.g. rebuild only the .rrd)", + help="skip AprilTag/GTSAM (e.g. rebuild only the .rrd)", ) parser.add_argument("--no-rrd", action="store_true", help="skip the .rrd visualization step") parser.add_argument( @@ -278,7 +257,6 @@ def run( distortion=distortion, optical_in_base=optical_in_base, resolution=resolution, - reanchor_pairs=reanchor_pairs, image_stream=args.image_stream, apriltag_stream=args.apriltag_stream, gtsam_stream=args.gtsam_stream, From 961110591251d330061bd6cfe920e7fde5563f38 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 12 Jun 2026 17:29:38 -0700 Subject: [PATCH 042/185] fastlio2: offline pcap replay + pcap_to_db tool (mirror pointlio) Port the minimal pcap-replay subsystem from jeff/feat/go2_record into the clean branch so FAST-LIO can run offline from a Mid-360 pcap, matching the Point-LIO pcap_to_db workflow: - cpp: pcap_replay.hpp + timing.hpp (header-only), main.cpp refactored so the main loop runs from either the live SDK or a pcap feeder thread, with an optional deterministic sensor-clock mode. Keeps the clean branch's velocity-cap (guarded set_max_velocity_norm_ms) and flake (velocity-cap fast-lio); does not pull go2_record's tcpdump record path. - module.py: replay_pcap / replay_skip_until_ns / first_packet_marker / deterministic_clock config fields; skip network validation in replay mode. - tools/pcap_to_db.py: replay a pcap through FastLio2 (real-time, non- deterministic) and append fastlio_odometry + fastlio_lidar into an existing memory2 db, time-aligned onto its clock. --force overwrites. --- .../sensors/lidar/fastlio2/cpp/main.cpp | 437 ++++++++++++++---- .../lidar/fastlio2/cpp/pcap_replay.hpp | 222 +++++++++ .../sensors/lidar/fastlio2/cpp/timing.hpp | 128 +++++ .../hardware/sensors/lidar/fastlio2/module.py | 20 +- .../lidar/fastlio2/tools/pcap_to_db.py | 361 +++++++++++++++ 5 files changed, 1085 insertions(+), 83 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/pcap_replay.hpp create mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/timing.hpp create mode 100644 dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index 114ceabc60..5381852663 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -32,6 +33,8 @@ #include "cloud_filter.hpp" #include "dimos_native_module.hpp" +#include "pcap_replay.hpp" +#include "timing.hpp" #include "voxel_map.hpp" // dimos LCM message headers @@ -59,6 +62,79 @@ static std::atomic g_running{true}; static lcm::LCM* g_lcm = nullptr; static FastLio* g_fastlio = nullptr; +// Virtual clock: in replay mode, tracks the pcap timestamp of the packet +// currently being fed so publish_*() reports the original capture time +// instead of replay wall time. Live mode leaves it at 0 and publish_*() +// falls back to system_clock::now(). +static std::atomic g_replay_mode{false}; +static std::atomic g_virtual_clock_ns{0}; + +// Deterministic clock mode. When set, both live and replay drive +// g_virtual_clock_ns from the packet's sensor-clock pkt->timestamp (which +// is identical bit-for-bit between SDK delivery and pcap), and use it as +// the source for scan-boundary rate limits and publish timestamps. This +// removes wall-clock jitter from scan boundaries → live and replay produce +// the same algorithm state. Trade-off: published header.stamp values +// become sensor-boot-relative seconds instead of unix wall time, so this +// is off by default and only flipped on by the record/replay demos. +static std::atomic g_deterministic_clock{false}; + +// First packet's sensor ts (deterministic mode only). Used to seed the +// main loop's rate-limit bookmarks at exactly the first delivered packet, +// independent of when the main loop's first iteration happens to run. +static std::atomic g_first_packet_clock_ns{0}; + +// First-packet marker. Used by record/replay tooling to align the SDK's +// warmup-induced packet drop with replay. The C++ binary writes the wall +// clock of the first on_point_cloud / on_imu_data callback (live mode +// only) to this file; demo_replay reads it back and passes the value as +// --replay_skip_until_ns so pcap_replay drops the same SDK-eaten prefix. +static std::string g_first_packet_marker_path; +static std::atomic g_first_packet_marker_written{false}; + +// The packet's sensor-clock timestamp (pkt->timestamp) is identical bit-for-bit +// between the live SDK delivery path and the recorded pcap, so writing it from +// the first SDK callback gives replay an exact boundary to skip on. Wall clock +// would only let us match within delivery latency (sub-ms). +static void mark_first_packet(uint64_t pkt_timestamp_ns) { + if (g_first_packet_marker_path.empty()) { + return; + } + bool expected = false; + if (!g_first_packet_marker_written.compare_exchange_strong(expected, true)) { + return; + } + FILE* f = std::fopen(g_first_packet_marker_path.c_str(), "w"); + if (f) { + std::fprintf(f, "%lu\n", static_cast(pkt_timestamp_ns)); + std::fclose(f); + } +} + +static double get_publish_ts() { + if (g_deterministic_clock.load() || g_replay_mode.load()) { + return static_cast(g_virtual_clock_ns.load()) / 1e9; + } + return std::chrono::duration( + std::chrono::system_clock::now().time_since_epoch()).count(); +} + +// Virtualized clock for the main loop's frame/publish rate limiters. In +// replay mode this returns a time_point derived from g_virtual_clock_ns so +// scan boundaries are determined by packet arrival, not by wall-clock thread +// scheduling jitter. Returns nullopt if replay hasn't yet seen a packet +// (caller should skip rate-limit checks in that case). +static std::optional virtual_now() { + if (g_deterministic_clock.load() || g_replay_mode.load()) { + uint64_t ns = g_virtual_clock_ns.load(); + if (ns == 0) { + return std::nullopt; + } + return std::chrono::steady_clock::time_point(std::chrono::nanoseconds(ns)); + } + return std::chrono::steady_clock::now(); +} + static std::string g_lidar_topic; static std::string g_odometry_topic; static std::string g_map_topic; @@ -257,6 +333,7 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times g_lcm->publish(g_odometry_topic, &msg); } + // --------------------------------------------------------------------------- // Livox SDK callbacks // --------------------------------------------------------------------------- @@ -268,8 +345,25 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ uint64_t ts_ns = get_timestamp_ns(data); uint16_t dot_num = data->dot_num; + if (!g_replay_mode.load()) { + mark_first_packet(ts_ns); + } + std::lock_guard lock(g_pc_mutex); + // Update the deterministic-mode virtual clock INSIDE the accumulator + // mutex so the main loop can never observe a clock advance without + // also seeing the matching points (race that drifts scan composition). + // Monotonic update: SDK threads can deliver packets slightly out of + // sensor-ts order, and we must not let a later store(lower_ts) undo + // a previous store(higher_ts). + if (g_deterministic_clock.load()) { + uint64_t expected = 0; + g_first_packet_clock_ns.compare_exchange_strong(expected, ts_ns); + uint64_t cur = g_virtual_clock_ns.load(); + while (cur < ts_ns && !g_virtual_clock_ns.compare_exchange_weak(cur, ts_ns)) {} + } + if (!g_frame_has_timestamp) { g_frame_start_ns = ts_ns; g_frame_has_timestamp = true; @@ -308,7 +402,12 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, LivoxLidarEthernetPacket* data, void* /*client_data*/) { if (!g_running.load() || data == nullptr || !g_fastlio) return; - double ts = static_cast(get_timestamp_ns(data)) / 1e9; + uint64_t pkt_ts_ns = get_timestamp_ns(data); + if (!g_replay_mode.load()) { + mark_first_packet(pkt_ts_ns); + } + + double ts = static_cast(pkt_ts_ns) / 1e9; auto* imu_pts = reinterpret_cast(data->data); uint16_t dot_num = data->dot_num; @@ -339,6 +438,18 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, g_fastlio->feed_imu(imu_msg); } + + // Advance the deterministic-mode virtual clock AFTER feed_imu has + // queued the sample, holding g_pc_mutex so this is fully serialized + // with on_point_cloud / the main-loop scan swap. Monotonic CAS so + // out-of-order SDK delivery can't roll the clock backward. + if (g_deterministic_clock.load()) { + std::lock_guard lock(g_pc_mutex); + uint64_t expected = 0; + g_first_packet_clock_ns.compare_exchange_strong(expected, pkt_ts_ns); + uint64_t cur = g_virtual_clock_ns.load(); + while (cur < pkt_ts_ns && !g_virtual_clock_ns.compare_exchange_weak(cur, pkt_ts_ns)) {} + } } static void on_info_change(const uint32_t handle, const LivoxLidarInfo* info, @@ -395,6 +506,14 @@ int main(int argc, char** argv) { double msr_freq = mod.arg_float("msr_freq", 50.0f); double main_freq = mod.arg_float("main_freq", 5000.0f); + // Post-IESKF-update velocity cap. When |v_world| exceeds this value + // the EKF state is restored to the last accepted scan with vel=0 and + // map_incremental is skipped. Breaks the reinforcing-loop divergence + // that gives FAST-LIO multi-km/s velocity runaway on aggressive + // motion or large IMU gaps. Zero disables. Defaults sized to the + // Go2 quadruped envelope (~3.1 m/s); raise for faster platforms. + double max_velocity_norm_ms = mod.arg_float("max_velocity_norm_ms", 0.0f); + // Livox hardware config std::string host_ip = mod.arg("host_ip", "192.168.1.5"); std::string lidar_ip = mod.arg("lidar_ip", "192.168.1.155"); @@ -411,11 +530,6 @@ int main(int argc, char** argv) { float map_max_range = mod.arg_float("map_max_range", 100.0f); float map_freq = mod.arg_float("map_freq", 0.0f); - // Optional override of the FAST-LIO post-IESKF-update velocity cap (m/s). - // Zero (default) means use the upstream default; any positive value - // overrides. Size to the platform's physical envelope. - double max_velocity_norm_ms = mod.arg_float("max_velocity_norm_ms", 0.0f); - // Verbose logging — propagates to the FAST-LIO C++ core via the // `fastlio_debug` global. Default false → only real errors print. bool debug = mod.arg_bool("debug", false); @@ -435,6 +549,34 @@ int main(int argc, char** argv) { ports.host_imu_data = mod.arg_int("host_imu_data_port", port_defaults.host_imu_data); ports.host_log_data = mod.arg_int("host_log_data_port", port_defaults.host_log_data); + // Replay mode (offline). When --replay_pcap is given the SDK is not + // initialized; a feeder thread reads the pcap and calls the existing + // on_point_cloud / on_imu_data callbacks directly. publish_*() uses + // the pcap timestamps as the clock so outputs match the original run. + std::string replay_pcap = mod.arg("replay_pcap", ""); + g_replay_mode.store(!replay_pcap.empty()); + + // Drop pcap packets with pcap_ts < this value. Used in replay to mimic + // the SDK warmup discard that the live run experienced — so the + // algorithm starts from the same first packet in both modes. + uint64_t replay_skip_until_ns = 0; + { + std::string s = mod.arg("replay_skip_until_ns", "0"); + if (!s.empty()) { + replay_skip_until_ns = std::stoull(s); + } + } + + // Live mode: write the wall_clock_ns of the first SDK callback to this + // file. Pair with replay's --replay_skip_until_ns to align packet sets. + g_first_packet_marker_path = mod.arg("first_packet_marker", ""); + + // Drive virtual_now() and get_publish_ts() off the packet's sensor + // clock in both live and replay. Eliminates wall-clock jitter from + // scan boundaries and makes outputs bit-comparable across modes. + // Changes header.stamp from unix wall time to sensor-boot seconds. + g_deterministic_clock.store(mod.arg_bool("deterministic_clock", false)); + // Initial pose offset [x, y, z, qx, qy, qz, qw] { std::string init_str = mod.arg("init_pose", ""); @@ -498,112 +640,158 @@ int main(int argc, char** argv) { g_fastlio = &fast_lio; if (debug) printf("[fastlio2] FAST-LIO initialized.\n"); - // Init Livox SDK (in-memory config, no temp files). - // Pass `debug` so the SDK's spdlog console sink is disabled when off. - if (!livox_common::init_livox_sdk(host_ip, lidar_ip, ports, debug)) { - return 1; - } - - // Register SDK callbacks - SetLivoxLidarPointCloudCallBack(on_point_cloud, nullptr); - SetLivoxLidarImuDataCallback(on_imu_data, nullptr); - SetLivoxLidarInfoChangeCallback(on_info_change, nullptr); - - // Start SDK - if (!LivoxLidarSdkStart()) { - fprintf(stderr, "Error: LivoxLidarSdkStart failed\n"); - LivoxLidarSdkUninit(); - return 1; - } - - if (debug) printf("[fastlio2] SDK started, waiting for device...\n"); - - // Main loop + // Main-loop state. The body lives in `run_main_iter` below so it can be + // invoked from either the wall-clock-paced main thread (live) or the + // pcap-paced feeder thread (replay), with no race on accumulator + // contents in the replay case. auto frame_interval = std::chrono::microseconds( static_cast(1e6 / g_frequency)); - auto last_emit = std::chrono::steady_clock::now(); + std::optional last_emit; const double process_period_ms = 1000.0 / main_freq; - // Rate limiters for output publishing auto pc_interval = std::chrono::microseconds( static_cast(1e6 / pointcloud_freq)); auto odom_interval = std::chrono::microseconds( static_cast(1e6 / odom_freq)); - auto last_pc_publish = std::chrono::steady_clock::now(); - auto last_odom_publish = std::chrono::steady_clock::now(); + std::optional last_pc_publish; + std::optional last_odom_publish; - // Global voxel map (only if map topic is configured AND map_freq > 0) std::unique_ptr global_map; std::chrono::microseconds map_interval{0}; - auto last_map_publish = std::chrono::steady_clock::now(); + std::optional last_map_publish; if (!g_map_topic.empty() && map_freq > 0.0f) { global_map = std::make_unique(map_voxel_size, map_max_range); map_interval = std::chrono::microseconds( static_cast(1e6 / map_freq)); } - while (g_running.load()) { - auto loop_start = std::chrono::high_resolution_clock::now(); - - // At frame rate: build CustomMsg from accumulated points and feed to FAST-LIO - auto now = std::chrono::steady_clock::now(); - if (now - last_emit >= frame_interval) { - std::vector points; - uint64_t frame_start = 0; + // Per-section timing counters for `run_main_iter`. Active only when + // --debug is on (Scope's constructor reads `fastlio_debug` and no-ops + // otherwise). `timing::maybe_flush(now)` at the bottom prints a per- + // section summary every second of wall clock so we can see both how + // often each part fires and how long each call takes. + static timing::Section t_iter{"run_main_iter"}; + static timing::Section t_emit_check{"emit.lock+swap"}; + static timing::Section t_feed_lidar{"fast_lio.feed_lidar"}; + static timing::Section t_process{"fast_lio.process"}; + static timing::Section t_get_world_cloud{"fast_lio.get_world_cloud"}; + static timing::Section t_filter_cloud{"filter_cloud"}; + static timing::Section t_publish_lidar{"publish_lidar"}; + static timing::Section t_map_insert{"global_map.insert"}; + static timing::Section t_map_publish{"global_map.publish"}; + static timing::Section t_publish_odom{"publish_odometry"}; + + auto run_main_iter = [&](std::chrono::steady_clock::time_point now) { + timing::Scope iter_scope(t_iter); + // Lazy-seed all rate-limit bookmarks on the first iteration so they + // line up with the chosen clock (wall in live, pcap in replay) and + // don't fire immediately based on an arbitrary "since program start" + // delta. In deterministic mode we seed from the FIRST packet's + // sensor ts (not the current ts) so both live and replay anchor + // their first scan boundary at the same packet — required for + // bit-for-bit live↔replay parity. + auto seed = now; + if (g_deterministic_clock.load()) { + uint64_t first = g_first_packet_clock_ns.load(); + if (first != 0) { + seed = std::chrono::steady_clock::time_point( + std::chrono::nanoseconds(first)); + } + } + if (!last_emit.has_value()) { + last_emit = seed; + } + if (!last_pc_publish.has_value()) { + last_pc_publish = seed; + } + if (!last_odom_publish.has_value()) { + last_odom_publish = seed; + } + if (global_map && !last_map_publish.has_value()) { + last_map_publish = seed; + } - { - std::lock_guard lock(g_pc_mutex); + // At frame rate: drain accumulated raw points into a CustomMsg + // and feed to FAST-LIO. Hold g_pc_mutex across the rate-limit + // CHECK as well as the swap, so in deterministic mode the + // virtual clock + accumulator are observed atomically with no + // other thread able to slip a packet in between the decision + // and the swap. + std::vector points; + uint64_t frame_start = 0; + { + timing::Scope s(t_emit_check); + std::lock_guard lock(g_pc_mutex); + auto check_now = now; + if (g_deterministic_clock.load()) { + uint64_t ns = g_virtual_clock_ns.load(); + if (ns != 0) { + check_now = std::chrono::steady_clock::time_point( + std::chrono::nanoseconds(ns)); + } + } + if (check_now - *last_emit >= frame_interval) { if (!g_accumulated_points.empty()) { points.swap(g_accumulated_points); frame_start = g_frame_start_ns; g_frame_has_timestamp = false; } + last_emit = check_now; } - - if (!points.empty()) { - // Build CustomMsg - auto lidar_msg = boost::make_shared(); - lidar_msg->header.seq = 0; - lidar_msg->header.stamp = custom_messages::Time().fromSec( - static_cast(frame_start) / 1e9); - lidar_msg->header.frame_id = "livox_frame"; - lidar_msg->timebase = frame_start; - lidar_msg->lidar_id = 0; - for (int i = 0; i < 3; i++) - lidar_msg->rsvd[i] = 0; - lidar_msg->point_num = static_cast(points.size()); - lidar_msg->points = std::move(points); - - fast_lio.feed_lidar(lidar_msg); - } - - last_emit = now; + } + if (!points.empty()) { + // Build CustomMsg + auto lidar_msg = boost::make_shared(); + lidar_msg->header.seq = 0; + lidar_msg->header.stamp = custom_messages::Time().fromSec( + static_cast(frame_start) / 1e9); + lidar_msg->header.frame_id = "livox_frame"; + lidar_msg->timebase = frame_start; + lidar_msg->lidar_id = 0; + for (int i = 0; i < 3; i++) lidar_msg->rsvd[i] = 0; + lidar_msg->point_num = static_cast(points.size()); + lidar_msg->points = std::move(points); + timing::Scope s(t_feed_lidar); + fast_lio.feed_lidar(lidar_msg); } - // Run FAST-LIO processing step (high frequency) - fast_lio.process(); + // Run one FAST-LIO IESKF step. Cheap when the IMU/lidar queues + // are empty; the heavy work happens after a feed_lidar above. + { + timing::Scope s(t_process); + fast_lio.process(); + } - // Check for new results and accumulate/publish (rate-limited) + // Check for new SLAM results and publish (rate-limited). auto pose = fast_lio.get_pose(); if (!pose.empty() && (pose[0] != 0.0 || pose[1] != 0.0 || pose[2] != 0.0)) { - double ts = std::chrono::duration( - std::chrono::system_clock::now().time_since_epoch()).count(); - - auto world_cloud = fast_lio.get_world_cloud(); + double ts = get_publish_ts(); + auto world_cloud = ([&]() { + timing::Scope s(t_get_world_cloud); + return fast_lio.get_world_cloud(); + })(); if (world_cloud && !world_cloud->empty()) { - auto filtered = filter_cloud(world_cloud, filter_cfg); - - // Per-scan publish at pointcloud_freq - if (!g_lidar_topic.empty() && now - last_pc_publish >= pc_interval) { + auto filtered = ([&]() { + timing::Scope s(t_filter_cloud); + return filter_cloud(world_cloud, filter_cfg); + })(); + + // Per-scan world-frame cloud at pointcloud_freq. + if (!g_lidar_topic.empty() && now - *last_pc_publish >= pc_interval) { + timing::Scope s(t_publish_lidar); publish_lidar(filtered, ts); last_pc_publish = now; } - // Global map: insert, prune, and publish at map_freq + // Global voxel map: insert this scan, prune, then publish + // a snapshot at map_freq. if (global_map) { - global_map->insert(filtered); - - if (now - last_map_publish >= map_interval) { + { + timing::Scope s(t_map_insert); + global_map->insert(filtered); + } + if (now - *last_map_publish >= map_interval) { + timing::Scope s(t_map_publish); global_map->prune( static_cast(pose[0]), static_cast(pose[1]), @@ -615,17 +803,97 @@ int main(int argc, char** argv) { } } - // Publish odometry (rate-limited to odom_freq) - if (!g_odometry_topic.empty() && (now - last_odom_publish >= odom_interval)) { + // Pose + covariance, rate-limited to odom_freq. + if (!g_odometry_topic.empty() && now - *last_odom_publish >= odom_interval) { + timing::Scope s(t_publish_odom); publish_odometry(fast_lio.get_odometry(), ts); last_odom_publish = now; } } - // Handle LCM messages + // Periodic per-section summary to stderr (no-op when --debug off). + timing::maybe_flush(std::chrono::steady_clock::now()); + }; + + // Source of point/IMU packets: + // live mode -> Livox SDK opens UDP sockets + dispatches via callbacks + // from its own threads. Main thread drives run_main_iter + // at main_freq, consuming whatever the SDK queued. + // replay mode -> the feeder thread reads the pcap and pushes packets + // through the same on_point/on_imu callbacks (paced at + // realtime via sleep_until). The MAIN thread — same + // one that runs in live mode — owns run_main_iter and + // drains the accumulator. Two threads in both modes, + // matching architectures, so the only difference is + // the source of packets (SDK vs pcap). + std::thread replay_thread; + if (g_replay_mode.load()) { + if (debug) printf("[fastlio2] REPLAY mode, pcap=%s\n", replay_pcap.c_str()); + replay_thread = std::thread([&]() { + pcap_replay::Replayer rep; + rep.path = replay_pcap; + rep.host_point_port = static_cast(ports.host_point_data); + rep.host_imu_port = static_cast(ports.host_imu_data); + rep.on_point = [](LivoxLidarEthernetPacket* p) { + on_point_cloud(0, 0, p, nullptr); + }; + rep.on_imu = [](LivoxLidarEthernetPacket* p) { + on_imu_data(0, 0, p, nullptr); + }; + rep.on_clock = [](uint64_t pcap_ts_ns) { + // In deterministic mode the callbacks already pushed the + // sensor pkt->timestamp into g_virtual_clock_ns — don't + // overwrite with the pcap (wall) ts. + if (g_deterministic_clock.load()) { + return; + } + g_virtual_clock_ns.store(pcap_ts_ns); + }; + // No rep.on_iter — the main thread drives run_main_iter in + // replay mode now, same as in live. This decouples packet + // ingestion from per-iter filter_cloud cost and lets replay + // run at the same wall throughput as live. + rep.running = &g_running; + // Pace the replay feeder at live wall-clock rate. sleep_until + // throttles the feeder so packets land in the accumulator at + // the same wall cadence as the SDK delivers in live mode. + rep.realtime = true; + rep.skip_until_ns = replay_skip_until_ns; + rep.run(); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + g_running.store(false); + }); + } else { + if (!livox_common::init_livox_sdk(host_ip, lidar_ip, ports, debug)) { + return 1; + } + SetLivoxLidarPointCloudCallBack(on_point_cloud, nullptr); + SetLivoxLidarImuDataCallback(on_imu_data, nullptr); + SetLivoxLidarInfoChangeCallback(on_info_change, nullptr); + if (!LivoxLidarSdkStart()) { + fprintf(stderr, "Error: LivoxLidarSdkStart failed\n"); + LivoxLidarSdkUninit(); + return 1; + } + if (debug) printf("[fastlio2] SDK started, waiting for device...\n"); + } + + while (g_running.load()) { + auto loop_start = std::chrono::high_resolution_clock::now(); + auto now_opt = virtual_now(); + if (!now_opt.has_value()) { + // No clock yet — in replay this means the feeder hasn't read + // its first packet; in live it shouldn't happen because + // virtual_now falls back to steady_clock::now(). + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + continue; + } + run_main_iter(*now_opt); + + // Drain LCM messages. lcm.handleTimeout(0); - // Rate control (~5kHz processing) + // Rate control (~main_freq, 5kHz default). auto loop_end = std::chrono::high_resolution_clock::now(); auto elapsed_ms = std::chrono::duration(loop_end - loop_start).count(); if (elapsed_ms < process_period_ms) { @@ -637,7 +905,12 @@ int main(int argc, char** argv) { // Cleanup if (debug) printf("[fastlio2] Shutting down...\n"); g_fastlio = nullptr; - LivoxLidarSdkUninit(); + if (replay_thread.joinable()) { + replay_thread.join(); + } + if (!g_replay_mode.load()) { + LivoxLidarSdkUninit(); + } g_lcm = nullptr; if (debug) printf("[fastlio2] Done.\n"); diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/pcap_replay.hpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/pcap_replay.hpp new file mode 100644 index 0000000000..cfbc0e58c1 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/pcap_replay.hpp @@ -0,0 +1,222 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Read a pcap of recorded Mid-360 UDP traffic and feed each point/imu +// payload to the existing SDK callbacks. Used by `--replay_pcap` to bypass +// the Livox SDK for deterministic offline regression testing. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "livox_lidar_def.h" + +namespace pcap_replay { + +constexpr uint32_t PCAP_MAGIC_LE_US = 0xa1b2c3d4u; +constexpr uint32_t PCAP_MAGIC_LE_NS = 0xa1b23c4du; +constexpr uint32_t LINKTYPE_ETHERNET = 1u; +constexpr uint16_t ETHERTYPE_IPV4 = 0x0800u; +constexpr uint8_t IPPROTO_UDP = 17u; +constexpr size_t ETH_HDR_LEN = 14; +constexpr size_t IP_MIN_HDR_LEN = 20; +constexpr size_t UDP_HDR_LEN = 8; +constexpr size_t LIVOX_ETH_HDR_LEN = 36; + +using PacketCb = std::function; +using ClockCb = std::function; +using IterCb = std::function; + +struct Replayer { + std::string path; + uint16_t host_point_port = 0; + uint16_t host_imu_port = 0; + PacketCb on_point; + PacketCb on_imu; + ClockCb on_clock; + // Called synchronously after every packet, once the payload has been + // appended and the virtual clock advanced. The replay path runs the + // main-loop body here so feeding + processing happen on a single + // thread — eliminates the feeder-vs-main-loop race on accumulator + // contents. + IterCb on_iter; + std::atomic* running = nullptr; + bool realtime = true; + // Drop Livox packets whose sensor timestamp (pkt->timestamp) is + // strictly less than this. Used to mimic the SDK warmup window from a + // paired live run so the algorithm starts from the same first packet + // in both modes. Comparing on sensor ts (which is identical bit-for-bit + // between live SDK delivery and pcap replay) is exact; comparing on + // wall pcap_ts would be off by SDK delivery latency. + uint64_t skip_until_ns = 0; + + bool run() { + std::ifstream f(path, std::ios::binary); + if (!f) { + fprintf(stderr, "[replay] cannot open %s\n", path.c_str()); + return false; + } + + uint8_t global_hdr[24]; + f.read(reinterpret_cast(global_hdr), 24); + if (!f) { + fprintf(stderr, "[replay] short read on pcap global header\n"); + return false; + } + uint32_t magic; + std::memcpy(&magic, global_hdr, 4); + const bool nanos = (magic == PCAP_MAGIC_LE_NS); + if (magic != PCAP_MAGIC_LE_US && magic != PCAP_MAGIC_LE_NS) { + fprintf(stderr, "[replay] unsupported pcap magic 0x%08x\n", magic); + return false; + } + uint32_t linktype; + std::memcpy(&linktype, global_hdr + 20, 4); + if (linktype != LINKTYPE_ETHERNET) { + fprintf(stderr, "[replay] unsupported linktype %u (need ETHERNET=1)\n", linktype); + return false; + } + + printf("[replay] reading %s (port=%u imu=%u realtime=%d)\n", + path.c_str(), host_point_port, host_imu_port, realtime ? 1 : 0); + + uint64_t first_pcap_ts_ns = 0; + std::chrono::steady_clock::time_point start_wall; + bool seeded = false; + + size_t pkts = 0, pts = 0, imu = 0, other = 0; + uint8_t rec_hdr[16]; + std::vector buf; + + while (running == nullptr || running->load()) { + f.read(reinterpret_cast(rec_hdr), 16); + if (!f) { + break; + } + + uint32_t ts_sec, ts_sub, incl_len, orig_len; + std::memcpy(&ts_sec, rec_hdr + 0, 4); + std::memcpy(&ts_sub, rec_hdr + 4, 4); + std::memcpy(&incl_len, rec_hdr + 8, 4); + std::memcpy(&orig_len, rec_hdr + 12, 4); + (void)orig_len; + + const uint64_t pcap_ts_ns = + static_cast(ts_sec) * 1'000'000'000ULL + + (nanos ? static_cast(ts_sub) : static_cast(ts_sub) * 1000ULL); + + buf.resize(incl_len); + f.read(reinterpret_cast(buf.data()), incl_len); + if (!f) { + break; + } + pkts++; + + if (buf.size() < ETH_HDR_LEN) { + continue; + } + uint16_t ethertype = (static_cast(buf[12]) << 8) | buf[13]; + if (ethertype != ETHERTYPE_IPV4) { + continue; + } + size_t ip_off = ETH_HDR_LEN; + if (buf.size() < ip_off + IP_MIN_HDR_LEN) { + continue; + } + uint8_t vihl = buf[ip_off]; + if ((vihl >> 4) != 4) { + continue; + } + int ihl = (vihl & 0x0f) * 4; + if (ihl < static_cast(IP_MIN_HDR_LEN)) { + continue; + } + if (buf[ip_off + 9] != IPPROTO_UDP) { + continue; + } + size_t udp_off = ip_off + ihl; + if (buf.size() < udp_off + UDP_HDR_LEN) { + continue; + } + uint16_t dst_port = (static_cast(buf[udp_off + 2]) << 8) | buf[udp_off + 3]; + uint16_t udp_len = (static_cast(buf[udp_off + 4]) << 8) | buf[udp_off + 5]; + size_t payload_off = udp_off + UDP_HDR_LEN; + size_t payload_end = std::min(buf.size(), static_cast(udp_off + udp_len)); + if (payload_end <= payload_off) { + continue; + } + size_t payload_len = payload_end - payload_off; + if (payload_len < LIVOX_ETH_HDR_LEN) { + continue; + } + + auto* livox_pkt = + reinterpret_cast(buf.data() + payload_off); + + // Sensor-clock skip: drop packets the live SDK wouldn't have + // seen (those before its first delivered callback) so the + // algorithm processes the same input set in both modes. + if (skip_until_ns > 0) { + uint64_t pkt_ts; + std::memcpy(&pkt_ts, livox_pkt->timestamp, sizeof(uint64_t)); + if (pkt_ts < skip_until_ns) { + continue; + } + } + + if (realtime) { + if (!seeded) { + first_pcap_ts_ns = pcap_ts_ns; + start_wall = std::chrono::steady_clock::now(); + seeded = true; + } else { + auto target = start_wall + std::chrono::nanoseconds(pcap_ts_ns - first_pcap_ts_ns); + auto now = std::chrono::steady_clock::now(); + if (target > now) { + std::this_thread::sleep_until(target); + } + } + } + + if (dst_port == host_point_port) { + if (on_point) { + on_point(livox_pkt); + } + pts++; + } else if (dst_port == host_imu_port) { + if (on_imu) { + on_imu(livox_pkt); + } + imu++; + } else { + other++; + } + // Advance the virtual clock AFTER the payload has been added to + // accumulators. Reverse order would let the main-loop thread see + // the clock advance and emit a scan that's missing this packet. + if (on_clock) { + on_clock(pcap_ts_ns); + } + + // Run one main-loop iteration synchronously so feeding and + // processing are strictly serialized in replay mode. + if (on_iter) { + on_iter(); + } + } + + printf("[replay] done: %zu pcap records (point=%zu imu=%zu other=%zu)\n", + pkts, pts, imu, other); + return true; + } +}; + +} // namespace pcap_replay diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/timing.hpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/timing.hpp new file mode 100644 index 0000000000..d1fbe8ded4 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/timing.hpp @@ -0,0 +1,128 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Lightweight per-section timing for diagnosing where wall time goes in +// `run_main_iter`. Active only when --debug is on (i.e. the global +// `fastlio_debug` flag is true) so non-debug runs pay only a single +// branch per scope. +// +// Usage: +// +// static timing::Section sec{"filter_cloud"}; +// { +// timing::Scope s(sec); +// // ...do work... +// } +// // and periodically: +// timing::maybe_flush(now); +// +// The flush prints one line per section to stderr every flush interval +// (1 second of wall clock) summarising count / mean / max / total, then +// resets the accumulators. The flush is cheap when nothing was recorded. + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "fast_lio_debug.hpp" // for the global `fastlio_debug` flag + +namespace timing { + +struct Section { + const char* name; + std::atomic count{0}; + std::atomic total_ns{0}; + std::atomic max_ns{0}; + + explicit Section(const char* n); + + void add(uint64_t ns) { + count.fetch_add(1, std::memory_order_relaxed); + total_ns.fetch_add(ns, std::memory_order_relaxed); + uint64_t prev = max_ns.load(std::memory_order_relaxed); + while (ns > prev && + !max_ns.compare_exchange_weak(prev, ns, std::memory_order_relaxed)) { + // prev is updated on failure by compare_exchange_weak. + } + } +}; + +inline std::vector& registry() { + static std::vector r; + return r; +} + +inline Section::Section(const char* n) : name(n) { + registry().push_back(this); +} + +struct Scope { + Section& sec; + std::chrono::steady_clock::time_point t0; + bool active; + + explicit Scope(Section& s) : sec(s), active(fastlio_debug) { + if (active) { + t0 = std::chrono::steady_clock::now(); + } + } + + ~Scope() { + if (!active) { + return; + } + auto dt = std::chrono::duration_cast( + std::chrono::steady_clock::now() - t0).count(); + sec.add(static_cast(dt)); + } +}; + +// Print one summary line per section to stderr every FLUSH_INTERVAL wall +// seconds, then reset accumulators. The check is cheap: a single time +// comparison guarded by the fastlio_debug flag. The mutex serialises the +// flush between threads (replay's feeder vs live's main loop) so we +// never see torn output. +inline void maybe_flush(std::chrono::steady_clock::time_point now) { + if (!fastlio_debug) { + return; + } + constexpr auto FLUSH_INTERVAL = std::chrono::seconds(1); + static std::mutex mtx; + static std::chrono::steady_clock::time_point last; + std::lock_guard lock(mtx); + if (last.time_since_epoch().count() == 0) { + last = now; + return; + } + if (now - last < FLUSH_INTERVAL) { + return; + } + auto dt_ms = std::chrono::duration(now - last).count(); + last = now; + + for (Section* s : registry()) { + uint64_t c = s->count.exchange(0, std::memory_order_relaxed); + uint64_t tot = s->total_ns.exchange(0, std::memory_order_relaxed); + uint64_t mx = s->max_ns.exchange(0, std::memory_order_relaxed); + if (c == 0) { + std::fprintf(stderr, "[timing] %-24s n=0\n", s->name); + continue; + } + double mean_us = static_cast(tot) / static_cast(c) / 1e3; + double max_us = static_cast(mx) / 1e3; + double total_ms = static_cast(tot) / 1e6; + double rate_hz = static_cast(c) * 1000.0 / dt_ms; + std::fprintf(stderr, + "[timing] %-24s n=%5lu rate=%7.1fHz mean=%8.3fus max=%9.3fus tot=%7.2fms\n", + s->name, + static_cast(c), + rate_hz, mean_us, max_us, total_ms); + } +} + +} // namespace timing diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 259a726987..dc6803ceed 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -137,6 +137,23 @@ class FastLio2Config(NativeModuleConfig): # Resolved in __post_init__, passed as --config_path to the binary config_path: str | None = None + # Offline replay. When set, the C++ binary skips SDK init and feeds + # packets from this pcap into the same callbacks the SDK would, with + # publish timestamps driven by the pcap clock. + replay_pcap: Path | None = None + # Replay-only: drop pcap records with pcap_ts < this. Used to mimic the + # SDK warmup window from the paired live run. + replay_skip_until_ns: int | None = None + # Live-only: file path where the C++ binary writes the wall_ns of the + # first SDK callback, so a later replay can align its first packet. + first_packet_marker: Path | None = None + # Drive scan boundaries + publish ts off the sensor packet timestamp in + # both live and replay so they produce bit-for-bit identical outputs. + # Side effect: published header.stamp is sensor-boot seconds, not unix + # wall time. Off by default; only the deterministic record/replay path + # flips it on (real-time replay leaves it False). + deterministic_clock: bool = False + # init_pose is computed from mount; config is resolved to config_path init_pose: list[float] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] cli_exclude: frozenset[str] = frozenset({"config", "mount"}) @@ -169,7 +186,8 @@ class FastLio2(NativeModule, perception.Lidar, perception.Odometry, mapping.Glob @rpc def start(self) -> None: - self._validate_network() + if self.config.replay_pcap is None: + self._validate_network() super().start() self.register_disposable( Disposable(self.odometry.transport.subscribe(self._on_odom_for_tf, self.odometry)) diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py new file mode 100644 index 0000000000..50685f8fec --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -0,0 +1,361 @@ +# 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. + +"""Run FAST-LIO over a .pcap and append its outputs into an existing .db. + +Given a Livox Mid-360 pcap capture and a memory2 SQLite database, this streams +the pcap through the FastLio2 native module and writes two streams into the +database, both time-aligned onto the db's existing clock: + +* ``fastlio_odometry`` -- the IESKF pose at the native odom rate (~30 Hz). +* ``fastlio_lidar`` -- the registered (deskewed, odom-frame) point cloud at the + native pointcloud rate (~10 Hz). + +This mirrors the Point-LIO ``pcap_to_db.py`` tool, with one deliberate +difference: FAST-LIO is *not* bit-deterministic (OpenMP reduction order), so the +replay runs ``deterministic_clock=False`` -- the feeder paces packets at +wall-clock realtime, exactly as the live SDK delivers them, and publish +timestamps come from the pcap's capture clock. A 20-minute recording therefore +takes ~20 minutes of wall time to replay. + +If either stream already exists in the db the run aborts, unless ``--force`` is +given, in which case the existing ``fastlio_odometry`` and ``fastlio_lidar`` +streams are dropped before the new ones are written. + +Timing conversion +----------------- +With ``deterministic_clock=False`` FAST-LIO publishes with the pcap packet +clock, which for a real recording is the original capture's *unix wall time* -- +the same clock the db's other streams already use. So the common case needs no +shift. The offset is auto-derived from the two clocks: + +* db + replay on the same clock family (both wall, or both sensor): offset 0. +* cross-clock (e.g. a deterministic sensor-clock replay into a wall-clock db): + start-align by shifting the replay's first ts onto the db's earliest ts. +* db has no existing timestamped rows: offset 0. + +Pass ``--time-offset`` to override the auto choice. + +Usage (from the dimos5 venv):: + + source .venv/bin/activate + python -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db \ + --pcap /path/to/capture.pcap --db /path/to/memory.db +""" + +from __future__ import annotations + +import argparse +from collections.abc import AsyncIterator +import math +from pathlib import Path +import sqlite3 +import sys +import time + +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +# Below this an absolute timestamp is sensor-boot seconds, not unix wall time. +_SENSOR_CLOCK_MAX = 1e8 +# Strictly-increasing tie-breaker so two samples never collide on ts. +_EPS = 1e-9 +# Poll the db on this cadence while the replay drains the pcap. +_POLL_SEC = 1.0 +# Stop after the odom stream has been stagnant this long (pcap fully drained). +_STAGNANT_SEC = 6.0 +# Go2 quadruped post-update velocity cap (m/s). Breaks the FAST-LIO velocity +# runaway on aggressive motion; the dog cannot physically exceed this, so it +# only ever clamps divergence. Zero disables. See FastLio2Config. +_GO2_MAX_VELOCITY_MS = 3.1 + + +class RecConfig(ModuleConfig): + """Configures the recorder with the target db and timing conversion.""" + + db_path: str = "" + # Earliest existing ts in the db, or -1.0 if the db has no timestamped rows. + ref_start_ts: float = -1.0 + # Explicit offset override; NaN means auto-derive from ref_start_ts. + time_offset: float = float("nan") + + +class Rec(Module): + """Append FAST-LIO odometry + lidar into an existing SQLite db with ts conversion.""" + + config: RecConfig + fastlio_odometry: In[Odometry] + fastlio_lidar: In[PointCloud2] + _offset: float | None = None + _last_odom_ts: float = 0.0 + _last_lidar_ts: float = 0.0 + _last_pose: object = None + _odom_count: int = 0 + _lidar_count: int = 0 + + async def main(self) -> AsyncIterator[None]: + from dimos.memory2.store.sqlite import SqliteStore + + self._store = SqliteStore(path=self.config.db_path) + self._os = self._store.stream("fastlio_odometry", Odometry) + self._ls = self._store.stream("fastlio_lidar", PointCloud2) + yield + self._store.stop() + + def _resolve_offset(self, first_ts: float) -> float: + override = self.config.time_offset + if not math.isnan(override): + return override + ref = self.config.ref_start_ts + if ref < 0.0: + return 0.0 + # Same clock family (both wall, or both sensor) -> already aligned. + # Cross-clock -> start-align the replay's first ts onto the db's first. + if (first_ts > _SENSOR_CLOCK_MAX) == (ref > _SENSOR_CLOCK_MAX): + return 0.0 + return ref - first_ts + + def _aligned_ts(self, raw_ts: float, last_ts: float) -> float: + """Convert a replay ts onto the db clock, kept strictly above last_ts.""" + if self._offset is None: + self._offset = self._resolve_offset(raw_ts) + return max(raw_ts + self._offset, last_ts + _EPS) + + async def handle_fastlio_odometry(self, v: Odometry) -> None: + raw_ts = getattr(v, "ts", None) or time.time() + ts = self._aligned_ts(raw_ts, self._last_odom_ts) + self._last_odom_ts = ts + pose = getattr(v, "pose", None) + self._last_pose = getattr(pose, "pose", None) if pose is not None else None + self._os.append(v, ts=ts, pose=self._last_pose) + self._odom_count += 1 + + async def handle_fastlio_lidar(self, v: PointCloud2) -> None: + raw_ts = getattr(v, "ts", None) or time.time() + ts = self._aligned_ts(raw_ts, self._last_lidar_ts) + self._last_lidar_ts = ts + self._ls.append(v, ts=ts, pose=self._last_pose) + self._lidar_count += 1 + + +def _db_ref_start_ts(db_path: Path) -> float: + """Min ts across the db's existing streams, or -1.0 if none/absent.""" + if not db_path.exists(): + return -1.0 + con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) + try: + tables = [ + row[0] + for row in con.execute("SELECT name FROM sqlite_master WHERE type='table'").fetchall() + ] + best: float | None = None + for table in tables: + if table.startswith("_") or table.startswith("sqlite_"): + continue + try: + # vec0/rtree virtual tables (sqlite-vec etc.) raise "no such + # module" here when the extension isn't loaded -- skip them. + cols = [c[1] for c in con.execute(f"PRAGMA table_info('{table}')").fetchall()] + if "ts" not in cols: + continue + row = con.execute(f"SELECT MIN(ts) FROM '{table}'").fetchone() + except sqlite3.OperationalError: + continue + if row and row[0] is not None: + best = row[0] if best is None else min(best, row[0]) + return best if best is not None else -1.0 + finally: + con.close() + + +def _table_stats(db_path: Path, table: str) -> tuple[int, float, float]: + """(count, min_ts, max_ts) for a stream table; zeros if absent.""" + if not db_path.exists(): + return 0, 0.0, 0.0 + con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) + try: + try: + row = con.execute(f"SELECT COUNT(*), MIN(ts), MAX(ts) FROM '{table}'").fetchone() + except sqlite3.OperationalError: + return 0, 0.0, 0.0 + cnt = row[0] or 0 + return cnt, row[1] or 0.0, row[2] or 0.0 + finally: + con.close() + + +def _run(args: argparse.Namespace) -> int: + pcap_path = Path(args.pcap).expanduser().resolve() + db_path = Path(args.db).expanduser().resolve() + if not pcap_path.exists(): + print(f"[pcap_to_db] missing pcap: {pcap_path}", file=sys.stderr) + return 2 + if args.max_sensor_sec < 0: + print("[pcap_to_db] --max-sensor-sec must be >= 0", file=sys.stderr) + return 2 + + from dimos.core.coordination.blueprints import autoconnect + from dimos.core.coordination.module_coordinator import ModuleCoordinator + from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 + from dimos.memory2.store.sqlite import SqliteStore + + fastlio_streams = ("fastlio_odometry", "fastlio_lidar") + store = SqliteStore(path=str(db_path)) + try: + existing = sorted(set(store.list_streams()) & set(fastlio_streams)) + if existing and not args.force: + print( + f"[pcap_to_db] {db_path.name} already has {existing}; pass --force to overwrite", + file=sys.stderr, + ) + return 2 + for name in existing: + store.delete_stream(name) + if existing: + print(f"[pcap_to_db] --force: dropped existing {existing}", flush=True) + finally: + store.stop() + + ref_start_ts = _db_ref_start_ts(db_path) + time_offset = float("nan") if args.time_offset is None else args.time_offset + if not math.isnan(time_offset): + offset_desc = f"explicit {time_offset:+.3f}s" + elif ref_start_ts < 0.0: + offset_desc = "auto: db empty -> 0" + elif ref_start_ts < _SENSOR_CLOCK_MAX: + offset_desc = f"auto: db sensor-clock (R0={ref_start_ts:.2f})" + else: + offset_desc = f"auto: db wall-clock (R0={ref_start_ts:.2f})" + print( + f"[pcap_to_db] pcap={pcap_path.name} db={db_path.name} " + f"odom_freq={args.odom_freq}Hz vmax={args.max_velocity_norm_ms}m/s offset={offset_desc}", + flush=True, + ) + + fastlio_kwargs: dict[str, object] = dict( + frame_id="world", + map_freq=-1, + odom_freq=args.odom_freq, + max_velocity_norm_ms=args.max_velocity_norm_ms, + replay_pcap=pcap_path, + deterministic_clock=False, + debug=False, + ) + # Omit config to fall back to the module default (config/mid360.yaml). + if args.config: + fastlio_kwargs["config"] = Path(args.config) + fastlio = FastLio2.blueprint(**fastlio_kwargs).remappings( + [ + (FastLio2, "odometry", "fastlio_odometry"), + (FastLio2, "lidar", "fastlio_lidar"), + ] + ) + blueprint = autoconnect( + fastlio, + Rec.blueprint( + db_path=str(db_path), + ref_start_ts=ref_start_ts, + time_offset=time_offset, + ), + ).global_config(n_workers=4, robot_model="mid360_fastlio2_pcap_to_db") + coord = ModuleCoordinator.build(blueprint) + + t0 = time.time() + last_max = 0.0 + first_max: float | None = None + stagnant_since: float | None = None + try: + while True: + time.sleep(_POLL_SEC) + cnt, min_ts, max_ts = _table_stats(db_path, "fastlio_odometry") + if cnt == 0: + continue + if first_max is None: + first_max = min_ts + if args.max_sensor_sec > 0 and (max_ts - first_max) >= args.max_sensor_sec: + print( + f"[pcap_to_db] reached --max-sensor-sec={args.max_sensor_sec:.1f}s", + flush=True, + ) + break + if max_ts == last_max: + if stagnant_since is None: + stagnant_since = time.time() + elif time.time() - stagnant_since > _STAGNANT_SEC: + break + else: + last_max = max_ts + stagnant_since = None + finally: + coord.stop() + + o_cnt, o_min, o_max = _table_stats(db_path, "fastlio_odometry") + l_cnt = _table_stats(db_path, "fastlio_lidar")[0] + span = o_max - o_min + print( + f"[pcap_to_db] done odom={o_cnt} lidar={l_cnt} " + f"ts=[{o_min:.3f}, {o_max:.3f}] span={span:.1f}s " + f"wall={time.time() - t0:.1f}s", + flush=True, + ) + return 0 + + +def main(argv: list[str]) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--pcap", required=True, help="Livox Mid-360 pcap capture") + parser.add_argument("--db", required=True, help="target memory2 SQLite db (appended to)") + parser.add_argument( + "--odom-freq", + type=float, + default=30.0, + help="FAST-LIO odometry publish rate in Hz (default 30)", + ) + parser.add_argument( + "--max-velocity-norm-ms", + type=float, + default=_GO2_MAX_VELOCITY_MS, + help=f"post-update velocity cap in m/s, anti-divergence (default {_GO2_MAX_VELOCITY_MS} " + "for go2; 0 disables)", + ) + parser.add_argument( + "--config", + type=str, + default=None, + help="FAST-LIO yaml (relative to config/ or absolute); omit for the module default", + ) + parser.add_argument( + "--max-sensor-sec", + type=float, + default=0.0, + help="stop after this many seconds of sensor time (0 = whole pcap)", + ) + parser.add_argument( + "--time-offset", + type=float, + default=None, + help="seconds added to every output ts; omit to auto-derive from the db clock", + ) + parser.add_argument( + "--force", + action="store_true", + help="overwrite existing fastlio_odometry/fastlio_lidar streams in the db", + ) + return _run(parser.parse_args(argv)) + + +if __name__ == "__main__": + raise SystemExit(main(sys.argv[1:])) From 6e5e336b0a69152c02186776498c273cc9426b3d Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 12 Jun 2026 18:36:38 -0700 Subject: [PATCH 043/185] fastlio2: remove the velocity cap entirely Drop max_velocity_norm_ms (post-IESKF velocity cap) from the module, native binary, replay tool, and the go2 record blueprint, and revert the fast-lio flake input from the velocity-cap branch back to v0.3.0-quiet-logs (whose only delta was the cap). FAST-LIO now runs unclamped. --- .../hardware/sensors/lidar/fastlio2/cpp/flake.lock | 8 ++++---- .../hardware/sensors/lidar/fastlio2/cpp/flake.nix | 2 +- dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp | 11 ----------- dimos/hardware/sensors/lidar/fastlio2/module.py | 7 ------- .../sensors/lidar/fastlio2/tools/pcap_to_db.py | 14 +------------- dimos/mapping/recording/go2_mid360/record.py | 1 - 6 files changed, 6 insertions(+), 37 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock index 615efd3891..ed10ba8629 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock @@ -37,16 +37,16 @@ "fast-lio": { "flake": false, "locked": { - "lastModified": 1780353421, - "narHash": "sha256-HL64XVYsbiEbXqADj07BwX1Eij2Xm6vj+a2o0RQhSrs=", + "lastModified": 1778784133, + "narHash": "sha256-bZEmIJlrzcqrHLxCiXXXf7fgi2yjVRuad8z08HN1eMU=", "owner": "dimensionalOS", "repo": "dimos-module-fastlio2", - "rev": "fd28a0995384352962558c4bf843d7bdfc17c1ed", + "rev": "d93bc6795babe9e63cc77d2bf2b72294d9afa839", "type": "github" }, "original": { "owner": "dimensionalOS", - "ref": "jeff/feat/velocity-cap", + "ref": "v0.3.0-quiet-logs", "repo": "dimos-module-fastlio2", "type": "github" } diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix index 9ddaa468a7..d73c4fd631 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix @@ -13,7 +13,7 @@ flake = false; }; fast-lio = { - url = "github:dimensionalOS/dimos-module-fastlio2/jeff/feat/velocity-cap"; + url = "github:dimensionalOS/dimos-module-fastlio2/v0.3.0-quiet-logs"; flake = false; }; lcm-extended = { diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index 5381852663..8c0565bf36 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -506,14 +506,6 @@ int main(int argc, char** argv) { double msr_freq = mod.arg_float("msr_freq", 50.0f); double main_freq = mod.arg_float("main_freq", 5000.0f); - // Post-IESKF-update velocity cap. When |v_world| exceeds this value - // the EKF state is restored to the last accepted scan with vel=0 and - // map_incremental is skipped. Breaks the reinforcing-loop divergence - // that gives FAST-LIO multi-km/s velocity runaway on aggressive - // motion or large IMU gaps. Zero disables. Defaults sized to the - // Go2 quadruped envelope (~3.1 m/s); raise for faster platforms. - double max_velocity_norm_ms = mod.arg_float("max_velocity_norm_ms", 0.0f); - // Livox hardware config std::string host_ip = mod.arg("host_ip", "192.168.1.5"); std::string lidar_ip = mod.arg("lidar_ip", "192.168.1.155"); @@ -634,9 +626,6 @@ int main(int argc, char** argv) { // Init FAST-LIO with config if (debug) printf("[fastlio2] Initializing FAST-LIO...\n"); FastLio fast_lio(config_path, msr_freq, main_freq); - if (max_velocity_norm_ms > 0.0) { - fast_lio.set_max_velocity_norm_ms(max_velocity_norm_ms); - } g_fastlio = &fast_lio; if (debug) printf("[fastlio2] FAST-LIO initialized.\n"); diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index dc6803ceed..7113a5de3a 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -107,13 +107,6 @@ class FastLio2Config(NativeModuleConfig): map_voxel_size: float = 0.1 map_max_range: float = 100.0 - # VERY IMPORTANT - # this is used to prevent catestrophic divergence - # go2 dog should set this to 3.1 m/s - # it needs some buffer room (dog can't actually move that fast) - # but other than that buffer room, tigher=less chance of catestrophic divergence - max_velocity_norm_ms: float = 0.0 - # FAST-LIO YAML config (relative to config/ dir, or absolute path) # C++ binary reads YAML directly via yaml-cpp config: Annotated[ diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index 50685f8fec..811163aa0f 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -77,10 +77,6 @@ _POLL_SEC = 1.0 # Stop after the odom stream has been stagnant this long (pcap fully drained). _STAGNANT_SEC = 6.0 -# Go2 quadruped post-update velocity cap (m/s). Breaks the FAST-LIO velocity -# runaway on aggressive motion; the dog cannot physically exceed this, so it -# only ever clamps divergence. Zero disables. See FastLio2Config. -_GO2_MAX_VELOCITY_MS = 3.1 class RecConfig(ModuleConfig): @@ -241,7 +237,7 @@ def _run(args: argparse.Namespace) -> int: offset_desc = f"auto: db wall-clock (R0={ref_start_ts:.2f})" print( f"[pcap_to_db] pcap={pcap_path.name} db={db_path.name} " - f"odom_freq={args.odom_freq}Hz vmax={args.max_velocity_norm_ms}m/s offset={offset_desc}", + f"odom_freq={args.odom_freq}Hz offset={offset_desc}", flush=True, ) @@ -249,7 +245,6 @@ def _run(args: argparse.Namespace) -> int: frame_id="world", map_freq=-1, odom_freq=args.odom_freq, - max_velocity_norm_ms=args.max_velocity_norm_ms, replay_pcap=pcap_path, deterministic_clock=False, debug=False, @@ -324,13 +319,6 @@ def main(argv: list[str]) -> int: default=30.0, help="FAST-LIO odometry publish rate in Hz (default 30)", ) - parser.add_argument( - "--max-velocity-norm-ms", - type=float, - default=_GO2_MAX_VELOCITY_MS, - help=f"post-update velocity cap in m/s, anti-divergence (default {_GO2_MAX_VELOCITY_MS} " - "for go2; 0 disables)", - ) parser.add_argument( "--config", type=str, diff --git a/dimos/mapping/recording/go2_mid360/record.py b/dimos/mapping/recording/go2_mid360/record.py index 84ebf84076..69c606e5d1 100644 --- a/dimos/mapping/recording/go2_mid360/record.py +++ b/dimos/mapping/recording/go2_mid360/record.py @@ -148,7 +148,6 @@ class FastLio2NoCap(FastLio2): frame_id="world", map_freq=-1, lidar_ip=_LIDAR_IP, - max_velocity_norm_ms=3.1, # meters/sec, 3.1 => 7mph, 5=>12mph. We want some padding ).remappings( [ (FastLio2, "lidar", "fastlio_lidar"), From 01d8872951c44458dcb0dc967f24b4f8a4f813d3 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 12 Jun 2026 18:47:02 -0700 Subject: [PATCH 044/185] pointlio: single default.yaml config; drop extra sensor presets + demo tools - Consolidate config/ to one default.yaml (the tuned Mid-360/iVox config); remove the unused upstream sensor presets (avia, horizon, marsim, ouster64, velodyne, mid360). Module now defaults to default.yaml. - Remove tools/replay_ruwik2_pt3.py (pcap_to_db.py covers offline replay) and tools/demo_live_test.py. --- .../sensors/lidar/pointlio/config/avia.yaml | 35 --- .../lidar/pointlio/config/default.yaml | 81 ++++-- .../lidar/pointlio/config/horizon.yaml | 35 --- .../sensors/lidar/pointlio/config/marsim.yaml | 35 --- .../sensors/lidar/pointlio/config/mid360.yaml | 74 ----- .../lidar/pointlio/config/ouster64.yaml | 36 --- .../lidar/pointlio/config/velodyne.yaml | 37 --- .../sensors/lidar/pointlio/cpp/README.md | 2 +- .../sensors/lidar/pointlio/cpp/main.cpp | 2 +- .../hardware/sensors/lidar/pointlio/module.py | 2 +- .../lidar/pointlio/tools/demo_live_test.py | 121 -------- .../lidar/pointlio/tools/replay_ruwik2_pt3.py | 261 ------------------ 12 files changed, 64 insertions(+), 657 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/pointlio/config/avia.yaml delete mode 100644 dimos/hardware/sensors/lidar/pointlio/config/horizon.yaml delete mode 100644 dimos/hardware/sensors/lidar/pointlio/config/marsim.yaml delete mode 100644 dimos/hardware/sensors/lidar/pointlio/config/mid360.yaml delete mode 100644 dimos/hardware/sensors/lidar/pointlio/config/ouster64.yaml delete mode 100644 dimos/hardware/sensors/lidar/pointlio/config/velodyne.yaml delete mode 100644 dimos/hardware/sensors/lidar/pointlio/tools/demo_live_test.py delete mode 100644 dimos/hardware/sensors/lidar/pointlio/tools/replay_ruwik2_pt3.py diff --git a/dimos/hardware/sensors/lidar/pointlio/config/avia.yaml b/dimos/hardware/sensors/lidar/pointlio/config/avia.yaml deleted file mode 100644 index 8447b64658..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/config/avia.yaml +++ /dev/null @@ -1,35 +0,0 @@ -common: - lid_topic: "/livox/lidar" - imu_topic: "/livox/imu" - time_sync_en: false # ONLY turn on when external time synchronization is really not possible - time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). - # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 - -preprocess: - lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, - scan_line: 6 - blind: 4 - -mapping: - acc_cov: 0.1 - gyr_cov: 0.1 - b_acc_cov: 0.0001 - b_gyr_cov: 0.0001 - fov_degree: 90 - det_range: 450.0 - extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic - extrinsic_T: [ 0.04165, 0.02326, -0.0284 ] - extrinsic_R: [ 1, 0, 0, - 0, 1, 0, - 0, 0, 1] - -publish: - path_en: false - scan_publish_en: true # false: close all the point cloud output - dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. - scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame - -pcd_save: - pcd_save_en: true - interval: -1 # how many LiDAR frames saved in each pcd file; - # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/pointlio/config/default.yaml b/dimos/hardware/sensors/lidar/pointlio/config/default.yaml index 688597d850..a989359c0c 100644 --- a/dimos/hardware/sensors/lidar/pointlio/config/default.yaml +++ b/dimos/hardware/sensors/lidar/pointlio/config/default.yaml @@ -1,32 +1,73 @@ +# Non-ROS Point-LIO config (Mid-360). Parsed by parameters.cpp via yaml-cpp. common: - lid_topic: "/livox/lidar" - imu_topic: "/livox/imu" - time_sync_en: false - time_offset_lidar_to_imu: 0.0 + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + con_frame: false + con_frame_num: 1 + cut_frame: false + cut_frame_time_interval: 0.1 + time_lag_imu_to_lidar: 0.0 preprocess: - lidar_type: 1 # 1 for Livox serials LiDAR + lidar_type: 1 # 1 = Livox CustomMsg scan_line: 4 - blind: 0.5 # spherical min range (metres) + scan_rate: 10 + timestamp_unit: 3 # 3 = nanosecond + blind: 0.5 + point_filter_num: 3 mapping: - acc_cov: 0.01 # tighter than mid360 default (0.1) - gyr_cov: 0.01 # tighter than mid360 default (0.1) - b_acc_cov: 0.0001 + use_imu_as_input: false # false = IMU-as-output model (Point-LIO's robust path) + prop_at_freq_of_imu: true + check_satu: true + init_map_size: 10 + space_down_sample: true + satu_acc: 3.0 # g (acc_norm=1.0). hku-mars master value: accel >= 3g saturated → EKF residual zeroed, keeps velocity bounded. + satu_gyro: 35.0 + acc_norm: 1.0 # IMU accel in g + plane_thr: 0.1 + filter_size_surf: 0.5 + filter_size_map: 0.5 + ivox_grid_resolution: 2.0 # iVox local-map grid (m). Matches hku-mars master avia/horizon Livox configs. + ivox_nearby_type: 6 # NEARBY6. Matches the hku_repro mapping_mid360.launch override (NOT the yaml default) that produced the bounded 5.028m baseline. + cube_side_length: 1000.0 + det_range: 100.0 + fov_degree: 360.0 + imu_en: true + start_in_aggressive_motion: false + extrinsic_est_en: false + imu_time_inte: 0.005 + lidar_meas_cov: 0.01 + acc_cov_input: 0.1 + vel_cov: 20.0 + gyr_cov_input: 0.01 + gyr_cov_output: 1000.0 + acc_cov_output: 500.0 b_gyr_cov: 0.0001 - fov_degree: 360 - det_range: 60.0 # reduced from 100 — less noise from distant points - extrinsic_est_en: true # enable live calibration - extrinsic_T: [ -0.011, -0.02329, 0.04412 ] - extrinsic_R: [ 1, 0, 0, - 0, 1, 0, - 0, 0, 1] + b_acc_cov: 0.0001 + imu_meas_acc_cov: 0.01 + imu_meas_omg_cov: 0.01 + match_s: 81.0 + gravity_align: true + gravity: [0.0, 0.0, -9.810] + gravity_init: [0.0, 0.0, -9.810] + extrinsic_T: [-0.011, -0.02329, 0.04412] + extrinsic_R: [1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0] + +odometry: + publish_odometry_without_downsample: false + odom_only: false + odom_header_frame_id: "camera_init" + odom_child_frame_id: "aft_mapped" publish: - path_en: false - scan_publish_en: true - dense_publish_en: true - scan_bodyframe_pub_en: true + path_en: false + scan_publish_en: false + scan_bodyframe_pub_en: false + +runtime_pos_log_enable: false pcd_save: pcd_save_en: false diff --git a/dimos/hardware/sensors/lidar/pointlio/config/horizon.yaml b/dimos/hardware/sensors/lidar/pointlio/config/horizon.yaml deleted file mode 100644 index 43db0c3bff..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/config/horizon.yaml +++ /dev/null @@ -1,35 +0,0 @@ -common: - lid_topic: "/livox/lidar" - imu_topic: "/livox/imu" - time_sync_en: false # ONLY turn on when external time synchronization is really not possible - time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). - # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 - -preprocess: - lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, - scan_line: 6 - blind: 4 - -mapping: - acc_cov: 0.1 - gyr_cov: 0.1 - b_acc_cov: 0.0001 - b_gyr_cov: 0.0001 - fov_degree: 100 - det_range: 260.0 - extrinsic_est_en: true # true: enable the online estimation of IMU-LiDAR extrinsic - extrinsic_T: [ 0.05512, 0.02226, -0.0297 ] - extrinsic_R: [ 1, 0, 0, - 0, 1, 0, - 0, 0, 1] - -publish: - path_en: false - scan_publish_en: true # false: close all the point cloud output - dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. - scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame - -pcd_save: - pcd_save_en: true - interval: -1 # how many LiDAR frames saved in each pcd file; - # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/pointlio/config/marsim.yaml b/dimos/hardware/sensors/lidar/pointlio/config/marsim.yaml deleted file mode 100644 index ad6c89121a..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/config/marsim.yaml +++ /dev/null @@ -1,35 +0,0 @@ -common: - lid_topic: "/quad0_pcl_render_node/sensor_cloud" - imu_topic: "/quad_0/imu" - time_sync_en: false # ONLY turn on when external time synchronization is really not possible - time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). - # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 - -preprocess: - lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, - scan_line: 4 - blind: 0.5 - -mapping: - acc_cov: 0.1 - gyr_cov: 0.1 - b_acc_cov: 0.0001 - b_gyr_cov: 0.0001 - fov_degree: 90 - det_range: 50.0 - extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic - extrinsic_T: [ -0.0, -0.0, 0.0 ] - extrinsic_R: [ 1, 0, 0, - 0, 1, 0, - 0, 0, 1] - -publish: - path_en: false - scan_publish_en: true # false: close all the point cloud output - dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. - scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame - -pcd_save: - pcd_save_en: true - interval: -1 # how many LiDAR frames saved in each pcd file; - # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/pointlio/config/mid360.yaml b/dimos/hardware/sensors/lidar/pointlio/config/mid360.yaml deleted file mode 100644 index a989359c0c..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/config/mid360.yaml +++ /dev/null @@ -1,74 +0,0 @@ -# Non-ROS Point-LIO config (Mid-360). Parsed by parameters.cpp via yaml-cpp. -common: - lid_topic: "/livox/lidar" - imu_topic: "/livox/imu" - con_frame: false - con_frame_num: 1 - cut_frame: false - cut_frame_time_interval: 0.1 - time_lag_imu_to_lidar: 0.0 - -preprocess: - lidar_type: 1 # 1 = Livox CustomMsg - scan_line: 4 - scan_rate: 10 - timestamp_unit: 3 # 3 = nanosecond - blind: 0.5 - point_filter_num: 3 - -mapping: - use_imu_as_input: false # false = IMU-as-output model (Point-LIO's robust path) - prop_at_freq_of_imu: true - check_satu: true - init_map_size: 10 - space_down_sample: true - satu_acc: 3.0 # g (acc_norm=1.0). hku-mars master value: accel >= 3g saturated → EKF residual zeroed, keeps velocity bounded. - satu_gyro: 35.0 - acc_norm: 1.0 # IMU accel in g - plane_thr: 0.1 - filter_size_surf: 0.5 - filter_size_map: 0.5 - ivox_grid_resolution: 2.0 # iVox local-map grid (m). Matches hku-mars master avia/horizon Livox configs. - ivox_nearby_type: 6 # NEARBY6. Matches the hku_repro mapping_mid360.launch override (NOT the yaml default) that produced the bounded 5.028m baseline. - cube_side_length: 1000.0 - det_range: 100.0 - fov_degree: 360.0 - imu_en: true - start_in_aggressive_motion: false - extrinsic_est_en: false - imu_time_inte: 0.005 - lidar_meas_cov: 0.01 - acc_cov_input: 0.1 - vel_cov: 20.0 - gyr_cov_input: 0.01 - gyr_cov_output: 1000.0 - acc_cov_output: 500.0 - b_gyr_cov: 0.0001 - b_acc_cov: 0.0001 - imu_meas_acc_cov: 0.01 - imu_meas_omg_cov: 0.01 - match_s: 81.0 - gravity_align: true - gravity: [0.0, 0.0, -9.810] - gravity_init: [0.0, 0.0, -9.810] - extrinsic_T: [-0.011, -0.02329, 0.04412] - extrinsic_R: [1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0] - -odometry: - publish_odometry_without_downsample: false - odom_only: false - odom_header_frame_id: "camera_init" - odom_child_frame_id: "aft_mapped" - -publish: - path_en: false - scan_publish_en: false - scan_bodyframe_pub_en: false - -runtime_pos_log_enable: false - -pcd_save: - pcd_save_en: false - interval: -1 diff --git a/dimos/hardware/sensors/lidar/pointlio/config/ouster64.yaml b/dimos/hardware/sensors/lidar/pointlio/config/ouster64.yaml deleted file mode 100644 index 9d891bbeba..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/config/ouster64.yaml +++ /dev/null @@ -1,36 +0,0 @@ -common: - lid_topic: "/os_cloud_node/points" - imu_topic: "/os_cloud_node/imu" - time_sync_en: false # ONLY turn on when external time synchronization is really not possible - time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). - # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 - -preprocess: - lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, - scan_line: 64 - timestamp_unit: 3 # 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. - blind: 4 - -mapping: - acc_cov: 0.1 - gyr_cov: 0.1 - b_acc_cov: 0.0001 - b_gyr_cov: 0.0001 - fov_degree: 180 - det_range: 150.0 - extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic - extrinsic_T: [ 0.0, 0.0, 0.0 ] - extrinsic_R: [1, 0, 0, - 0, 1, 0, - 0, 0, 1] - -publish: - path_en: false - scan_publish_en: true # false: close all the point cloud output - dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. - scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame - -pcd_save: - pcd_save_en: true - interval: -1 # how many LiDAR frames saved in each pcd file; - # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/pointlio/config/velodyne.yaml b/dimos/hardware/sensors/lidar/pointlio/config/velodyne.yaml deleted file mode 100644 index 450eda48b8..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/config/velodyne.yaml +++ /dev/null @@ -1,37 +0,0 @@ -common: - lid_topic: "/velodyne_points" - imu_topic: "/imu/data" - time_sync_en: false # ONLY turn on when external time synchronization is really not possible - time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). - # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 - -preprocess: - lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, - scan_line: 32 - scan_rate: 10 # only need to be set for velodyne, unit: Hz, - timestamp_unit: 2 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. - blind: 2 - -mapping: - acc_cov: 0.1 - gyr_cov: 0.1 - b_acc_cov: 0.0001 - b_gyr_cov: 0.0001 - fov_degree: 180 - det_range: 100.0 - extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, - extrinsic_T: [ 0, 0, 0.28] - extrinsic_R: [ 1, 0, 0, - 0, 1, 0, - 0, 0, 1] - -publish: - path_en: false - scan_publish_en: true # false: close all the point cloud output - dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. - scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame - -pcd_save: - pcd_save_en: true - interval: -1 # how many LiDAR frames saved in each pcd file; - # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/README.md b/dimos/hardware/sensors/lidar/pointlio/cpp/README.md index bbbfdf1929..61ef47384b 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/README.md +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/README.md @@ -74,7 +74,7 @@ autoconnect( --odometry '/odometry#nav_msgs.Odometry' \ --host_ip 192.168.1.5 \ --lidar_ip 192.168.1.155 \ - --config_path ../config/mid360.yaml + --config_path ../config/default.yaml ``` Topic strings must include the `#type` suffix -- this is the actual LCM channel diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 865fe49585..72b59f5682 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -11,7 +11,7 @@ // ./fastlio2_native \ // --lidar '/lidar#sensor_msgs.PointCloud2' \ // --odometry '/odometry#nav_msgs.Odometry' \ -// --config_path /path/to/mid360.yaml \ +// --config_path /path/to/default.yaml \ // --host_ip 192.168.1.5 --lidar_ip 192.168.1.155 #include diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index 02b670cfa5..9faa44d474 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -111,7 +111,7 @@ class PointLioConfig(NativeModuleConfig): # C++ binary reads YAML directly via yaml-cpp config: Annotated[ Path, validate_as(...).transform(lambda p: p if p.is_absolute() else _CONFIG_DIR / p) - ] = Path("mid360.yaml") + ] = Path("default.yaml") debug: bool = False diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/demo_live_test.py b/dimos/hardware/sensors/lidar/pointlio/tools/demo_live_test.py deleted file mode 100644 index 4bba36ec2a..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/tools/demo_live_test.py +++ /dev/null @@ -1,121 +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. - -"""Live Mid-360 smoke test for the Point-LIO module. - -Runs the PointLio module against the physically-connected lidar for a short -window, mirrors odometry into a sqlite db, then prints a summary so we can -confirm the EKF stays bounded (stationary ⇒ near-origin, no satu_acc runaway). - - cd ~/repos/dimos6 && source .venv/bin/activate - python -m dimos.hardware.sensors.lidar.pointlio.tools.demo_live_test -""" - -from __future__ import annotations - -from collections.abc import AsyncIterator -import os -from pathlib import Path -import sqlite3 -import sys -import time - -from dimos.core.module import Module, ModuleConfig -from dimos.core.stream import In -from dimos.msgs.nav_msgs.Odometry import Odometry - -RUN_SEC = float(os.environ.get("RUN_SEC", "25.0")) -DB_PATH = Path("/tmp/pointlio_live.db") - -_EPS = 1e-9 - - -class RecConfig(ModuleConfig): - db_path: str = "" - - -class Rec(Module): - config: RecConfig - pointlio_odometry: In[Odometry] - _last_o: float = 0.0 - - async def main(self) -> AsyncIterator[None]: - from dimos.memory2.store.sqlite import SqliteStore - - self._store = SqliteStore(path=self.config.db_path) - self._os = self._store.stream("pointlio_odometry", Odometry) - yield - self._store.stop() - - async def handle_pointlio_odometry(self, v: Odometry) -> None: - ts = max(getattr(v, "ts", None) or time.time(), self._last_o + _EPS) - self._last_o = ts - pose = getattr(v, "pose", None) - pose_inner = getattr(pose, "pose", None) if pose is not None else None - self._os.append(v, ts=ts, pose=pose_inner) - - -def main() -> int: - if DB_PATH.exists(): - DB_PATH.unlink() - - from dimos.core.coordination.blueprints import autoconnect - from dimos.core.coordination.module_coordinator import ModuleCoordinator - from dimos.hardware.sensors.lidar.pointlio.module import PointLio - - bp = autoconnect( - PointLio.blueprint(frame_id="world", map_freq=-1, debug=False).remappings( - [(PointLio, "odometry", "pointlio_odometry")] - ), - Rec.blueprint(db_path=str(DB_PATH)), - ).global_config(n_workers=2, robot_model="mid360_pointlio_live_test") - coord = ModuleCoordinator.build(bp) - - t0 = time.time() - try: - while time.time() - t0 < RUN_SEC: - time.sleep(1.0) - finally: - coord.stop() - - if not DB_PATH.exists(): - print("[live_test] NO DB — module never produced odometry", file=sys.stderr) - return 1 - con = sqlite3.connect(f"file:{DB_PATH}?mode=ro", uri=True) - rows = list(con.execute("SELECT ts,pose_x,pose_y,pose_z FROM pointlio_odometry ORDER BY ts")) - con.close() - print(f"[live_test] odometry rows: {len(rows)}") - if not rows: - print("[live_test] NO ROWS — lidar likely not streaming", file=sys.stderr) - return 1 - import math - - t0r = rows[0][0] - xs = [r[1] for r in rows] - ys = [r[2] for r in rows] - zs = [r[3] for r in rows] - dmax = max(math.sqrt(x * x + y * y + z * z) for x, y, z in zip(xs, ys, zs, strict=False)) - print( - f"[live_test] dur={rows[-1][0] - t0r:.1f}s rate≈{len(rows) / max(rows[-1][0] - t0r, 1e-3):.1f}Hz" - ) - print( - f"[live_test] x=({min(xs):.3f},{max(xs):.3f}) y=({min(ys):.3f},{max(ys):.3f}) z=({min(zs):.3f},{max(zs):.3f})" - ) - print(f"[live_test] max |pos| from origin: {dmax:.3f} m") - print(f"[live_test] final pos: ({xs[-1]:.3f},{ys[-1]:.3f},{zs[-1]:.3f})") - return 0 - - -if __name__ == "__main__": - raise SystemExit(main()) diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/replay_ruwik2_pt3.py b/dimos/hardware/sensors/lidar/pointlio/tools/replay_ruwik2_pt3.py deleted file mode 100644 index e4c6058cf3..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/tools/replay_ruwik2_pt3.py +++ /dev/null @@ -1,261 +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. - -"""Replay the ruwik2_pt3 5/29 pcap through the Point-LIO native module. - -Mirror of the fastlio2 replay_ruwik2_pt3 harness, but driving the PointLio -module. Same orchestrator+worker shape, same pcap, same deterministic clock. -Captures PointLio odometry into a per-attempt sqlite db so the trajectory can -be compared against the fastlio replay (which diverges to ~2257 m/s on this -same wire data). - -Run from the dimos6 venv: - - cd ~/repos/dimos6 - source .venv/bin/activate - python -m dimos.hardware.sensors.lidar.pointlio.tools.replay_ruwik2_pt3 -""" - -from __future__ import annotations - -from collections.abc import AsyncIterator -import json -import os -from pathlib import Path -import subprocess -import sys -import time - -from dimos.core.module import Module, ModuleConfig -from dimos.core.stream import In -from dimos.msgs.nav_msgs.Odometry import Odometry - -PCAP_PATH = Path( - os.environ.get( - "REPLAY_PCAP_PATH", - "/home/dimos/repos/dimos/fastlio2_pcap/mid360_20260529_225346.pcap", - ) -) - -RUNS_ROOT = Path("/home/dimos/repos/dimos6/pointlio_ruwik2_pt3_replays") - -_ATTEMPT_DIR_ENV = "_REPLAY_POINTLIO_RUWIK2_PT3_ATTEMPT_DIR" - -MAX_WALL_SEC = float(os.environ.get("REPLAY_MAX_WALL_SEC", "480.0")) - -REPLAY_MAX_SENSOR_SEC = float(os.environ.get("REPLAY_MAX_SENSOR_SEC", "60.0")) - -# deterministic_clock=true cuts scan boundaries on the sensor virtual clock read -# atomically under g_pc_mutex, which removes the feeder/main scan-composition race -# and makes replay reproducibly bounded. Set REPLAY_DETERMINISTIC_CLOCK=0 to cut -# scans on wall time (realtime feeder) — that restores the live threading race and -# is the only way replay reproduces the live divergence offline. -REPLAY_DETERMINISTIC_CLOCK = os.environ.get("REPLAY_DETERMINISTIC_CLOCK", "1") != "0" - -# Feed point + IMU on two separate threads (mimics the live Livox SDK's -# concurrent delivery). Set REPLAY_DUAL_THREAD=1 with REPLAY_DETERMINISTIC_CLOCK=0 -# to reproduce live thread-interleaving offline. -REPLAY_DUAL_THREAD = os.environ.get("REPLAY_DUAL_THREAD", "0") == "1" - - -def _next_attempt_dir() -> Path: - RUNS_ROOT.mkdir(parents=True, exist_ok=True) - existing = sorted(p.name for p in RUNS_ROOT.iterdir() if p.name.startswith("attempt_")) - n = 0 - for name in existing: - try: - n = max(n, int(name.split("_", 1)[1]) + 1) - except ValueError: - continue - attempt = RUNS_ROOT / f"attempt_{n:03d}" - attempt.mkdir() - return attempt - - -def _commit_hash() -> str: - try: - return subprocess.check_output( - ["git", "-C", str(Path(__file__).resolve().parents[6]), "rev-parse", "HEAD"], - text=True, - ).strip() - except subprocess.CalledProcessError: - return "unknown" - - -class RecConfig(ModuleConfig): - """Configures Rec with the per-attempt sqlite db path.""" - - db_path: str = "" - - -_EPS = 1e-9 - - -class Rec(Module): - """Mirror replay PointLio odometry into a SqliteStore.""" - - config: RecConfig - pointlio_odometry: In[Odometry] - _last_o: float = 0.0 - - async def main(self) -> AsyncIterator[None]: - from dimos.memory2.store.sqlite import SqliteStore - - self._store = SqliteStore(path=self.config.db_path) - self._os = self._store.stream("pointlio_odometry", Odometry) - yield - self._store.stop() - - async def handle_pointlio_odometry(self, v: Odometry) -> None: - ts = max(getattr(v, "ts", None) or time.time(), self._last_o + _EPS) - self._last_o = ts - pose = getattr(v, "pose", None) - pose_inner = getattr(pose, "pose", None) if pose is not None else None - self._os.append(v, ts=ts, pose=pose_inner) - - -def _orchestrate() -> int: - if not PCAP_PATH.exists(): - print(f"[replay_pointlio] missing pcap: {PCAP_PATH}", file=sys.stderr) - return 2 - attempt_dir = _next_attempt_dir() - stdout_path = attempt_dir / "stdout.txt" - stderr_path = attempt_dir / "stderr.txt" - meta = { - "attempt_dir": str(attempt_dir), - "pcap_path": str(PCAP_PATH), - "commit": _commit_hash(), - "started_at": time.time(), - } - print(f"[replay_pointlio] attempt {attempt_dir.name} commit {meta['commit'][:8]}", flush=True) - t0 = time.time() - env = {**os.environ, _ATTEMPT_DIR_ENV: str(attempt_dir)} - with stdout_path.open("w") as out, stderr_path.open("w") as err: - rc = subprocess.run( - [sys.executable, "-m", __spec__.name, "--worker"], - stdout=out, - stderr=err, - env=env, - check=False, - ).returncode - meta["finished_at"] = time.time() - meta["wall_sec"] = meta["finished_at"] - t0 - meta["worker_rc"] = rc - (attempt_dir / "meta.json").write_text(json.dumps(meta, indent=2)) - print( - f"[replay_pointlio] done attempt {attempt_dir.name} rc={rc} wall={meta['wall_sec']:.1f}s", - flush=True, - ) - return rc - - -def _worker() -> int: - attempt_dir = Path(os.environ[_ATTEMPT_DIR_ENV]) - db_path = attempt_dir / "pointlio.db" - if db_path.exists(): - db_path.unlink() - db_path_str = str(db_path) - - from dimos.core.coordination.blueprints import autoconnect - from dimos.core.coordination.module_coordinator import ModuleCoordinator - from dimos.hardware.sensors.lidar.pointlio.module import PointLio - - subprocess.run(["kd"], check=False) - time.sleep(1.0) - - bp = autoconnect( - PointLio.blueprint( - frame_id="world", - map_freq=-1, - replay_pcap=PCAP_PATH, - deterministic_clock=REPLAY_DETERMINISTIC_CLOCK, - replay_dual_thread=REPLAY_DUAL_THREAD, - debug=False, - ).remappings( - [ - (PointLio, "odometry", "pointlio_odometry"), - ] - ), - Rec.blueprint(db_path=db_path_str), - ).global_config(n_workers=4, robot_model="mid360_pointlio_replay_ruwik2") - coord = ModuleCoordinator.build(bp) - - import sqlite3 - - t0 = time.time() - last_ts_seen = 0.0 - first_ts_seen = 0.0 - stagnant_since: float | None = None - saw_first_row = False - try: - while time.time() - t0 < MAX_WALL_SEC: - time.sleep(1.0) - if not db_path.exists(): - continue - try: - con = sqlite3.connect(f"file:{db_path_str}?mode=ro", uri=True, timeout=0.5) - row = con.execute( - "SELECT MIN(ts), MAX(ts), COUNT(*) FROM pointlio_odometry" - ).fetchone() - con.close() - except Exception: - continue - first_ts = row[0] if row and row[0] else 0.0 - last_ts = row[1] if row and row[1] else 0.0 - cnt = row[2] if row else 0 - if cnt > 0: - saw_first_row = True - if first_ts_seen == 0.0: - first_ts_seen = first_ts - if not saw_first_row: - continue - if ( - REPLAY_MAX_SENSOR_SEC > 0 - and first_ts_seen > 0 - and (last_ts - first_ts_seen) >= REPLAY_MAX_SENSOR_SEC - ): - print( - f"[replay_pointlio.worker] reached REPLAY_MAX_SENSOR_SEC=" - f"{REPLAY_MAX_SENSOR_SEC:.1f} s — stopping", - flush=True, - ) - break - if last_ts == last_ts_seen: - if stagnant_since is None: - stagnant_since = time.time() - elif time.time() - stagnant_since > 3.0: - break - else: - last_ts_seen = last_ts - stagnant_since = None - finally: - coord.stop() - - if db_path.exists(): - size_mb = db_path.stat().st_size / 1e6 - print( - f"[replay_pointlio.worker] db_size={size_mb:.2f}MB wall={time.time() - t0:.1f}s", - flush=True, - ) - return 0 - - -def main(argv: list[str]) -> int: - if len(argv) >= 2 and argv[1] == "--worker": - return _worker() - return _orchestrate() - - -if __name__ == "__main__": - raise SystemExit(main(sys.argv)) From 7ee330808aef2e212a9b749b3003c54890ebed00 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 12 Jun 2026 19:17:34 -0700 Subject: [PATCH 045/185] pointlio: pcap_to_db builds a db from scratch; keep recorder out of registry - --db is now optional: with no existing db, build one from scratch (defaults to .db next to the pcap); with an existing db, append + time-align as before. - Rename the internal recorder Rec/RecConfig -> _Rec/_RecConfig so the blueprint-registry generator skips it (a tool helper isn't a public module); fixes test_all_blueprints_is_current. - Docstring: document from-scratch generation using the ruwik2_part3 LFS sample. --- .../lidar/pointlio/tools/pcap_to_db.py | 55 +++++++++++++++---- 1 file changed, 43 insertions(+), 12 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py index b5b72eb470..dab9f64e66 100644 --- a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py @@ -12,17 +12,22 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Run Point-LIO over a .pcap and append its outputs into an existing .db. +"""Run Point-LIO over a .pcap and write its outputs into a .db. -Given a Livox Mid-360 pcap capture and a memory2 SQLite database, this streams -the pcap through the Point-LIO native module (deterministic clock, single feeder --> never loads the whole pcap into memory) and writes two streams into the -database, both time-aligned onto the db's existing clock: +Given a Livox Mid-360 pcap capture, this streams the pcap through the Point-LIO +native module (deterministic clock, single feeder -> never loads the whole pcap +into memory) and writes two streams into a memory2 SQLite database: * ``pointlio_odometry`` -- the IESKF pose at the native odom rate (~30 Hz). * ``pointlio_lidar`` -- the registered (deskewed, odom-frame) point cloud at the native pointcloud rate (~10 Hz). +The ``--db`` is optional. With no existing db the tool builds one **from +scratch** (omit ``--db`` and it defaults to ``.db`` next to the pcap). +With an existing db the two streams are appended and time-aligned onto the db's +clock, so Point-LIO output can be compared against whatever it already holds +(e.g. a fastlio replay). + If either stream already exists in the db the run aborts, unless ``--force`` is given, in which case the existing ``pointlio_odometry`` and ``pointlio_lidar`` streams are dropped before the new ones are written. @@ -44,6 +49,16 @@ Usage (from the dimos6 venv):: source .venv/bin/activate + + # Build a fresh db from scratch (no existing db needed). The ruwik2_part3 + # sample pcap is available via LFS: + PCAP=$(python -c "from dimos.utils.data import get_data; \ + print(get_data('ruwik2_part3') / 'ruwik2_part3.pcap')") + python -m dimos.hardware.sensors.lidar.pointlio.tools.pcap_to_db --pcap "$PCAP" + # -> writes ruwik2_part3.pcap.db next to the sample with pointlio_odometry + # + pointlio_lidar. + + # Or append into an existing recording db for comparison: python -m dimos.hardware.sensors.lidar.pointlio.tools.pcap_to_db \ --pcap /path/to/capture.pcap --db /path/to/memory.db """ @@ -73,7 +88,7 @@ _STAGNANT_SEC = 4.0 -class RecConfig(ModuleConfig): +class _RecConfig(ModuleConfig): """Configures the recorder with the target db and timing conversion.""" db_path: str = "" @@ -83,10 +98,14 @@ class RecConfig(ModuleConfig): time_offset: float = float("nan") -class Rec(Module): - """Append Point-LIO odometry + lidar into an existing SQLite db with ts conversion.""" +class _Rec(Module): + """Append Point-LIO odometry + lidar into a SQLite db with ts conversion. + + Underscore-prefixed so the blueprint registry generator skips it — this is + an internal helper for the tool, not a public robot module. + """ - config: RecConfig + config: _RecConfig pointlio_odometry: In[Odometry] pointlio_lidar: In[PointCloud2] _offset: float | None = None @@ -186,13 +205,18 @@ def _table_stats(db_path: Path, table: str) -> tuple[int, float, float]: def _run(args: argparse.Namespace) -> int: pcap_path = Path(args.pcap).expanduser().resolve() - db_path = Path(args.db).expanduser().resolve() if not pcap_path.exists(): print(f"[pcap_to_db] missing pcap: {pcap_path}", file=sys.stderr) return 2 if args.max_sensor_sec < 0: print("[pcap_to_db] --max-sensor-sec must be >= 0", file=sys.stderr) return 2 + # --db is optional: with no existing db, build one from scratch. When + # omitted the output defaults to .db next to the pcap, so a fresh + # db can be generated with just --pcap. + db_path = Path(args.db).expanduser().resolve() if args.db else pcap_path.with_suffix(".db") + db_existed = db_path.exists() + db_path.parent.mkdir(parents=True, exist_ok=True) from dimos.core.coordination.blueprints import autoconnect from dimos.core.coordination.module_coordinator import ModuleCoordinator @@ -228,6 +252,7 @@ def _run(args: argparse.Namespace) -> int: offset_desc = f"auto: db wall-clock (R0={ref_start_ts:.2f}) -> start-align" print( f"[pcap_to_db] pcap={pcap_path.name} db={db_path.name} " + f"({'append' if db_existed else 'new'}) " f"odom_freq={args.odom_freq}Hz offset={offset_desc}", flush=True, ) @@ -247,7 +272,7 @@ def _run(args: argparse.Namespace) -> int: (PointLio, "lidar", "pointlio_lidar"), ] ), - Rec.blueprint( + _Rec.blueprint( db_path=str(db_path), ref_start_ts=ref_start_ts, time_offset=time_offset, @@ -299,7 +324,13 @@ def _run(args: argparse.Namespace) -> int: def main(argv: list[str]) -> int: parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("--pcap", required=True, help="Livox Mid-360 pcap capture") - parser.add_argument("--db", required=True, help="target memory2 SQLite db (appended to)") + parser.add_argument( + "--db", + default=None, + help="target memory2 SQLite db. If it exists, pointlio streams are appended/aligned " + "onto its clock; if it doesn't, a fresh db is built from scratch. " + "Omit to default to .db next to the pcap.", + ) parser.add_argument( "--odom-freq", type=float, From b0c8b2a5881db04720c544d4f47eb3760ebe92b4 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 12 Jun 2026 19:20:51 -0700 Subject: [PATCH 046/185] fastlio2: privatize pcap_to_db recorder + regenerate all_blueprints Rename Rec/RecConfig -> _Rec/_RecConfig so the blueprint generator skips them (underscore-prefixed classes are not auto-registered), and regenerate all_blueprints.py to reconcile it with the merged code (the -X ours merge had kept a stale copy). Fixes test_all_blueprints_is_current. --- .../hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py | 8 ++++---- dimos/robot/all_blueprints.py | 9 +++++++-- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index 811163aa0f..a4ec61782a 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -79,7 +79,7 @@ _STAGNANT_SEC = 6.0 -class RecConfig(ModuleConfig): +class _RecConfig(ModuleConfig): """Configures the recorder with the target db and timing conversion.""" db_path: str = "" @@ -89,10 +89,10 @@ class RecConfig(ModuleConfig): time_offset: float = float("nan") -class Rec(Module): +class _Rec(Module): """Append FAST-LIO odometry + lidar into an existing SQLite db with ts conversion.""" - config: RecConfig + config: _RecConfig fastlio_odometry: In[Odometry] fastlio_lidar: In[PointCloud2] _offset: float | None = None @@ -260,7 +260,7 @@ def _run(args: argparse.Namespace) -> int: ) blueprint = autoconnect( fastlio, - Rec.blueprint( + _Rec.blueprint( db_path=str(db_path), ref_start_ts=ref_start_ts, time_offset=time_offset, diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 291c3f9d39..a1f91e350a 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -72,6 +72,7 @@ "openarm-mock-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints:openarm_mock_planner_coordinator", "openarm-planner-coordinator": "dimos.robot.manipulators.openarm.blueprints:openarm_planner_coordinator", "path-planner-eval": "dimos.navigation.nav_3d.evaluator.blueprints:path_planner_eval", + "realsense-mid360-record": "dimos.mapping.recording.mid360_realsense.record:realsense_mid360_record", "teleop-phone": "dimos.teleop.phone.blueprints:teleop_phone", "teleop-phone-go2": "dimos.teleop.phone.blueprints:teleop_phone_go2", "teleop-phone-go2-fleet": "dimos.teleop.phone.blueprints:teleop_phone_go2_fleet", @@ -107,7 +108,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-record": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_record:unitree_go2_record", + "unitree-go2-record": "dimos.mapping.recording.go2_mid360.record:unitree_go2_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", @@ -154,6 +155,7 @@ "evaluator": "dimos.navigation.nav_3d.evaluator.evaluator.Evaluator", "far-planner": "dimos.navigation.nav_stack.modules.far_planner.far_planner.FarPlanner", "fast-lio2": "dimos.hardware.sensors.lidar.fastlio2.module.FastLio2", + "fast-lio2-no-cap": "dimos.mapping.recording.go2_mid360.record.FastLio2NoCap", "fast-lio2-recorder": "dimos.hardware.sensors.lidar.fastlio2.recorder.FastLio2Recorder", "g1-connection": "dimos.robot.unitree.g1.connection.G1Connection", "g1-connection-base": "dimos.robot.unitree.g1.connection.G1ConnectionBase", @@ -165,6 +167,7 @@ "go2-fleet-connection": "dimos.robot.unitree.go2.fleet_connection.Go2FleetConnection", "go2-memory": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2.Go2Memory", "go2-teleop-module": "dimos.teleop.quest.quest_extensions.Go2TeleopModule", + "go2-tf-hack-recorder": "dimos.mapping.recording.go2_mid360.record.Go2TfHackRecorder", "google-maps-skill-container": "dimos.agents.skills.google_maps_skill_container.GoogleMapsSkillContainer", "gps-nav-skill-container": "dimos.agents.skills.gps_nav_skill.GpsNavSkillContainer", "grasping-module": "dimos.manipulation.grasping.grasping.GraspingModule", @@ -173,6 +176,7 @@ "joystick-module": "dimos.robot.unitree.b1.joystick_module.JoystickModule", "keyboard-teleop": "dimos.robot.unitree.keyboard_teleop.KeyboardTeleop", "keyboard-teleop-module": "dimos.teleop.keyboard.keyboard_teleop_module.KeyboardTeleopModule", + "keyboard-teleop-tui": "dimos.robot.unitree.keyboard_teleop_tui.KeyboardTeleopTUI", "local-planner": "dimos.navigation.nav_stack.modules.local_planner.local_planner.LocalPlanner", "manipulation-module": "dimos.manipulation.manipulation_module.ManipulationModule", "map": "dimos.robot.unitree.type.map.Map", @@ -220,11 +224,12 @@ "slam-sim-adapter": "dimos.navigation.smart_nav.slam_sim_adapter.SlamSimAdapter", "spatial-memory": "dimos.perception.spatial_perception.SpatialMemory", "speak-skill": "dimos.agents.skills.speak_skill.SpeakSkill", - "speed-warner": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_record.SpeedWarner", + "speed-warner": "dimos.hardware.sensors.lidar.fastlio2.speed_warner.SpeedWarner", "tare-planner": "dimos.navigation.nav_stack.modules.tare_planner.tare_planner.TarePlanner", "temporal-memory": "dimos.perception.experimental.temporal_memory.temporal_memory.TemporalMemory", "terrain-analysis": "dimos.navigation.nav_stack.modules.terrain_analysis.terrain_analysis.TerrainAnalysis", "terrain-map-ext": "dimos.navigation.nav_stack.modules.terrain_map_ext.terrain_map_ext.TerrainMapExt", + "tf-hack-recorder": "dimos.mapping.recording.mid360_realsense.record.TfHackRecorder", "twist-teleop-module": "dimos.teleop.quest.quest_extensions.TwistTeleopModule", "unitree-g1-skill-container": "dimos.robot.unitree.g1.skill_container.UnitreeG1SkillContainer", "unitree-skill-container": "dimos.robot.unitree.unitree_skill_container.UnitreeSkillContainer", From a472d25db2093ae4fb219ff9f79c56d42ce00b0f Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 12 Jun 2026 19:26:50 -0700 Subject: [PATCH 047/185] pointlio: document lidar_type enum + link Livox CustomMsg in default.yaml --- dimos/hardware/sensors/lidar/pointlio/config/default.yaml | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/config/default.yaml b/dimos/hardware/sensors/lidar/pointlio/config/default.yaml index a989359c0c..b3bb054972 100644 --- a/dimos/hardware/sensors/lidar/pointlio/config/default.yaml +++ b/dimos/hardware/sensors/lidar/pointlio/config/default.yaml @@ -9,7 +9,13 @@ common: time_lag_imu_to_lidar: 0.0 preprocess: - lidar_type: 1 # 1 = Livox CustomMsg + # Selects the point-cloud parser. LID_TYPE enum (Point-LIO src/preprocess.h): + # 1 = AVIA (Livox), 2 = VELO16, 3 = OUST64, 4 = HESAIxt32, 5 = UNILIDAR + # 1 picks the Livox branch (preprocess.cpp avia_handler), which expects the + # Livox CustomMsg point layout (per-point offset_time + line/tag) that the + # Mid-360 emits. CustomMsg definition: + # https://github.com/Livox-SDK/livox_ros_driver2/blob/master/msg/CustomMsg.msg + lidar_type: 1 scan_line: 4 scan_rate: 10 timestamp_unit: 3 # 3 = nanosecond From eeb90e84393d4e0b9bcc52f13b064b062baa8cbe Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 12 Jun 2026 19:36:40 -0700 Subject: [PATCH 048/185] fastlio2: pcap_to_db builds a db from scratch (mirror pointlio) --db is now optional: with no existing db, build one from scratch (defaults to .db next to the pcap); with an existing db, append + time-align as before. Mirrors the Point-LIO pcap_to_db cleanup. --- .../lidar/fastlio2/tools/pcap_to_db.py | 35 +++++++++++++++---- 1 file changed, 29 insertions(+), 6 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index a4ec61782a..478085308b 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -12,16 +12,20 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Run FAST-LIO over a .pcap and append its outputs into an existing .db. +"""Run FAST-LIO over a .pcap and write its outputs into a .db. -Given a Livox Mid-360 pcap capture and a memory2 SQLite database, this streams -the pcap through the FastLio2 native module and writes two streams into the -database, both time-aligned onto the db's existing clock: +Given a Livox Mid-360 pcap capture, this streams the pcap through the FastLio2 +native module and writes two streams into a memory2 SQLite database: * ``fastlio_odometry`` -- the IESKF pose at the native odom rate (~30 Hz). * ``fastlio_lidar`` -- the registered (deskewed, odom-frame) point cloud at the native pointcloud rate (~10 Hz). +The ``--db`` is optional. With no existing db the tool builds one **from +scratch** (omit ``--db`` and it defaults to ``.db`` next to the pcap). +With an existing db the two streams are appended and time-aligned onto the db's +clock, so FAST-LIO output can be compared against whatever it already holds. + This mirrors the Point-LIO ``pcap_to_db.py`` tool, with one deliberate difference: FAST-LIO is *not* bit-deterministic (OpenMP reduction order), so the replay runs ``deterministic_clock=False`` -- the feeder paces packets at @@ -50,6 +54,13 @@ Usage (from the dimos5 venv):: source .venv/bin/activate + + # Build a fresh db from scratch (no existing db needed); defaults to + # .db next to the pcap. + python -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db \ + --pcap /path/to/capture.pcap + + # Or append into an existing recording db for comparison. python -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db \ --pcap /path/to/capture.pcap --db /path/to/memory.db """ @@ -195,13 +206,18 @@ def _table_stats(db_path: Path, table: str) -> tuple[int, float, float]: def _run(args: argparse.Namespace) -> int: pcap_path = Path(args.pcap).expanduser().resolve() - db_path = Path(args.db).expanduser().resolve() if not pcap_path.exists(): print(f"[pcap_to_db] missing pcap: {pcap_path}", file=sys.stderr) return 2 if args.max_sensor_sec < 0: print("[pcap_to_db] --max-sensor-sec must be >= 0", file=sys.stderr) return 2 + # --db is optional: with no existing db, build one from scratch. When + # omitted the output defaults to .db next to the pcap, so a fresh + # db can be generated with just --pcap. + db_path = Path(args.db).expanduser().resolve() if args.db else pcap_path.with_suffix(".db") + db_existed = db_path.exists() + db_path.parent.mkdir(parents=True, exist_ok=True) from dimos.core.coordination.blueprints import autoconnect from dimos.core.coordination.module_coordinator import ModuleCoordinator @@ -237,6 +253,7 @@ def _run(args: argparse.Namespace) -> int: offset_desc = f"auto: db wall-clock (R0={ref_start_ts:.2f})" print( f"[pcap_to_db] pcap={pcap_path.name} db={db_path.name} " + f"({'append' if db_existed else 'new'}) " f"odom_freq={args.odom_freq}Hz offset={offset_desc}", flush=True, ) @@ -312,7 +329,13 @@ def _run(args: argparse.Namespace) -> int: def main(argv: list[str]) -> int: parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("--pcap", required=True, help="Livox Mid-360 pcap capture") - parser.add_argument("--db", required=True, help="target memory2 SQLite db (appended to)") + parser.add_argument( + "--db", + default=None, + help="target memory2 SQLite db. If it exists, fastlio streams are appended/aligned " + "onto its clock; if it doesn't, a fresh db is built from scratch. " + "Omit to default to .db next to the pcap.", + ) parser.add_argument( "--odom-freq", type=float, From 3b5f98813a6eb5b217dc8fdc943c7c597cee47c0 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 12 Jun 2026 19:45:13 -0700 Subject: [PATCH 049/185] pointlio: one default.yaml (strip ROS-only keys, note mid360 params); rm cpp/README + config copies; concise comments - config: consolidate to a single config/default.yaml; drop the ROS-only lid_topic/imu_topic + odom frame-id/publish/pcd_save keys (parsed-but-unused by the native binary), and note which params are Mid-360-specific. - delete cpp/config/{mid360.json,mid360.yaml} (duplicate) and cpp/README.md. - tighten verbose comments in main.cpp, pcap_replay.hpp, timing.hpp (comments only; binary rebuilds clean). --- .../lidar/pointlio/config/default.yaml | 37 +-- .../sensors/lidar/pointlio/cpp/README.md | 109 -------- .../lidar/pointlio/cpp/config/mid360.json | 38 --- .../lidar/pointlio/cpp/config/mid360.yaml | 74 ----- .../sensors/lidar/pointlio/cpp/main.cpp | 257 ++++++------------ .../lidar/pointlio/cpp/pcap_replay.hpp | 49 ++-- .../sensors/lidar/pointlio/cpp/timing.hpp | 30 +- 7 files changed, 117 insertions(+), 477 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/pointlio/cpp/README.md delete mode 100644 dimos/hardware/sensors/lidar/pointlio/cpp/config/mid360.json delete mode 100644 dimos/hardware/sensors/lidar/pointlio/cpp/config/mid360.yaml diff --git a/dimos/hardware/sensors/lidar/pointlio/config/default.yaml b/dimos/hardware/sensors/lidar/pointlio/config/default.yaml index b3bb054972..b4b93405d1 100644 --- a/dimos/hardware/sensors/lidar/pointlio/config/default.yaml +++ b/dimos/hardware/sensors/lidar/pointlio/config/default.yaml @@ -1,7 +1,8 @@ -# Non-ROS Point-LIO config (Mid-360). Parsed by parameters.cpp via yaml-cpp. +# Point-LIO config, parsed by parameters.cpp via yaml-cpp. +# Mid-360-specific values: preprocess.lidar_type (Livox), blind/scan_line, +# mapping.extrinsic_T/R (Mid-360 IMU->lidar mount), det_range, fov_degree. +# Retune those for a different sensor; the rest is platform-agnostic. common: - lid_topic: "/livox/lidar" - imu_topic: "/livox/imu" con_frame: false con_frame_num: 1 cut_frame: false @@ -9,11 +10,10 @@ common: time_lag_imu_to_lidar: 0.0 preprocess: - # Selects the point-cloud parser. LID_TYPE enum (Point-LIO src/preprocess.h): + # LID_TYPE enum (Point-LIO src/preprocess.h): # 1 = AVIA (Livox), 2 = VELO16, 3 = OUST64, 4 = HESAIxt32, 5 = UNILIDAR - # 1 picks the Livox branch (preprocess.cpp avia_handler), which expects the - # Livox CustomMsg point layout (per-point offset_time + line/tag) that the - # Mid-360 emits. CustomMsg definition: + # 1 selects the Livox branch (preprocess.cpp avia_handler), which expects the + # Livox CustomMsg point layout the Mid-360 emits: # https://github.com/Livox-SDK/livox_ros_driver2/blob/master/msg/CustomMsg.msg lidar_type: 1 scan_line: 4 @@ -28,14 +28,14 @@ mapping: check_satu: true init_map_size: 10 space_down_sample: true - satu_acc: 3.0 # g (acc_norm=1.0). hku-mars master value: accel >= 3g saturated → EKF residual zeroed, keeps velocity bounded. + satu_acc: 3.0 # g; accel >= this is treated as saturated (residual zeroed) to bound velocity satu_gyro: 35.0 - acc_norm: 1.0 # IMU accel in g + acc_norm: 1.0 # IMU accel unit: g plane_thr: 0.1 filter_size_surf: 0.5 filter_size_map: 0.5 - ivox_grid_resolution: 2.0 # iVox local-map grid (m). Matches hku-mars master avia/horizon Livox configs. - ivox_nearby_type: 6 # NEARBY6. Matches the hku_repro mapping_mid360.launch override (NOT the yaml default) that produced the bounded 5.028m baseline. + ivox_grid_resolution: 2.0 # iVox local-map grid (m) + ivox_nearby_type: 6 # NEARBY6 cube_side_length: 1000.0 det_range: 100.0 fov_degree: 360.0 @@ -57,7 +57,7 @@ mapping: gravity_align: true gravity: [0.0, 0.0, -9.810] gravity_init: [0.0, 0.0, -9.810] - extrinsic_T: [-0.011, -0.02329, 0.04412] + extrinsic_T: [-0.011, -0.02329, 0.04412] # Mid-360 IMU->lidar offset (m) extrinsic_R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] @@ -65,16 +65,3 @@ mapping: odometry: publish_odometry_without_downsample: false odom_only: false - odom_header_frame_id: "camera_init" - odom_child_frame_id: "aft_mapped" - -publish: - path_en: false - scan_publish_en: false - scan_bodyframe_pub_en: false - -runtime_pos_log_enable: false - -pcd_save: - pcd_save_en: false - interval: -1 diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/README.md b/dimos/hardware/sensors/lidar/pointlio/cpp/README.md deleted file mode 100644 index 61ef47384b..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/README.md +++ /dev/null @@ -1,109 +0,0 @@ -# FAST-LIO2 Native Module (C++) - -Real-time LiDAR SLAM using FAST-LIO2 with integrated Livox Mid-360 driver. -Binds Livox SDK2 directly into FAST-LIO-NON-ROS: SDK callbacks feed -CustomMsg/Imu to FastLio, which performs EKF-LOAM SLAM. Registered -(world-frame) point clouds and odometry are published on LCM. - -## Build - -### Nix (recommended) - -```bash -cd dimos/hardware/sensors/lidar/fastlio2/cpp -nix build .#fastlio2_native -``` - -Binary lands at `result/bin/fastlio2_native`. - -The flake pulls Livox SDK2 from the livox sub-flake and -[FAST-LIO-NON-ROS](https://github.com/leshy/FAST-LIO-NON-ROS) from GitHub -automatically. - -### Native (CMake) - -Requires: -- CMake >= 3.14 -- [LCM](https://lcm-proj.github.io/) (`pacman -S lcm` or build from source) -- [Livox SDK2](https://github.com/Livox-SDK/Livox-SDK2) installed to `/usr/local` -- Eigen3, PCL (common, filters), yaml-cpp, Boost, OpenMP -- [FAST-LIO-NON-ROS](https://github.com/leshy/FAST-LIO-NON-ROS) checked out locally - -```bash -cd dimos/hardware/sensors/lidar/fastlio2/cpp -cmake -B build -DFASTLIO_DIR=$HOME/coding/FAST-LIO-NON-ROS -cmake --build build -j$(nproc) -cmake --install build -``` - -Binary lands at `result/bin/fastlio2_native` (same location as nix). - -If `-DFASTLIO_DIR` is omitted, CMake auto-fetches FAST-LIO-NON-ROS from GitHub. - -## Network setup - -The Mid-360 communicates over USB ethernet. Configure the interface: - -```bash -sudo nmcli con add type ethernet ifname usbeth0 con-name livox-mid360 \ - ipv4.addresses 192.168.1.5/24 ipv4.method manual -sudo nmcli con up livox-mid360 -``` - -This persists across reboots. The lidar defaults to `192.168.1.155`. - -## Usage - -Normally launched by `FastLio2` via the NativeModule framework: - -```python -from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 -from dimos.core.coordination.blueprints import autoconnect - -autoconnect( - FastLio2.blueprint(host_ip="192.168.1.5"), - SomeConsumer.blueprint(), -).build().loop() -``` - -### Manual invocation (for debugging) - -```bash -./result/bin/fastlio2_native \ - --lidar '/pointcloud#sensor_msgs.PointCloud2' \ - --odometry '/odometry#nav_msgs.Odometry' \ - --host_ip 192.168.1.5 \ - --lidar_ip 192.168.1.155 \ - --config_path ../config/default.yaml -``` - -Topic strings must include the `#type` suffix -- this is the actual LCM channel -name used by dimos subscribers. - -For full vis: -```sh -rerun-bridge -``` - -For LCM traffic: -```sh -lcm-spy -``` - -## Configuration - -FAST-LIO2 config files live in `config/`. The YAML config controls filter -parameters, EKF tuning, and point cloud processing settings. - -## File overview - -| File | Description | -|---------------------------|--------------------------------------------------------------| -| `main.cpp` | Livox SDK2 + FAST-LIO2 integration, EKF SLAM, LCM publishing | -| `cloud_filter.hpp` | Point cloud filtering (range, voxel downsampling) | -| `voxel_map.hpp` | Global voxel map accumulation | -| `dimos_native_module.hpp` | Reusable header for parsing NativeModule CLI args | -| `config/` | FAST-LIO2 YAML configuration files | -| `flake.nix` | Nix flake for hermetic builds | -| `CMakeLists.txt` | Build config, fetches dimos-lcm headers automatically | -| `../module.py` | Python NativeModule wrapper (`FastLio2`) | diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/config/mid360.json b/dimos/hardware/sensors/lidar/pointlio/cpp/config/mid360.json deleted file mode 100644 index ff6cc6dbf6..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/config/mid360.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "common": { - "time_sync_en": false, - "time_offset_lidar_to_imu": 0.0, - "msr_freq": 50.0, - "main_freq": 5000.0 - }, - "preprocess": { - "lidar_type": 1, - "scan_line": 1, - "blind": 1 - }, - "mapping": { - "acc_cov": 0.1, - "gyr_cov": 0.1, - "b_acc_cov": 0.0001, - "b_gyr_cov": 0.0001, - "fov_degree": 360, - "det_range": 100.0, - "extrinsic_est_en": true, - "extrinsic_T": [ - 0.04165, - 0.02326, - -0.0284 - ], - "extrinsic_R": [ - 1.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 1.0 - ] - } -} diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/config/mid360.yaml b/dimos/hardware/sensors/lidar/pointlio/cpp/config/mid360.yaml deleted file mode 100644 index a989359c0c..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/config/mid360.yaml +++ /dev/null @@ -1,74 +0,0 @@ -# Non-ROS Point-LIO config (Mid-360). Parsed by parameters.cpp via yaml-cpp. -common: - lid_topic: "/livox/lidar" - imu_topic: "/livox/imu" - con_frame: false - con_frame_num: 1 - cut_frame: false - cut_frame_time_interval: 0.1 - time_lag_imu_to_lidar: 0.0 - -preprocess: - lidar_type: 1 # 1 = Livox CustomMsg - scan_line: 4 - scan_rate: 10 - timestamp_unit: 3 # 3 = nanosecond - blind: 0.5 - point_filter_num: 3 - -mapping: - use_imu_as_input: false # false = IMU-as-output model (Point-LIO's robust path) - prop_at_freq_of_imu: true - check_satu: true - init_map_size: 10 - space_down_sample: true - satu_acc: 3.0 # g (acc_norm=1.0). hku-mars master value: accel >= 3g saturated → EKF residual zeroed, keeps velocity bounded. - satu_gyro: 35.0 - acc_norm: 1.0 # IMU accel in g - plane_thr: 0.1 - filter_size_surf: 0.5 - filter_size_map: 0.5 - ivox_grid_resolution: 2.0 # iVox local-map grid (m). Matches hku-mars master avia/horizon Livox configs. - ivox_nearby_type: 6 # NEARBY6. Matches the hku_repro mapping_mid360.launch override (NOT the yaml default) that produced the bounded 5.028m baseline. - cube_side_length: 1000.0 - det_range: 100.0 - fov_degree: 360.0 - imu_en: true - start_in_aggressive_motion: false - extrinsic_est_en: false - imu_time_inte: 0.005 - lidar_meas_cov: 0.01 - acc_cov_input: 0.1 - vel_cov: 20.0 - gyr_cov_input: 0.01 - gyr_cov_output: 1000.0 - acc_cov_output: 500.0 - b_gyr_cov: 0.0001 - b_acc_cov: 0.0001 - imu_meas_acc_cov: 0.01 - imu_meas_omg_cov: 0.01 - match_s: 81.0 - gravity_align: true - gravity: [0.0, 0.0, -9.810] - gravity_init: [0.0, 0.0, -9.810] - extrinsic_T: [-0.011, -0.02329, 0.04412] - extrinsic_R: [1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0] - -odometry: - publish_odometry_without_downsample: false - odom_only: false - odom_header_frame_id: "camera_init" - odom_child_frame_id: "aft_mapped" - -publish: - path_en: false - scan_publish_en: false - scan_bodyframe_pub_en: false - -runtime_pos_log_enable: false - -pcd_save: - pcd_save_en: false - interval: -1 diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 72b59f5682..77d87a04a8 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -62,40 +62,27 @@ static std::atomic g_running{true}; static lcm::LCM* g_lcm = nullptr; static FastLio* g_fastlio = nullptr; -// Virtual clock: in replay mode, tracks the pcap timestamp of the packet -// currently being fed so publish_*() reports the original capture time -// instead of replay wall time. Live mode leaves it at 0 and publish_*() -// falls back to system_clock::now(). +// Replay: virtual clock holds the pcap timestamp of the packet being fed so +// publish_*() reports capture time. Live leaves it 0 → system_clock::now(). static std::atomic g_replay_mode{false}; static std::atomic g_virtual_clock_ns{0}; -// Deterministic clock mode. When set, both live and replay drive -// g_virtual_clock_ns from the packet's sensor-clock pkt->timestamp (which -// is identical bit-for-bit between SDK delivery and pcap), and use it as -// the source for scan-boundary rate limits and publish timestamps. This -// removes wall-clock jitter from scan boundaries → live and replay produce -// the same algorithm state. Trade-off: published header.stamp values -// become sensor-boot-relative seconds instead of unix wall time, so this -// is off by default and only flipped on by the record/replay demos. +// Deterministic mode: drive the virtual clock from pkt->timestamp (identical +// live vs pcap) for rate limits + publish ts, removing wall-clock jitter so +// live and replay produce identical state. Cost: header.stamp becomes +// sensor-boot seconds, not unix time. Off by default; record/replay demos only. static std::atomic g_deterministic_clock{false}; -// First packet's sensor ts (deterministic mode only). Used to seed the -// main loop's rate-limit bookmarks at exactly the first delivered packet, -// independent of when the main loop's first iteration happens to run. +// First packet's sensor ts (deterministic mode only): seeds the main loop's +// rate-limit bookmarks at the first delivered packet regardless of loop timing. static std::atomic g_first_packet_clock_ns{0}; -// First-packet marker. Used by record/replay tooling to align the SDK's -// warmup-induced packet drop with replay. The C++ binary writes the wall -// clock of the first on_point_cloud / on_imu_data callback (live mode -// only) to this file; demo_replay reads it back and passes the value as -// --replay_skip_until_ns so pcap_replay drops the same SDK-eaten prefix. +// First-packet marker: live writes the first callback's pkt->timestamp here; +// demo_replay reads it back as --replay_skip_until_ns to drop the same +// SDK-eaten warmup prefix. pkt->timestamp is bit-identical live vs pcap. static std::string g_first_packet_marker_path; static std::atomic g_first_packet_marker_written{false}; -// The packet's sensor-clock timestamp (pkt->timestamp) is identical bit-for-bit -// between the live SDK delivery path and the recorded pcap, so writing it from -// the first SDK callback gives replay an exact boundary to skip on. Wall clock -// would only let us match within delivery latency (sub-ms). static void mark_first_packet(uint64_t pkt_timestamp_ns) { if (g_first_packet_marker_path.empty()) { return; @@ -119,18 +106,11 @@ static double get_publish_ts() { std::chrono::system_clock::now().time_since_epoch()).count(); } -// Virtualized clock for the main loop's frame/publish rate limiters. In -// replay mode this returns a time_point derived from g_virtual_clock_ns so -// scan boundaries are determined by packet arrival, not by wall-clock thread -// scheduling jitter. Returns nullopt if replay hasn't yet seen a packet -// (caller should skip rate-limit checks in that case). +// Clock for the main loop's rate limiters. In deterministic mode returns a +// time_point from g_virtual_clock_ns (sensor-paced), else wall clock — the +// latter keeps the feeder/main scan-composition race needed to reproduce live +// divergence offline. nullopt = no packet seen yet; skip rate-limit checks. static std::optional virtual_now() { - // Only the deterministic-clock flag drives the virtual (sensor-paced) - // clock. Replay with deterministic_clock=false must use the real wall - // clock so scan boundaries are cut on thread-scheduling timing — that - // restores the live feeder/main scan-composition race that the - // deterministic path deliberately removes (needed to reproduce the live - // divergence offline). if (g_deterministic_clock.load()) { uint64_t ns = g_virtual_clock_ns.load(); if (ns == 0) { @@ -149,17 +129,15 @@ static std::string g_child_frame_id; // required via --child_frame_id static float g_frequency = 10.0f; // Initial pose offset (applied to all SLAM outputs) -// Position offset static double g_init_x = 0.0; static double g_init_y = 0.0; static double g_init_z = 0.0; -// Orientation offset as quaternion (identity = no rotation) static double g_init_qx = 0.0; static double g_init_qy = 0.0; static double g_init_qz = 0.0; static double g_init_qw = 1.0; -// Helper: quaternion multiply (Hamilton product) q_out = q1 * q2 +// Hamilton product: q_out = q1 * q2 static void quat_mul(double ax, double ay, double az, double aw, double bx, double by, double bz, double bw, double& ox, double& oy, double& oz, double& ow) { @@ -169,21 +147,18 @@ static void quat_mul(double ax, double ay, double az, double aw, oz = aw*bz + ax*by - ay*bx + az*bw; } -// Helper: rotate a vector by a quaternion v_out = q * v * q_inv +// Rotate vector by quaternion: v_out = q * v * q_inv static void quat_rotate(double qx, double qy, double qz, double qw, double vx, double vy, double vz, double& ox, double& oy, double& oz) { - // t = 2 * cross(q_xyz, v) double tx = 2.0 * (qy*vz - qz*vy); double ty = 2.0 * (qz*vx - qx*vz); double tz = 2.0 * (qx*vy - qy*vx); - // v_out = v + qw*t + cross(q_xyz, t) ox = vx + qw*tx + (qy*tz - qz*ty); oy = vy + qw*ty + (qz*tx - qx*tz); oz = vz + qw*tz + (qx*ty - qy*tx); } -// Check if initial pose is non-identity static bool has_init_pose() { return g_init_x != 0.0 || g_init_y != 0.0 || g_init_z != 0.0 || g_init_qx != 0.0 || g_init_qy != 0.0 || g_init_qz != 0.0 || g_init_qw != 1.0; @@ -226,7 +201,7 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, pc.is_bigendian = 0; pc.is_dense = 1; - // Fields: x, y, z, intensity (float32 each) + // x, y, z, intensity (float32 each) pc.fields_length = 4; pc.fields.resize(4); @@ -250,12 +225,7 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, pc.data_length = pc.row_step; pc.data.resize(pc.data_length); - // Apply the full init_pose transform (rotation + translation) to point clouds. - // FAST-LIO's map origin is at the sensor's initial position. The rotation - // corrects axis direction (e.g. 180° X for upside-down mount) and the - // translation shifts the origin so that ground sits at z≈0 (e.g. z=1.2 - // for a sensor mounted 1.2m above ground). This matches the odometry - // frame, which also gets the full init_pose applied. + // Apply full init_pose (rotation+translation) to match the odometry frame. const bool apply_init_pose = has_init_pose(); for (int i = 0; i < num_points; ++i) { float* dst = reinterpret_cast(pc.data.data() + i * 16); @@ -289,7 +259,7 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times msg.header = make_header(g_frame_id, timestamp); msg.child_frame_id = g_child_frame_id; - // Pose (apply initial pose offset: p_out = R_init * p_slam + t_init) + // p_out = R_init * p_slam + t_init if (has_init_pose()) { double rx, ry, rz; quat_rotate(g_init_qx, g_init_qy, g_init_qz, g_init_qw, @@ -322,12 +292,11 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times msg.pose.pose.orientation.w = odom.pose.pose.orientation.w; } - // Covariance (fixed-size double[36]) for (int i = 0; i < 36; ++i) { msg.pose.covariance[i] = odom.pose.covariance[i]; } - // Twist (zero — FAST-LIO doesn't output velocity directly) + // Twist zeroed — FAST-LIO doesn't output velocity. msg.twist.twist.linear.x = 0; msg.twist.twist.linear.y = 0; msg.twist.twist.linear.z = 0; @@ -351,11 +320,9 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ uint64_t ts_ns = get_timestamp_ns(data); uint16_t dot_num = data->dot_num; - // Per-point intra-packet time offset, matching livox_ros_driver2 - // (pub_handler.cpp: point_interval = time_interval*100/dot_num ns; - // offset_time = base + i*point_interval). Without this, every point in a - // packet collapses to one timestamp and per-point deskew is lost — fatal - // during aggressive motion. time_interval unit is 0.1us, so *100 → ns. + // Per-point intra-packet offset (matches livox_ros_driver2). Without it all + // points share one timestamp and per-point deskew is lost. time_interval + // unit is 0.1us, so *100 → ns. const uint64_t point_interval_ns = dot_num > 0 ? static_cast(data->time_interval) * 100 / dot_num : 0; @@ -365,12 +332,9 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ std::lock_guard lock(g_pc_mutex); - // Update the deterministic-mode virtual clock INSIDE the accumulator - // mutex so the main loop can never observe a clock advance without - // also seeing the matching points (race that drifts scan composition). - // Monotonic update: SDK threads can deliver packets slightly out of - // sensor-ts order, and we must not let a later store(lower_ts) undo - // a previous store(higher_ts). + // Advance the virtual clock under the accumulator mutex so the main loop + // can't see a clock advance without the matching points. Monotonic CAS: + // out-of-order SDK delivery must not roll the clock back. if (g_deterministic_clock.load()) { uint64_t expected = 0; g_first_packet_clock_ns.compare_exchange_strong(expected, ts_ns); @@ -392,7 +356,7 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ cp.z = static_cast(pts[i].z) / 1000.0; cp.reflectivity = pts[i].reflectivity; cp.tag = pts[i].tag; - cp.line = 0; // Mid-360: non-repetitive, single "line" + cp.line = 0; // Mid-360: single line cp.offset_time = static_cast((ts_ns - g_frame_start_ns) + i * point_interval_ns); g_accumulated_points.push_back(cp); } @@ -419,10 +383,8 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, uint64_t pkt_ts_ns = get_timestamp_ns(data); if (!g_replay_mode.load()) { mark_first_packet(pkt_ts_ns); - // Live IMU-drop instrumentation: a dropped UDP datagram shows up as a - // jump in the sensor timestamp (each pkt is ~5ms apart at 200Hz). Wall - // gaps that exceed sensor gaps mean callback starvation. Confirms (or - // refutes) the dropped-IMU divergence hypothesis on real hardware. + // Live IMU-drop instrumentation: a dropped datagram shows as a sensor-ts + // jump; wall gaps exceeding sensor gaps mean callback starvation. static std::atomic last_pkt_ts_ns{0}; static std::atomic imu_pkt_count{0}; static std::atomic imu_gap_count{0}; @@ -478,10 +440,8 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, for (int j = 0; j < 9; ++j) imu_msg->angular_velocity_covariance[j] = 0.0; - // Point-LIO consumes accel in g (config acc_norm=1.0; the EKF applies - // its own G_m_s2/acc_norm scaling internally). The Livox SDK already - // reports acc in g, so feed it raw — multiplying by GRAVITY_MS2 here - // would double-scale, blow up the gravity estimate, and permanently + // Point-LIO expects accel in g (EKF does its own scaling). SDK already + // reports g, so feed raw — scaling by GRAVITY_MS2 would double-scale and // trip the satu_acc check at rest. imu_msg->linear_acceleration.x = static_cast(imu_pts[i].acc_x); imu_msg->linear_acceleration.y = static_cast(imu_pts[i].acc_y); @@ -492,10 +452,8 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, g_fastlio->feed_imu(imu_msg); } - // Advance the deterministic-mode virtual clock AFTER feed_imu has - // queued the sample, holding g_pc_mutex so this is fully serialized - // with on_point_cloud / the main-loop scan swap. Monotonic CAS so - // out-of-order SDK delivery can't roll the clock backward. + // Advance the virtual clock after feed_imu, under g_pc_mutex so it's + // serialized with on_point_cloud / the scan swap. Monotonic CAS. if (g_deterministic_clock.load()) { std::lock_guard lock(g_pc_mutex); uint64_t expected = 0; @@ -575,8 +533,7 @@ int main(int argc, char** argv) { float map_max_range = mod.arg_float("map_max_range", 100.0f); float map_freq = mod.arg_float("map_freq", 0.0f); - // Verbose logging — propagates to the FAST-LIO C++ core via the - // `fastlio_debug` global. Default false → only real errors print. + // Propagates to the FAST-LIO core via the `fastlio_debug` global. bool debug = mod.arg_bool("debug", false); fastlio_debug = debug; @@ -594,21 +551,17 @@ int main(int argc, char** argv) { ports.host_imu_data = mod.arg_int("host_imu_data_port", port_defaults.host_imu_data); ports.host_log_data = mod.arg_int("host_log_data_port", port_defaults.host_log_data); - // Replay mode (offline). When --replay_pcap is given the SDK is not - // initialized; a feeder thread reads the pcap and calls the existing - // on_point_cloud / on_imu_data callbacks directly. publish_*() uses - // the pcap timestamps as the clock so outputs match the original run. + // Replay: skip SDK init; a feeder thread reads the pcap and calls + // on_point_cloud / on_imu_data directly, using pcap ts as the clock. std::string replay_pcap = mod.arg("replay_pcap", ""); - // Alternative replay source: a flat binary of driver CustomMsg/Imu frames - // dumped from a ROS bag (tools/dump_bag_frames.py). Feeds the port the EXACT - // driver output the ROS reference consumes, bypassing UDP->CustomMsg - // reconstruction — isolates port faithfulness from reconstruction fidelity. + // Alt source: flat binary of driver CustomMsg/Imu frames from a ROS bag + // (tools/dump_bag_frames.py). Bypasses UDP->CustomMsg reconstruction to + // isolate port faithfulness from reconstruction fidelity. std::string replay_bagframes = mod.arg("replay_bagframes", ""); g_replay_mode.store(!replay_pcap.empty() || !replay_bagframes.empty()); - // Drop pcap packets with pcap_ts < this value. Used in replay to mimic - // the SDK warmup discard that the live run experienced — so the - // algorithm starts from the same first packet in both modes. + // Drop pcap packets with pcap_ts < this, mimicking the live SDK warmup + // discard so both modes start from the same first packet. uint64_t replay_skip_until_ns = 0; { std::string s = mod.arg("replay_skip_until_ns", "0"); @@ -617,19 +570,14 @@ int main(int argc, char** argv) { } } - // Live mode: write the wall_clock_ns of the first SDK callback to this - // file. Pair with replay's --replay_skip_until_ns to align packet sets. + // Live: write the first callback's ts here; pairs with replay's + // --replay_skip_until_ns to align packet sets. g_first_packet_marker_path = mod.arg("first_packet_marker", ""); - // Replay: feed point and IMU on two separate threads (mimics the live - // Livox SDK's concurrent delivery threads). Only meaningful with - // deterministic_clock=false. + // Replay: feed point and IMU on two threads (mimics the SDK's concurrent + // delivery). Only meaningful with deterministic_clock=false. const bool replay_dual_thread = mod.arg_bool("replay_dual_thread", false); - // Drive virtual_now() and get_publish_ts() off the packet's sensor - // clock in both live and replay. Eliminates wall-clock jitter from - // scan boundaries and makes outputs bit-comparable across modes. - // Changes header.stamp from unix wall time to sensor-boot seconds. g_deterministic_clock.store(mod.arg_bool("deterministic_clock", false)); // Initial pose offset [x, y, z, qx, qy, qz, qw] @@ -686,16 +634,13 @@ int main(int argc, char** argv) { } g_lcm = &lcm; - // Init FAST-LIO with config if (debug) printf("[fastlio2] Initializing FAST-LIO...\n"); FastLio fast_lio(config_path, msr_freq, main_freq); g_fastlio = &fast_lio; if (debug) printf("[fastlio2] FAST-LIO initialized.\n"); - // Main-loop state. The body lives in `run_main_iter` below so it can be - // invoked from either the wall-clock-paced main thread (live) or the - // pcap-paced feeder thread (replay), with no race on accumulator - // contents in the replay case. + // Main-loop state. Body lives in `run_main_iter` so it can run from either + // the wall-paced main thread (live) or the pcap-paced feeder (replay). auto frame_interval = std::chrono::microseconds( static_cast(1e6 / g_frequency)); std::optional last_emit; @@ -717,11 +662,8 @@ int main(int argc, char** argv) { static_cast(1e6 / map_freq)); } - // Per-section timing counters for `run_main_iter`. Active only when - // --debug is on (Scope's constructor reads `fastlio_debug` and no-ops - // otherwise). `timing::maybe_flush(now)` at the bottom prints a per- - // section summary every second of wall clock so we can see both how - // often each part fires and how long each call takes. + // Per-section timing for `run_main_iter`, active only with --debug. + // maybe_flush() below prints a summary every second. static timing::Section t_iter{"run_main_iter"}; static timing::Section t_emit_check{"emit.lock+swap"}; static timing::Section t_feed_lidar{"fast_lio.feed_lidar"}; @@ -735,13 +677,10 @@ int main(int argc, char** argv) { auto run_main_iter = [&](std::chrono::steady_clock::time_point now) { timing::Scope iter_scope(t_iter); - // Lazy-seed all rate-limit bookmarks on the first iteration so they - // line up with the chosen clock (wall in live, pcap in replay) and - // don't fire immediately based on an arbitrary "since program start" - // delta. In deterministic mode we seed from the FIRST packet's - // sensor ts (not the current ts) so both live and replay anchor - // their first scan boundary at the same packet — required for - // bit-for-bit live↔replay parity. + // Lazy-seed rate-limit bookmarks on the first iteration so they align + // with the chosen clock. In deterministic mode seed from the FIRST + // packet's ts (not now) so live and replay anchor the same scan + // boundary — required for bit-for-bit parity. auto seed = now; if (g_deterministic_clock.load()) { uint64_t first = g_first_packet_clock_ns.load(); @@ -763,12 +702,9 @@ int main(int argc, char** argv) { last_map_publish = seed; } - // At frame rate: drain accumulated raw points into a CustomMsg - // and feed to FAST-LIO. Hold g_pc_mutex across the rate-limit - // CHECK as well as the swap, so in deterministic mode the - // virtual clock + accumulator are observed atomically with no - // other thread able to slip a packet in between the decision - // and the swap. + // At frame rate: drain accumulated points into a CustomMsg and feed + // FAST-LIO. Hold g_pc_mutex across the rate-limit check AND swap so the + // clock + accumulator are observed atomically (no packet slips between). std::vector points; uint64_t frame_start = 0; { @@ -792,7 +728,6 @@ int main(int argc, char** argv) { } } if (!points.empty()) { - // Build CustomMsg auto lidar_msg = boost::make_shared(); lidar_msg->header.seq = 0; lidar_msg->header.stamp = custom_messages::Time().fromSec( @@ -807,14 +742,12 @@ int main(int argc, char** argv) { fast_lio.feed_lidar(lidar_msg); } - // Run one FAST-LIO IESKF step. Cheap when the IMU/lidar queues - // are empty; the heavy work happens after a feed_lidar above. + // One FAST-LIO IESKF step (cheap when queues empty). { timing::Scope s(t_process); fast_lio.process(); } - // Check for new SLAM results and publish (rate-limited). auto pose = fast_lio.get_pose(); if (!pose.empty() && (pose[0] != 0.0 || pose[1] != 0.0 || pose[2] != 0.0)) { double ts = get_publish_ts(); @@ -824,14 +757,9 @@ int main(int argc, char** argv) { const bool map_due = global_map && now - *last_map_publish >= map_interval; - // get_world_cloud() + filter_cloud (SOR, mean_k=50) are by far the - // most expensive step in the loop, so build them ONLY when a lidar - // or map publish is actually due rather than every main_freq - // iteration. (Note: this is a CPU optimization, not the divergence - // fix — the live runaway was a std::deque race in the fastlio core's - // sync_packages/IESKF loop, since fixed by locking mtx_buffer there. - // Dropped IMU datagrams were ruled out: live diverged with zero - // dropped packets.) + // get_world_cloud + filter_cloud (SOR) is the loop's costliest step, + // so build it only when a publish is due. CPU optimization, not the + // divergence fix (that was a deque race in the core, fixed there). if (lidar_due || map_due) { auto world_cloud = ([&]() { timing::Scope s(t_get_world_cloud); @@ -843,15 +771,13 @@ int main(int argc, char** argv) { return filter_cloud(world_cloud, filter_cfg); })(); - // Per-scan world-frame cloud at pointcloud_freq. if (lidar_due) { timing::Scope s(t_publish_lidar); publish_lidar(filtered, ts); last_pc_publish = now; } - // Global voxel map: insert this scan, prune, then publish - // a snapshot at map_freq. + // Global voxel map: insert scan, prune, publish at map_freq. if (global_map) { { timing::Scope s(t_map_insert); @@ -871,7 +797,7 @@ int main(int argc, char** argv) { } } - // Pose + covariance, rate-limited to odom_freq. + // Pose + covariance at odom_freq. if (!g_odometry_topic.empty() && now - *last_odom_publish >= odom_interval) { timing::Scope s(t_publish_odom); publish_odometry(fast_lio.get_odometry(), ts); @@ -879,30 +805,20 @@ int main(int argc, char** argv) { } } - // Periodic per-section summary to stderr (no-op when --debug off). timing::maybe_flush(std::chrono::steady_clock::now()); }; - // Source of point/IMU packets: - // live mode -> Livox SDK opens UDP sockets + dispatches via callbacks - // from its own threads. Main thread drives run_main_iter - // at main_freq, consuming whatever the SDK queued. - // replay mode -> the feeder thread reads the pcap and pushes packets - // through the same on_point/on_imu callbacks (paced at - // realtime via sleep_until). The MAIN thread — same - // one that runs in live mode — owns run_main_iter and - // drains the accumulator. Two threads in both modes, - // matching architectures, so the only difference is - // the source of packets (SDK vs pcap). + // Packet source: live = Livox SDK callbacks from its own threads; replay = + // feeder thread reads pcap through the same callbacks. Either way the main + // thread owns run_main_iter, so the only difference is SDK vs pcap. std::thread replay_thread; if (g_replay_mode.load()) { if (debug) printf("[fastlio2] REPLAY mode, pcap=%s\n", replay_pcap.c_str()); replay_thread = std::thread([&]() { if (!replay_bagframes.empty()) { - // Bag-frame replay: read driver CustomMsg/Imu records from the - // flat dump and feed them straight into the port, serialized - // with the EKF (feed -> run_main_iter) on this one thread. No - // UDP reconstruction, no accumulator. Deterministic by design. + // Bag-frame replay: feed driver records straight into the port, + // serialized with the EKF on this thread. No reconstruction, no + // accumulator — deterministic by design. std::ifstream bf(replay_bagframes, std::ios::binary); if (!bf) { fprintf(stderr, "[bagframes] cannot open %s\n", replay_bagframes.c_str()); @@ -1009,9 +925,8 @@ int main(int argc, char** argv) { on_imu_data(0, 0, p, nullptr); }; rep.on_clock = [](uint64_t pcap_ts_ns) { - // In deterministic mode the callbacks already pushed the - // sensor pkt->timestamp into g_virtual_clock_ns — don't - // overwrite with the pcap (wall) ts. + // Deterministic mode already pushed pkt->timestamp; don't + // overwrite with the pcap ts. if (g_deterministic_clock.load()) { return; } @@ -1019,23 +934,18 @@ int main(int argc, char** argv) { }; rep.running = &g_running; if (g_deterministic_clock.load() && !replay_dual_thread) { - // Deterministic serial replay: the feeder thread drives the - // EKF synchronously after each packet (on_iter) with no - // wall-clock pacing. Feed+process are strictly serialized, so - // the run is reproducible and matches the reference Point-LIO's - // single-executor semantics. The two-thread realtime path - // (else branch) leaves which IMU samples are present at scan - // composition up to wall-clock scheduling — that interleaving - // race makes even clean data diverge nondeterministically. + // Serial replay: feeder drives the EKF synchronously per packet, + // unpaced. Feed+process strictly serialized → reproducible, + // matching Point-LIO's single-executor semantics. The realtime + // path's interleaving race makes even clean data diverge. rep.realtime = false; rep.on_iter = [&]() { auto now_opt = virtual_now(); if (now_opt.has_value()) run_main_iter(*now_opt); }; } else { - // Live-throughput path: feeder paced at wall-clock rate, the - // main thread drives run_main_iter. Used for wall-clock replay - // and dual-thread live-race reproduction. + // Realtime path: feeder paced at wall clock, main thread drives + // run_main_iter. For wall-clock replay and live-race repro. rep.realtime = true; } rep.skip_until_ns = replay_skip_until_ns; @@ -1059,32 +969,27 @@ int main(int argc, char** argv) { if (debug) printf("[fastlio2] SDK started, waiting for device...\n"); } - // Bag-frame replay always drives run_main_iter from the feeder thread - // (step() after each feed), so the main thread must stay out of the EKF - // regardless of the deterministic_clock flag — otherwise both threads + // Bag-frame replay drives run_main_iter from the feeder, so the main thread + // must stay out of the EKF regardless of deterministic_clock — else both // co-drive run_main_iter and race on the shared measurement cloud. const bool serial_replay = g_replay_mode.load() && !replay_dual_thread && (g_deterministic_clock.load() || !replay_bagframes.empty()); while (g_running.load()) { if (serial_replay) { - // The feeder thread's on_iter drives run_main_iter; the main - // thread only services LCM and waits for the feeder to finish. + // Feeder drives run_main_iter; main thread only services LCM. lcm.handleTimeout(10); continue; } auto loop_start = std::chrono::high_resolution_clock::now(); auto now_opt = virtual_now(); if (!now_opt.has_value()) { - // No clock yet — in replay this means the feeder hasn't read - // its first packet; in live it shouldn't happen because - // virtual_now falls back to steady_clock::now(). + // No clock yet (replay feeder hasn't read a packet). std::this_thread::sleep_for(std::chrono::milliseconds(1)); continue; } run_main_iter(*now_opt); - // Drain LCM messages. lcm.handleTimeout(0); // Rate control (~main_freq, 5kHz default). diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/pcap_replay.hpp b/dimos/hardware/sensors/lidar/pointlio/cpp/pcap_replay.hpp index b1e4b4b930..d4dd2d3b73 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/pcap_replay.hpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/pcap_replay.hpp @@ -42,38 +42,27 @@ struct Replayer { PacketCb on_point; PacketCb on_imu; ClockCb on_clock; - // Called synchronously after every packet, once the payload has been - // appended and the virtual clock advanced. The replay path runs the - // main-loop body here so feeding + processing happen on a single - // thread — eliminates the feeder-vs-main-loop race on accumulator - // contents. + // Runs the main-loop body inline after each packet so feed+process stay on + // one thread, avoiding a feeder-vs-main-loop race on accumulator contents. IterCb on_iter; std::atomic* running = nullptr; bool realtime = true; - // Drop Livox packets whose sensor timestamp (pkt->timestamp) is - // strictly less than this. Used to mimic the SDK warmup window from a - // paired live run so the algorithm starts from the same first packet - // in both modes. Comparing on sensor ts (which is identical bit-for-bit - // between live SDK delivery and pcap replay) is exact; comparing on - // wall pcap_ts would be off by SDK delivery latency. + // Drop packets with sensor ts < this to mimic the SDK warmup window from a + // paired live run. Sensor ts is bit-identical live vs replay; wall pcap_ts + // would be off by SDK delivery latency. uint64_t skip_until_ns = 0; - // When true, point and IMU packets are fed from TWO separate threads - // (each paced realtime against a shared wall anchor) instead of one - // serial feeder. This reproduces the live Livox SDK, which delivers - // point and IMU on independent threads — so on_point_cloud and - // on_imu_data actually overlap, exposing concurrency the single-feeder - // path can never hit. Requires deterministic_clock=false (wall clock). + // Feed point/IMU on two threads (vs one serial feeder) to reproduce the + // live SDK's independent delivery threads, exposing point/IMU overlap. + // Requires wall clock (deterministic_clock=false). bool dual_thread = false; - // One parsed Livox UDP payload plus its pcap (wall) and sensor timestamps. struct Pkt { uint64_t pcap_ts_ns = 0; bool is_point = false; std::vector payload; }; - // Parse the whole pcap into point and IMU payload streams (applying the - // sensor-ts skip window). Returns false on a malformed/unsupported file. + // Parse pcap into point/IMU streams, applying skip window. False on bad file. bool prebuffer(std::vector& point_pkts, std::vector& imu_pkts) { std::ifstream f(path, std::ios::binary); if (!f) { @@ -154,8 +143,7 @@ struct Replayer { return true; } - // Pace one pre-buffered stream against a shared wall anchor and dispatch - // each payload to its callback. Runs on its own thread in dual mode. + // Pace one stream against the shared wall anchor; own thread in dual mode. void feed_stream(const std::vector& pkts, const PacketCb& cb, std::chrono::steady_clock::time_point start_wall, uint64_t first_pcap_ts_ns) { @@ -173,9 +161,8 @@ struct Replayer { } } - // Two-thread feeder: reproduces the live SDK's concurrent point/IMU - // delivery. The main loop (run_main_iter) drains the accumulator as in - // live; no on_clock/on_iter (wall-clock mode only). + // Two-thread feeder; main loop drains accumulator as in live. Wall-clock + // mode only (no on_clock/on_iter). bool run_dual() { std::vector point_pkts, imu_pkts; if (!prebuffer(point_pkts, imu_pkts)) return false; @@ -303,9 +290,7 @@ struct Replayer { auto* livox_pkt = reinterpret_cast(buf.data() + payload_off); - // Sensor-clock skip: drop packets the live SDK wouldn't have - // seen (those before its first delivered callback) so the - // algorithm processes the same input set in both modes. + // Skip packets the live SDK wouldn't have delivered yet. if (skip_until_ns > 0) { uint64_t pkt_ts; std::memcpy(&pkt_ts, livox_pkt->timestamp, sizeof(uint64_t)); @@ -341,15 +326,13 @@ struct Replayer { } else { other++; } - // Advance the virtual clock AFTER the payload has been added to - // accumulators. Reverse order would let the main-loop thread see - // the clock advance and emit a scan that's missing this packet. + // Advance clock only after the payload is accumulated, else a scan + // could be emitted missing this packet. if (on_clock) { on_clock(pcap_ts_ns); } - // Run one main-loop iteration synchronously so feeding and - // processing are strictly serialized in replay mode. + // Serialize feed and process in replay mode. if (on_iter) { on_iter(); } diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp b/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp index d1fbe8ded4..8b2f2dc442 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp @@ -1,24 +1,14 @@ // Copyright 2026 Dimensional Inc. // SPDX-License-Identifier: Apache-2.0 // -// Lightweight per-section timing for diagnosing where wall time goes in -// `run_main_iter`. Active only when --debug is on (i.e. the global -// `fastlio_debug` flag is true) so non-debug runs pay only a single -// branch per scope. +// Lightweight per-section timing for `run_main_iter`. Active only when the +// global `fastlio_debug` flag is set, so non-debug runs pay one branch per +// scope. // // Usage: -// // static timing::Section sec{"filter_cloud"}; -// { -// timing::Scope s(sec); -// // ...do work... -// } -// // and periodically: -// timing::maybe_flush(now); -// -// The flush prints one line per section to stderr every flush interval -// (1 second of wall clock) summarising count / mean / max / total, then -// resets the accumulators. The flush is cheap when nothing was recorded. +// { timing::Scope s(sec); /* work */ } +// timing::maybe_flush(now); // periodically #pragma once @@ -29,7 +19,7 @@ #include #include -#include "fast_lio_debug.hpp" // for the global `fastlio_debug` flag +#include "fast_lio_debug.hpp" namespace timing { @@ -47,7 +37,6 @@ struct Section { uint64_t prev = max_ns.load(std::memory_order_relaxed); while (ns > prev && !max_ns.compare_exchange_weak(prev, ns, std::memory_order_relaxed)) { - // prev is updated on failure by compare_exchange_weak. } } }; @@ -82,11 +71,8 @@ struct Scope { } }; -// Print one summary line per section to stderr every FLUSH_INTERVAL wall -// seconds, then reset accumulators. The check is cheap: a single time -// comparison guarded by the fastlio_debug flag. The mutex serialises the -// flush between threads (replay's feeder vs live's main loop) so we -// never see torn output. +// Print one line per section to stderr every FLUSH_INTERVAL, then reset. +// Mutex serialises flushes across threads (replay feeder vs live main loop). inline void maybe_flush(std::chrono::steady_clock::time_point now) { if (!fastlio_debug) { return; From c53c73abb8bb9b0b24a60b0af088d6c4ea9854ae Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 12 Jun 2026 19:50:37 -0700 Subject: [PATCH 050/185] pointlio: add ruwik2_part3 divergence sample to LFS; fix from-scratch db name in docstring MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 120s clip (elapsed 55-175s) of the ruwik velocity-spike recording — the data that diverges through FAST-LIO at the 0.5m pre-KF voxel and is bounded under Point-LIO. Use via get_data('ruwik2_part3'); pcap_to_db --pcap builds the db from scratch. --- data/.lfs/ruwik2_part3.tar.gz | 3 +++ dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) create mode 100644 data/.lfs/ruwik2_part3.tar.gz diff --git a/data/.lfs/ruwik2_part3.tar.gz b/data/.lfs/ruwik2_part3.tar.gz new file mode 100644 index 0000000000..0820c3e481 --- /dev/null +++ b/data/.lfs/ruwik2_part3.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:574d14edc55e015b57248db93a91009d319e2498d9aaff5b2538657c276d5318 +size 135443751 diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py index dab9f64e66..56deacbaac 100644 --- a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py @@ -55,7 +55,7 @@ PCAP=$(python -c "from dimos.utils.data import get_data; \ print(get_data('ruwik2_part3') / 'ruwik2_part3.pcap')") python -m dimos.hardware.sensors.lidar.pointlio.tools.pcap_to_db --pcap "$PCAP" - # -> writes ruwik2_part3.pcap.db next to the sample with pointlio_odometry + # -> writes ruwik2_part3.db next to the sample with pointlio_odometry # + pointlio_lidar. # Or append into an existing recording db for comparison: From d48a945f88a30860b2c675cf962dea1fb790207c Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 12 Jun 2026 19:54:15 -0700 Subject: [PATCH 051/185] pointlio: pcap_to_db docstring uses nested get_data for the ruwik2_part3 sample --- dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py index 56deacbaac..66e6144c61 100644 --- a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py @@ -51,9 +51,9 @@ source .venv/bin/activate # Build a fresh db from scratch (no existing db needed). The ruwik2_part3 - # sample pcap is available via LFS: + # sample pcap (120s, includes the velocity-spike segment) is in LFS: PCAP=$(python -c "from dimos.utils.data import get_data; \ - print(get_data('ruwik2_part3') / 'ruwik2_part3.pcap')") + print(get_data('ruwik2_part3/ruwik2_part3.pcap'))") python -m dimos.hardware.sensors.lidar.pointlio.tools.pcap_to_db --pcap "$PCAP" # -> writes ruwik2_part3.db next to the sample with pointlio_odometry # + pointlio_lidar. From c0627272079b8b237e301a4952ede8e30725c929 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 12 Jun 2026 21:49:41 -0700 Subject: [PATCH 052/185] =?UTF-8?q?pointlio:=20address=20greptile=20review?= =?UTF-8?q?=20=E2=80=94=20fix=20stale=20fastlio=20docstring;=20ts=3D0.0=20?= =?UTF-8?q?clock=20bug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - module.py docstring: Point-LIO (not FAST-LIO2) + correct import path. - pcap_to_db.py: use 'is not None' (not 'or') for the ts fallback so a real sensor ts of 0.0 isn't replaced by wall time (which would misclassify the stream clock in _resolve_offset). --- dimos/hardware/sensors/lidar/pointlio/module.py | 6 +++--- dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py | 8 ++++++-- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index 9faa44d474..59d6e897ae 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -12,14 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Python NativeModule wrapper for the FAST-LIO2 + Livox Mid-360 binary. +"""Python NativeModule wrapper for the Point-LIO + Livox Mid-360 binary. -Binds Livox SDK2 directly into FAST-LIO-NON-ROS for real-time LiDAR SLAM. +Binds Livox SDK2 directly into Point-LIO for real-time LiDAR SLAM. Outputs registered (world-frame) point clouds and odometry with covariance. Usage:: - from dimos.hardware.sensors.lidar.fastlio2.module import PointLio + from dimos.hardware.sensors.lidar.pointlio.module import PointLio from dimos.core.coordination.blueprints import autoconnect from dimos.core.coordination.module_coordinator import ModuleCoordinator diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py index 66e6144c61..d81ddab5d2 100644 --- a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py @@ -141,7 +141,10 @@ def _aligned_ts(self, raw_ts: float, last_ts: float) -> float: return max(raw_ts + self._offset, last_ts + _EPS) async def handle_pointlio_odometry(self, v: Odometry) -> None: - raw_ts = getattr(v, "ts", None) or time.time() + # `is not None`, not `or`: a real sensor ts of 0.0 must not fall back to + # wall time (would misclassify the stream's clock in _resolve_offset). + raw_ts_raw = getattr(v, "ts", None) + raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() ts = self._aligned_ts(raw_ts, self._last_odom_ts) self._last_odom_ts = ts pose = getattr(v, "pose", None) @@ -150,7 +153,8 @@ async def handle_pointlio_odometry(self, v: Odometry) -> None: self._odom_count += 1 async def handle_pointlio_lidar(self, v: PointCloud2) -> None: - raw_ts = getattr(v, "ts", None) or time.time() + raw_ts_raw = getattr(v, "ts", None) + raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() ts = self._aligned_ts(raw_ts, self._last_lidar_ts) self._last_lidar_ts = ts self._ls.append(v, ts=ts) From 00b07200d78b71845028047f84dfb157f0329ddf Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 12 Jun 2026 22:27:18 -0700 Subject: [PATCH 053/185] fastlio2: set mid360 acc_cov=0.5 to bound velocity divergence MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The voxel-0.1 change (#2466) does not bound the velocity-spike divergence on real raw-pcap data — that finding was a re-encoding artifact. acc_cov is the actual lever: the ruwik2_pt3 raw pcap has a sharp threshold between 0.3 (diverges) and 0.35 (bounded); 0.5 holds with a safety margin (reliably bounded across reps, peak ~5 m/s). See jhist dimos-fastlio-velocity-spike.md. --- .../sensors/lidar/fastlio2/config/mid360.yaml | 20 +++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml b/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml index b710afd29e..b3337d9ed3 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml +++ b/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml @@ -11,15 +11,23 @@ preprocess: blind: 0.5 mapping: - acc_cov: 0.1 + # acc_cov is the divergence guard. At the upstream default (0.1) FAST-LIO + # odometry runs away to km/s on aggressive Go2 motion — the IMU acceleration + # prediction dominates the IESKF and diverges. Raising acc_cov down-weights + # that prediction. On the ruwik2_pt3 raw pcap the threshold is sharp between + # 0.3 (diverges) and 0.35 (bounded); 0.5 holds with margin (reliably bounded + # across reps, peak ~5 m/s, matching Point-LIO). See jhist + # dimos-fastlio-velocity-spike.md (2026-06-13 correction). + acc_cov: 0.5 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 - # Scan downsampling that feeds the IESKF. The upstream default (0.5 m) is too - # sparse on aggressive Go2 motion: the point-to-plane update becomes - # ill-conditioned and odometry velocity diverges to km/s. Replaying ruwik2_pt3 - # shows a sharp threshold between 0.38 m (diverges) and 0.35 m (bounded); 0.1 m - # stays bounded with margin. See jhist dimos-fastlio-velocity-spike.md (2026-06-10). + # Scan voxel that feeds the IESKF. NOTE: an earlier finding that voxel 0.1 m + # bounds the velocity divergence turned out to be a re-encoding artifact + # (measured on a bag with synthesized per-point timestamps). On the raw pcap, + # voxel size does NOT bound divergence at any value — acc_cov above is the + # real guard. 0.1 m retained only for scan density, not for stability. + # See jhist dimos-fastlio-velocity-spike.md (2026-06-13 correction). filter_size_surf: 0.1 filter_size_map: 0.1 fov_degree: 360 From 22d9ec0e5ed3a3af938a523fb2a7cacf39cdab99 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sat, 13 Jun 2026 07:02:13 -0700 Subject: [PATCH 054/185] =?UTF-8?q?feat(virtual=5Fmid360):=20Rust=20native?= =?UTF-8?q?=20module=20=E2=80=94=20pcap=20replay=20over=20a=20virtual=20NI?= =?UTF-8?q?C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Reads a Livox Mid-360 pcap into RAM, rewrites packet timestamps to current-time, and replays point/IMU/status onto a virtual network interface at a configurable rate + delay. Synthesizes the Livox SDK2 control protocol (discovery + GetInternalInfo/FwType ACKs, CRC16/CRC32) so an unmodified consumer (pointlio) handshakes with it as a real sensor. Builds via nix (rustPlatform.buildRustPackage, cargoLock from Cargo.lock). --- .../lidar/livox/virtual_mid360/Cargo.lock | 900 ++++++++++++++++++ .../lidar/livox/virtual_mid360/Cargo.toml | 18 + .../lidar/livox/virtual_mid360/flake.lock | 78 ++ .../lidar/livox/virtual_mid360/flake.nix | 50 + .../lidar/livox/virtual_mid360/src/main.rs | 511 ++++++++++ 5 files changed, 1557 insertions(+) create mode 100644 dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.lock create mode 100644 dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml create mode 100644 dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.lock create mode 100644 dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.nix create mode 100644 dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.lock b/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.lock new file mode 100644 index 0000000000..f9b68b4d57 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.lock @@ -0,0 +1,900 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 4 + +[[package]] +name = "aho-corasick" +version = "1.1.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ddd31a130427c27518df266943a5308ed92d4b226cc639f5a8f1002816174301" +dependencies = [ + "memchr", +] + +[[package]] +name = "byteorder" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" + +[[package]] +name = "bytes" +version = "1.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1e748733b7cbc798e1434b6ac524f0c1ff2ab456fe201501e6497c8417a4fc33" + +[[package]] +name = "cfg-if" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9330f8b2ff13f34540b44e946ef35111825727b38d33286ef986142615121801" + +[[package]] +name = "darling" +version = "0.20.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fc7f46116c46ff9ab3eb1597a45688b6715c6e628b5c133e288e709a29bcb4ee" +dependencies = [ + "darling_core", + "darling_macro", +] + +[[package]] +name = "darling_core" +version = "0.20.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0d00b9596d185e565c2207a0b01f8bd1a135483d02d9b7b0a54b11da8d53412e" +dependencies = [ + "fnv", + "ident_case", + "proc-macro2", + "quote", + "strsim", + "syn", +] + +[[package]] +name = "darling_macro" +version = "0.20.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fc34b93ccb385b40dc71c6fceac4b2ad23662c7eeb248cf10d529b7e055b6ead" +dependencies = [ + "darling_core", + "quote", + "syn", +] + +[[package]] +name = "dimos-lcm" +version = "0.1.0" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#e7c9428b7201cdfeadecd181c77c9e2d60a14503" +dependencies = [ + "byteorder", + "socket2 0.5.10", + "tokio", +] + +[[package]] +name = "dimos-module" +version = "0.1.0" +dependencies = [ + "dimos-lcm", + "dimos-module-macros", + "serde", + "serde_json", + "tokio", + "tracing", + "tracing-subscriber", + "validator", +] + +[[package]] +name = "dimos-module-macros" +version = "0.1.0" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "displaydoc" +version = "0.2.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1ac70aa55017e108007fbaf5aa0f54b021c98f92ff8af59d42eda9da96e3dd4f" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "errno" +version = "0.3.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "39cab71617ae0d63f51a36d69f866391735b51691dbda63cf6f96d042b63efeb" +dependencies = [ + "libc", + "windows-sys 0.61.2", +] + +[[package]] +name = "fnv" +version = "1.0.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" + +[[package]] +name = "form_urlencoded" +version = "1.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cb4cb245038516f5f85277875cdaa4f7d2c9a0fa0468de06ed190163b1581fcf" +dependencies = [ + "percent-encoding", +] + +[[package]] +name = "icu_collections" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2984d1cd16c883d7935b9e07e44071dca8d917fd52ecc02c04d5fa0b5a3f191c" +dependencies = [ + "displaydoc", + "potential_utf", + "utf8_iter", + "yoke", + "zerofrom", + "zerovec", +] + +[[package]] +name = "icu_locale_core" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "92219b62b3e2b4d88ac5119f8904c10f8f61bf7e95b640d25ba3075e6cac2c29" +dependencies = [ + "displaydoc", + "litemap", + "tinystr", + "writeable", + "zerovec", +] + +[[package]] +name = "icu_normalizer" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c56e5ee99d6e3d33bd91c5d85458b6005a22140021cc324cea84dd0e72cff3b4" +dependencies = [ + "icu_collections", + "icu_normalizer_data", + "icu_properties", + "icu_provider", + "smallvec", + "zerovec", +] + +[[package]] +name = "icu_normalizer_data" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "da3be0ae77ea334f4da67c12f149704f19f81d1adf7c51cf482943e84a2bad38" + +[[package]] +name = "icu_properties" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bee3b67d0ea5c2cca5003417989af8996f8604e34fb9ddf96208a033901e70de" +dependencies = [ + "icu_collections", + "icu_locale_core", + "icu_properties_data", + "icu_provider", + "zerotrie", + "zerovec", +] + +[[package]] +name = "icu_properties_data" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e2bbb201e0c04f7b4b3e14382af113e17ba4f63e2c9d2ee626b720cbce54a14" + +[[package]] +name = "icu_provider" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "139c4cf31c8b5f33d7e199446eff9c1e02decfc2f0eec2c8d71f65befa45b421" +dependencies = [ + "displaydoc", + "icu_locale_core", + "writeable", + "yoke", + "zerofrom", + "zerotrie", + "zerovec", +] + +[[package]] +name = "ident_case" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39" + +[[package]] +name = "idna" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3b0875f23caa03898994f6ddc501886a45c7d3d62d04d2d90788d47be1b1e4de" +dependencies = [ + "idna_adapter", + "smallvec", + "utf8_iter", +] + +[[package]] +name = "idna_adapter" +version = "1.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cb68373c0d6620ef8105e855e7745e18b0d00d3bdb07fb532e434244cdb9a714" +dependencies = [ + "icu_normalizer", + "icu_properties", +] + +[[package]] +name = "itoa" +version = "1.0.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8f42a60cbdf9a97f5d2305f08a87dc4e09308d1276d28c869c684d7777685682" + +[[package]] +name = "lazy_static" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" + +[[package]] +name = "libc" +version = "0.2.186" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "68ab91017fe16c622486840e4c83c9a37afeff978bd239b5293d61ece587de66" + +[[package]] +name = "litemap" +version = "0.8.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "92daf443525c4cce67b150400bc2316076100ce0b3686209eb8cf3c31612e6f0" + +[[package]] +name = "log" +version = "0.4.32" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "953f07c43838f8e6f9758cab68bf5bed85465e7587ebe0b823f1bcd81978ad3a" + +[[package]] +name = "matchers" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d1525a2a28c7f4fa0fc98bb91ae755d1e2d1505079e05539e35bc876b5d65ae9" +dependencies = [ + "regex-automata", +] + +[[package]] +name = "memchr" +version = "2.8.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "88904434abc2901f197fe8cc55f0445e7ded921dba5911dad2e2b39b48e663c4" + +[[package]] +name = "mio" +version = "1.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "02bd0af71c67b473010cbbc60715ee815645a4dc942899111f494b4b737d6fda" +dependencies = [ + "libc", + "wasi", + "windows-sys 0.61.2", +] + +[[package]] +name = "nu-ansi-term" +version = "0.50.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7957b9740744892f114936ab4a57b3f487491bbeafaf8083688b16841a4240e5" +dependencies = [ + "windows-sys 0.61.2", +] + +[[package]] +name = "once_cell" +version = "1.21.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9f7c3e4beb33f85d45ae3e3a1792185706c8e16d043238c593331cc7cd313b50" + +[[package]] +name = "percent-encoding" +version = "2.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9b4f627cb1b25917193a259e49bdad08f671f8d9708acfd5fe0a8c1455d87220" + +[[package]] +name = "pin-project-lite" +version = "0.2.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a89322df9ebe1c1578d689c92318e070967d1042b512afbe49518723f4e6d5cd" + +[[package]] +name = "potential_utf" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0103b1cef7ec0cf76490e969665504990193874ea05c85ff9bab8b911d0a0564" +dependencies = [ + "zerovec", +] + +[[package]] +name = "proc-macro-error-attr2" +version = "2.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "96de42df36bb9bba5542fe9f1a054b8cc87e172759a1868aa05c1f3acc89dfc5" +dependencies = [ + "proc-macro2", + "quote", +] + +[[package]] +name = "proc-macro-error2" +version = "2.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11ec05c52be0a07b08061f7dd003e7d7092e0472bc731b4af7bb1ef876109802" +dependencies = [ + "proc-macro-error-attr2", + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "proc-macro2" +version = "1.0.106" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8fd00f0bb2e90d81d1044c2b32617f68fcb9fa3bb7640c23e9c748e53fb30934" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "quote" +version = "1.0.45" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "41f2619966050689382d2b44f664f4bc593e129785a36d6ee376ddf37259b924" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "regex" +version = "1.12.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f1292b7759ae1cb9ec195452d1390a074f0cd8541ab7a5a8c31cd6db45d4a6ba" +dependencies = [ + "aho-corasick", + "memchr", + "regex-automata", + "regex-syntax", +] + +[[package]] +name = "regex-automata" +version = "0.4.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6e1dd4122fc1595e8162618945476892eefca7b88c52820e74af6262213cae8f" +dependencies = [ + "aho-corasick", + "memchr", + "regex-syntax", +] + +[[package]] +name = "regex-syntax" +version = "0.8.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d6f6ff9a378485b298a5286656da665ba74413d36db0979633275d2e708145d4" + +[[package]] +name = "serde" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a8e94ea7f378bd32cbbd37198a4a91436180c5bb472411e48b5ec2e2124ae9e" +dependencies = [ + "serde_core", + "serde_derive", +] + +[[package]] +name = "serde_core" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "41d385c7d4ca58e59fc732af25c3983b67ac852c1a25000afe1175de458b67ad" +dependencies = [ + "serde_derive", +] + +[[package]] +name = "serde_derive" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d540f220d3187173da220f885ab66608367b6574e925011a9353e4badda91d79" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "serde_json" +version = "1.0.150" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e8014e44b4736ed0538adeecded0fce2a272f22dc9578a7eb6b2d9993c74cfb9" +dependencies = [ + "itoa", + "memchr", + "serde", + "serde_core", + "zmij", +] + +[[package]] +name = "sharded-slab" +version = "0.1.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f40ca3c46823713e0d4209592e8d6e826aa57e928f09752619fc696c499637f6" +dependencies = [ + "lazy_static", +] + +[[package]] +name = "signal-hook-registry" +version = "1.4.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c4db69cba1110affc0e9f7bcd48bbf87b3f4fc7c61fc9155afd4c469eb3d6c1b" +dependencies = [ + "errno", + "libc", +] + +[[package]] +name = "smallvec" +version = "1.15.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8ed6a63f02c8539c91a8685a86f4099661ba3da017932f6ebbea6de3f0fa7c90" + +[[package]] +name = "socket2" +version = "0.5.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e22376abed350d73dd1cd119b57ffccad95b4e585a7cda43e286245ce23c0678" +dependencies = [ + "libc", + "windows-sys 0.52.0", +] + +[[package]] +name = "socket2" +version = "0.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "52d1cfed4120b4d927bf7c0f86d2087a4a7d6027c906d9f9d525a80573b9be51" +dependencies = [ + "libc", + "windows-sys 0.61.2", +] + +[[package]] +name = "stable_deref_trait" +version = "1.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6ce2be8dc25455e1f91df71bfa12ad37d7af1092ae736f3a6cd0e37bc7810596" + +[[package]] +name = "strsim" +version = "0.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" + +[[package]] +name = "syn" +version = "2.0.117" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e665b8803e7b1d2a727f4023456bbbbe74da67099c585258af0ad9c5013b9b99" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "synstructure" +version = "0.13.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "728a70f3dbaf5bab7f0c4b1ac8d7ae5ea60a4b5549c8a5914361c99147a709d2" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "thread_local" +version = "1.1.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f60246a4944f24f6e018aa17cdeffb7818b76356965d03b07d6a9886e8962185" +dependencies = [ + "cfg-if", +] + +[[package]] +name = "tinystr" +version = "0.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c8323304221c2a851516f22236c5722a72eaa19749016521d6dff0824447d96d" +dependencies = [ + "displaydoc", + "zerovec", +] + +[[package]] +name = "tokio" +version = "1.52.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8fc7f01b389ac15039e4dc9531aa973a135d7a4135281b12d7c1bc79fd57fffe" +dependencies = [ + "bytes", + "libc", + "mio", + "pin-project-lite", + "signal-hook-registry", + "socket2 0.6.4", + "tokio-macros", + "windows-sys 0.61.2", +] + +[[package]] +name = "tokio-macros" +version = "2.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "385a6cb71ab9ab790c5fe8d67f1645e6c450a7ce006a33de03daa956cf70a496" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "tracing" +version = "0.1.44" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "63e71662fa4b2a2c3a26f570f037eb95bb1f85397f3cd8076caed2f026a6d100" +dependencies = [ + "pin-project-lite", + "tracing-attributes", + "tracing-core", +] + +[[package]] +name = "tracing-attributes" +version = "0.1.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7490cfa5ec963746568740651ac6781f701c9c5ea257c58e057f3ba8cf69e8da" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "tracing-core" +version = "0.1.36" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "db97caf9d906fbde555dd62fa95ddba9eecfd14cb388e4f491a66d74cd5fb79a" +dependencies = [ + "once_cell", + "valuable", +] + +[[package]] +name = "tracing-log" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ee855f1f400bd0e5c02d150ae5de3840039a3f54b025156404e34c23c03f47c3" +dependencies = [ + "log", + "once_cell", + "tracing-core", +] + +[[package]] +name = "tracing-serde" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "704b1aeb7be0d0a84fc9828cae51dab5970fee5088f83d1dd7ee6f6246fc6ff1" +dependencies = [ + "serde", + "tracing-core", +] + +[[package]] +name = "tracing-subscriber" +version = "0.3.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cb7f578e5945fb242538965c2d0b04418d38ec25c79d160cd279bf0731c8d319" +dependencies = [ + "matchers", + "nu-ansi-term", + "once_cell", + "regex-automata", + "serde", + "serde_json", + "sharded-slab", + "smallvec", + "thread_local", + "tracing", + "tracing-core", + "tracing-log", + "tracing-serde", +] + +[[package]] +name = "unicode-ident" +version = "1.0.24" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e6e4313cd5fcd3dad5cafa179702e2b244f760991f45397d14d4ebf38247da75" + +[[package]] +name = "url" +version = "2.5.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ff67a8a4397373c3ef660812acab3268222035010ab8680ec4215f38ba3d0eed" +dependencies = [ + "form_urlencoded", + "idna", + "percent-encoding", + "serde", +] + +[[package]] +name = "utf8_iter" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b6c140620e7ffbb22c2dee59cafe6084a59b5ffc27a8859a5f0d494b5d52b6be" + +[[package]] +name = "validator" +version = "0.20.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "43fb22e1a008ece370ce08a3e9e4447a910e92621bb49b85d6e48a45397e7cfa" +dependencies = [ + "idna", + "once_cell", + "regex", + "serde", + "serde_derive", + "serde_json", + "url", + "validator_derive", +] + +[[package]] +name = "validator_derive" +version = "0.20.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b7df16e474ef958526d1205f6dda359fdfab79d9aa6d54bafcb92dcd07673dca" +dependencies = [ + "darling", + "once_cell", + "proc-macro-error2", + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "valuable" +version = "0.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ba73ea9cf16a25df0c8caa16c51acb937d5712a8429db78a3ee29d5dcacd3a65" + +[[package]] +name = "virtual-mid360" +version = "0.1.0" +dependencies = [ + "dimos-module", + "serde", + "tokio", + "tracing", + "validator", +] + +[[package]] +name = "wasi" +version = "0.11.1+wasi-snapshot-preview1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ccf3ec651a847eb01de73ccad15eb7d99f80485de043efb2f370cd654f4ea44b" + +[[package]] +name = "windows-link" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f0805222e57f7521d6a62e36fa9163bc891acd422f971defe97d64e70d0a4fe5" + +[[package]] +name = "windows-sys" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "282be5f36a8ce781fad8c8ae18fa3f9beff57ec1b52cb3de0789201425d9a33d" +dependencies = [ + "windows-targets", +] + +[[package]] +name = "windows-sys" +version = "0.61.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ae137229bcbd6cdf0f7b80a31df61766145077ddf49416a728b02cb3921ff3fc" +dependencies = [ + "windows-link", +] + +[[package]] +name = "windows-targets" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973" +dependencies = [ + "windows_aarch64_gnullvm", + "windows_aarch64_msvc", + "windows_i686_gnu", + "windows_i686_gnullvm", + "windows_i686_msvc", + "windows_x86_64_gnu", + "windows_x86_64_gnullvm", + "windows_x86_64_msvc", +] + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469" + +[[package]] +name = "windows_i686_gnu" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b" + +[[package]] +name = "windows_i686_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66" + +[[package]] +name = "windows_i686_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" + +[[package]] +name = "writeable" +version = "0.6.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1ffae5123b2d3fc086436f8834ae3ab053a283cfac8fe0a0b8eaae044768a4c4" + +[[package]] +name = "yoke" +version = "0.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "709fe23a0424b6a435d82152b1bd3fdfb0833487d5fa90d05d42762a9891fef5" +dependencies = [ + "stable_deref_trait", + "yoke-derive", + "zerofrom", +] + +[[package]] +name = "yoke-derive" +version = "0.8.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "de844c262c8848816172cef550288e7dc6c7b7814b4ee56b3e1553f275f1858e" +dependencies = [ + "proc-macro2", + "quote", + "syn", + "synstructure", +] + +[[package]] +name = "zerofrom" +version = "0.1.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ec05a11813ea801ff6d75110ad09cd0824ddba17dfe17128ea0d5f68e6c5272" +dependencies = [ + "zerofrom-derive", +] + +[[package]] +name = "zerofrom-derive" +version = "0.1.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11532158c46691caf0f2593ea8358fed6bbf68a0315e80aae9bd41fbade684a1" +dependencies = [ + "proc-macro2", + "quote", + "syn", + "synstructure", +] + +[[package]] +name = "zerotrie" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0f9152d31db0792fa83f70fb2f83148effb5c1f5b8c7686c3459e361d9bc20bf" +dependencies = [ + "displaydoc", + "yoke", + "zerofrom", +] + +[[package]] +name = "zerovec" +version = "0.11.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "90f911cbc359ab6af17377d242225f4d75119aec87ea711a880987b18cd7b239" +dependencies = [ + "yoke", + "zerofrom", + "zerovec-derive", +] + +[[package]] +name = "zerovec-derive" +version = "0.11.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "625dc425cab0dca6dc3c3319506e6593dcb08a9f387ea3b284dbd52a92c40555" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "zmij" +version = "1.0.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b8848ee67ecc8aedbaf3e4122217aff892639231befc6a1b58d29fff4c2cabaa" diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml b/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml new file mode 100644 index 0000000000..c01c38cdae --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml @@ -0,0 +1,18 @@ +[package] +name = "virtual-mid360" +version = "0.1.0" +edition = "2021" + +# Fake Livox Mid-360: replays a recorded pcap over a virtual NIC and synthesizes +# the Livox SDK2 control handshake so an unmodified live-mode pointlio ingests it +# as if from a real sensor. Inverse of pointlio's in-process --replay_pcap. +[[bin]] +name = "virtual_mid360" +path = "src/main.rs" + +[dependencies] +dimos-module = { path = "../../../../../../native/rust/dimos-module" } +tokio = { version = "1", features = ["rt-multi-thread", "macros", "time", "net", "sync"] } +serde = { version = "1", features = ["derive"] } +tracing = "0.1" +validator = { version = "0.20", features = ["derive"] } diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.lock b/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.lock new file mode 100644 index 0000000000..a548660557 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.lock @@ -0,0 +1,78 @@ +{ + "nodes": { + "dimos-repo": { + "flake": false, + "locked": { + "lastModified": 1779865691, + "narHash": "sha256-2CVWcov7DiC1qX/B/zFKDJiSYsnbrZ3FNT/viprFWTQ=", + "ref": "refs/heads/jeff/feat/g1_raycast", + "rev": "51666bcd298c1d08bdee179f176f45c0a7dd417d", + "revCount": 744, + "type": "git", + "url": "file:../../../.." + }, + "original": { + "type": "git", + "url": "file:../../../.." + } + }, + "flake-utils": { + "inputs": { + "systems": "systems" + }, + "locked": { + "lastModified": 1731533236, + "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=", + "owner": "numtide", + "repo": "flake-utils", + "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b", + "type": "github" + }, + "original": { + "owner": "numtide", + "repo": "flake-utils", + "type": "github" + } + }, + "nixpkgs": { + "locked": { + "lastModified": 1779560665, + "narHash": "sha256-tpyBcxPpcQb8ukyNF7DoCwfSY3VPsxHoYwj00Cayv5o=", + "owner": "NixOS", + "repo": "nixpkgs", + "rev": "64c08a7ca051951c8eae34e3e3cb1e202fe36786", + "type": "github" + }, + "original": { + "owner": "NixOS", + "ref": "nixos-unstable", + "repo": "nixpkgs", + "type": "github" + } + }, + "root": { + "inputs": { + "dimos-repo": "dimos-repo", + "flake-utils": "flake-utils", + "nixpkgs": "nixpkgs" + } + }, + "systems": { + "locked": { + "lastModified": 1681028828, + "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", + "owner": "nix-systems", + "repo": "default", + "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", + "type": "github" + }, + "original": { + "owner": "nix-systems", + "repo": "default", + "type": "github" + } + } + }, + "root": "root", + "version": 7 +} diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.nix b/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.nix new file mode 100644 index 0000000000..a74d6bb71b --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.nix @@ -0,0 +1,50 @@ +{ + description = "Fake Livox Mid-360 pcap replayer (virtual NIC) native module for DimOS"; + + inputs = { + nixpkgs.url = "github:NixOS/nixpkgs/nixos-unstable"; + flake-utils.url = "github:numtide/flake-utils"; + # Relative git+file: reaches the repo root for the local dimos-module path + # deps (same approach as dimos/mapping/ray_tracing/rust). + dimos-repo = { url = "git+file:../../../../../.."; flake = false; }; + }; + + outputs = { self, nixpkgs, flake-utils, dimos-repo }: + flake-utils.lib.eachDefaultSystem (system: + let + pkgs = import nixpkgs { inherit system; }; + sub = "dimos/hardware/sensors/lidar/livox/virtual_mid360"; + + src = pkgs.runCommand "virtual-mid360-src" {} '' + mkdir -p $out/${sub} + cp -r ${./src} $out/${sub}/src + cp ${./Cargo.toml} $out/${sub}/Cargo.toml + cp ${./Cargo.lock} $out/${sub}/Cargo.lock + + mkdir -p $out/native/rust + cp -r ${dimos-repo}/native/rust/dimos-module $out/native/rust/dimos-module + cp -r ${dimos-repo}/native/rust/dimos-module-macros $out/native/rust/dimos-module-macros + ''; + in { + packages.default = pkgs.rustPlatform.buildRustPackage { + pname = "virtual-mid360"; + version = "0.1.0"; + + inherit src; + cargoRoot = sub; + buildAndTestSubdir = sub; + + # Vendor straight from Cargo.lock. nix's fetchurl sends a User-Agent, + # so crates.io won't 403 it the way nixpkgs' fetchCargoVendor util does. + # The dimos-lcm git dep needs its fetched tree hash pinned here. + cargoLock = { + lockFile = ./Cargo.lock; + outputHashes = { + "dimos-lcm-0.1.0" = "sha256-4DWFTf7Xqnx6pd2jXA/MVpRmZiFr6HqTSp9Qo9ZjToA="; + }; + }; + + meta.mainProgram = "virtual_mid360"; + }; + }); +} diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs new file mode 100644 index 0000000000..1472b8c17b --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs @@ -0,0 +1,511 @@ +// Fake Livox Mid-360 — replays a recorded pcap over a virtual NIC and synthesizes +// the Livox SDK2 control handshake so an unmodified, live-mode pointlio ingests it +// through the real Livox SDK as if from a live sensor. +// +// Inverse of pointlio's in-process `cpp/pcap_replay.hpp` (--replay_pcap), which +// bypasses the network. This exercises the full live stack: SDK discovery + +// control handshake, then point/IMU UDP off a (virtual) wire. +// +// Runs inside the "lidar" network namespace (see setup_commands()); the unmodified +// pointlio runs in the peer "drv" namespace. On any failure the error names the +// exact command to run, then asks the user to re-run the module. + +use dimos_module::{run, LcmTransport, Module}; +use serde::Deserialize; +use std::net::{Ipv4Addr, SocketAddrV4, UdpSocket}; +use std::sync::atomic::{AtomicBool, Ordering}; +use std::sync::Arc; +use std::time::{Duration, Instant, SystemTime, UNIX_EPOCH}; +use validator::Validate; + +// ---- Livox SDK2 control wire format (SdkPacket) ---- +const SOF: u8 = 0xAA; +const WRAPPER: usize = 24; // bytes before data[] +const CMD_PORT: u16 = 56100; +const DISCOVERY_PORT: u16 = 56000; +// data plane: lidar src port -> host dst port +const PORT_POINT: u16 = 56300; +const PORT_IMU: u16 = 56400; +const PORT_STATUS: u16 = 56200; +const DST_POINT: u16 = 56301; +const DST_IMU: u16 = 56401; +const DST_STATUS: u16 = 56201; +// Mid-360 multicasts point/IMU data to this group (the SDK joins it). Add a +// route (224.1.1.5/32 dev ) so it egresses the virtual NIC. +const MCAST_DATA: Ipv4Addr = Ipv4Addr::new(224, 1, 1, 5); +// cmd_id whose ACK means the host finished configuring -> start streaming +const CMD_WORKMODE: u16 = 0x0100; + +#[derive(Debug, Deserialize, Validate)] +#[serde(deny_unknown_fields)] +struct Config { + /// Recorded Mid-360 pcap (data plane: point/IMU/status UDP). Read fully into RAM. + pcap: String, + /// Replay-speed multiplier; 1.0 = original inter-packet timing, >1 = faster. + #[serde(default = "one")] + #[validate(range(min = 0.01, max = 1000.0))] + rate: f64, + /// Seconds to wait after start before streaming begins. + #[serde(default)] + #[validate(range(min = 0.0, max = 3600.0))] + delay: f64, + /// IP the fake lidar sends from (must be assigned to this netns's veth). + #[serde(default = "default_lidar_ip")] + lidar_ip: String, + /// Host IP the recorded data is delivered to (where pointlio's SDK listens). + #[serde(default = "default_host_ip")] + host_ip: String, + /// Network namespace the fake lidar must run inside. + #[serde(default = "default_netns")] + lidar_netns: String, +} + +fn one() -> f64 { + 1.0 +} +fn default_lidar_ip() -> String { + "192.168.1.155".into() +} +fn default_host_ip() -> String { + "192.168.1.5".into() +} +fn default_netns() -> String { + "lidar".into() +} + +#[derive(Module)] +#[module(setup = start)] +struct VirtualMid360 { + #[config] + config: Config, +} + +// ---- CRCs (Livox SDK2: CRC16-CCITT-FALSE over header[0:18], CRC32/IEEE over data[]) ---- +fn crc16_ccitt_false(data: &[u8]) -> u16 { + let mut crc: u16 = 0xFFFF; + for &b in data { + crc ^= (b as u16) << 8; + for _ in 0..8 { + crc = if crc & 0x8000 != 0 { + (crc << 1) ^ 0x1021 + } else { + crc << 1 + }; + } + } + crc +} + +fn crc32_ieee(data: &[u8]) -> u32 { + let mut crc: u32 = 0xFFFF_FFFF; + for &b in data { + crc ^= b as u32; + for _ in 0..8 { + crc = if crc & 1 != 0 { + (crc >> 1) ^ 0xEDB8_8320 + } else { + crc >> 1 + }; + } + } + !crc +} + +/// Build a Livox SDK2 ACK frame from scratch (synthesized, not replayed): +/// header[0:18] (SOF, version=0, length, seq, cmd_id, cmd_type=1 ACK, sender_type=1) +/// + crc16_h@18 + data[] + crc32_d. `data` is the per-cmd ACK payload. +fn build_ack(cmd_id: u16, seq: u32, data: &[u8]) -> Vec { + let length = (WRAPPER + data.len()) as u16; + let mut f = vec![0u8; WRAPPER + data.len()]; + f[0] = SOF; + f[1] = 0; // version + f[2..4].copy_from_slice(&length.to_le_bytes()); + f[4..8].copy_from_slice(&seq.to_le_bytes()); + f[8..10].copy_from_slice(&cmd_id.to_le_bytes()); + f[10] = 1; // cmd_type = ACK + f[11] = 1; // sender_type = lidar + // f[12..18] reserved (0) + let crc16 = crc16_ccitt_false(&f[0..18]); + f[18..20].copy_from_slice(&crc16.to_le_bytes()); + // f[20..24] = crc32 of data[] + f[24..].copy_from_slice(data); + let crc32 = crc32_ieee(data); + f[20..24].copy_from_slice(&crc32.to_le_bytes()); + f +} + +// ---- classic pcap (LE, magic d4c3b2a1) parser -> data-plane UDP packets ---- +struct Pkt { + ts: f64, + src_port: u16, + payload: Vec, +} + +fn parse_pcap(path: &str) -> std::io::Result> { + let buf = std::fs::read(path)?; + if buf.len() < 24 || buf[0..4] != [0xd4, 0xc3, 0xb2, 0xa1] { + return Err(std::io::Error::new( + std::io::ErrorKind::InvalidData, + format!("unsupported pcap (need classic little-endian, magic d4c3b2a1) at {path}"), + )); + } + let mut out = Vec::new(); + let mut off = 24usize; + while off + 16 <= buf.len() { + let ts_sec = u32::from_le_bytes(buf[off..off + 4].try_into().unwrap()); + let ts_usec = u32::from_le_bytes(buf[off + 4..off + 8].try_into().unwrap()); + let incl = u32::from_le_bytes(buf[off + 8..off + 12].try_into().unwrap()) as usize; + off += 16; + if off + incl > buf.len() { + break; + } + let frame = &buf[off..off + incl]; + off += incl; + // Ethernet(14) -> IPv4 -> UDP + if frame.len() < 14 + 20 + 8 || frame[12] != 0x08 || frame[13] != 0x00 { + continue; + } + let ihl = ((frame[14] & 0x0f) as usize) * 4; + if frame[14 + 9] != 17 { + continue; // not UDP + } + let udp = 14 + ihl; + if frame.len() < udp + 8 { + continue; + } + let src_port = u16::from_be_bytes([frame[udp], frame[udp + 1]]); + let udp_len = u16::from_be_bytes([frame[udp + 4], frame[udp + 5]]) as usize; + let payload_start = udp + 8; + let payload_end = (udp + udp_len).min(frame.len()); + if payload_end <= payload_start { + continue; + } + out.push(Pkt { + ts: ts_sec as f64 + ts_usec as f64 / 1e6, + src_port, + payload: frame[payload_start..payload_end].to_vec(), + }); + } + Ok(out) +} + +/// Verify we're in the lidar netns with lidar_ip bindable; else return a helpful +/// error naming the exact `sudo ip netns ...` commands and to re-run. +fn ensure_interface(cfg: &Config) -> Result { + let lidar_ip: Ipv4Addr = cfg + .lidar_ip + .parse() + .map_err(|_| format!("invalid lidar_ip '{}'", cfg.lidar_ip))?; + // Probe: can we bind the lidar control port on lidar_ip? If not, the veth/netns + // isn't set up (or we're in the wrong namespace). + let probe = UdpSocket::bind(SocketAddrV4::new(lidar_ip, CMD_PORT)); + if probe.is_err() { + let ns = &cfg.lidar_netns; + let lip = &cfg.lidar_ip; + let hip = &cfg.host_ip; + return Err(format!( + "cannot bind {lip}:{CMD_PORT} — the virtual network interface isn't set up \ + (or this process isn't in the '{ns}' netns).\n\ + Run this once (creates the lidar/drv veth pair), then re-run the module:\n\ + \n sudo ip netns add drv\n sudo ip netns add {ns}\n \ + sudo ip link add veth-drv type veth peer name veth-lidar\n \ + sudo ip link set veth-drv netns drv\n \ + sudo ip link set veth-lidar netns {ns}\n \ + sudo ip netns exec drv ip addr add {hip}/24 dev veth-drv\n \ + sudo ip netns exec {ns} ip addr add {lip}/24 dev veth-lidar\n \ + sudo ip netns exec drv ip link set veth-drv up\n \ + sudo ip netns exec {ns} ip link set veth-lidar up\n \ + sudo ip netns exec drv ip link set lo up\n \ + sudo ip netns exec {ns} ip link set lo up\n \ + sudo ip netns exec drv ip link set veth-drv multicast on\n \ + sudo ip netns exec {ns} ip link set veth-lidar multicast on\n \ + sudo ip netns exec {ns} ip route add 255.255.255.255/32 dev veth-lidar\n \ + sudo ip netns exec {ns} ip route add 224.1.1.5/32 dev veth-lidar # point/IMU multicast\n \ + sudo ip netns exec drv ip route add 224.0.0.0/4 dev lo # LCM (dimos transport)\n \ + sudo ip netns exec {ns} ip route add 224.0.0.0/4 dev lo # LCM (dimos transport)\n\ + \nThen launch this module inside the lidar netns:\n \ + sudo ip netns exec {ns} " + )); + } + Ok(lidar_ip) +} + +impl VirtualMid360 { + async fn start(&mut self) { + let cfg = &self.config; + let lidar_ip = match ensure_interface(cfg) { + Ok(ip) => ip, + Err(msg) => { + // Actionable error: print the fix command, then exit non-zero so the + // coordinator surfaces it and the user can re-run after setup. + tracing::error!("{msg}"); + eprintln!("\n[virtual_mid360] {msg}\n"); + std::process::exit(2); + } + }; + let host_ip: Ipv4Addr = cfg.host_ip.parse().expect("host_ip validated bindable"); + + let packets = match parse_pcap(&cfg.pcap) { + Ok(p) if !p.is_empty() => Arc::new(p), + Ok(_) => { + eprintln!( + "[virtual_mid360] pcap '{}' has no Livox UDP data packets. \ + Check the path / that it's a Mid-360 capture, then re-run.", + cfg.pcap + ); + std::process::exit(2); + } + Err(e) => { + eprintln!( + "[virtual_mid360] failed to read pcap '{}': {e}. Fix the path, then re-run.", + cfg.pcap + ); + std::process::exit(2); + } + }; + + let stop = Arc::new(AtomicBool::new(false)); + let armed = Arc::new(AtomicBool::new(false)); + let rate = cfg.rate; + let delay = cfg.delay; + + // Role 1: discovery responder (:56000 broadcast) — synthesize the 0x0000 ACK. + spawn_discovery(lidar_ip, stop.clone()); + // Role 2: control responder (:56100) — synthesize per-cmd ACKs; arm streaming + // when the host issues the work-mode/config command (0x0100). + spawn_control(lidar_ip, armed.clone(), stop.clone()); + // Role 3: data streamer — point/IMU/status, paced at `rate`, timestamps rewritten + // to now, armed by the handshake (with `delay` as a startup floor / fallback). + spawn_stream(lidar_ip, host_ip, packets, rate, delay, armed, stop); + tracing::info!(lidar = %lidar_ip, host = %host_ip, rate, delay, "virtual_mid360 started"); + } +} + +fn spawn_discovery(lidar_ip: Ipv4Addr, stop: Arc) { + std::thread::spawn(move || { + let sock = match UdpSocket::bind(SocketAddrV4::new(Ipv4Addr::UNSPECIFIED, DISCOVERY_PORT)) { + Ok(s) => s, + Err(e) => { + tracing::error!("discovery bind :{DISCOVERY_PORT} failed: {e}"); + return; + } + }; + let _ = sock.set_broadcast(true); + sock.set_read_timeout(Some(Duration::from_millis(500))).ok(); + let bcast = SocketAddrV4::new(Ipv4Addr::BROADCAST, DISCOVERY_PORT); + let mut buf = [0u8; 2048]; + while !stop.load(Ordering::Relaxed) { + let n = match sock.recv_from(&mut buf) { + Ok((n, _)) => n, + Err(_) => continue, + }; + if n < WRAPPER || buf[0] != SOF { + continue; + } + let cmd_id = u16::from_le_bytes([buf[8], buf[9]]); + let cmd_type = buf[10]; + if cmd_id != 0x0000 || cmd_type != 0 { + continue; + } + let seq = u32::from_le_bytes([buf[4], buf[5], buf[6], buf[7]]); + // TODO(payload): discovery ACK data describes the device (dev_type, serial, + // lidar_ip, cmd port). Enumerate the exact layout from livox-sdk2 source. + let ack = build_ack(0x0000, seq, &discovery_ack_payload(lidar_ip)); + let _ = sock.send_to(&ack, bcast); + } + }); +} + +fn spawn_control(lidar_ip: Ipv4Addr, armed: Arc, stop: Arc) { + std::thread::spawn(move || { + let sock = match UdpSocket::bind(SocketAddrV4::new(lidar_ip, CMD_PORT)) { + Ok(s) => s, + Err(e) => { + tracing::error!("control bind {lidar_ip}:{CMD_PORT} failed: {e}"); + return; + } + }; + sock.set_read_timeout(Some(Duration::from_millis(500))).ok(); + let mut buf = [0u8; 2048]; + while !stop.load(Ordering::Relaxed) { + let (n, from) = match sock.recv_from(&mut buf) { + Ok(x) => x, + Err(_) => continue, + }; + if n < WRAPPER || buf[0] != SOF { + continue; + } + let seq = u32::from_le_bytes([buf[4], buf[5], buf[6], buf[7]]); + let cmd_id = u16::from_le_bytes([buf[8], buf[9]]); + // TODO(payload): per-cmd_id ACK data. Most replies = ret_code(u8)=0 (success); + // queries echo the requested fields. Enumerate cmd_ids + payloads from + // livox-sdk2 source (comm/command_impl) or one captured real handshake. + let ack = build_ack(cmd_id, seq, &control_ack_payload(cmd_id)); + let _ = sock.send_to(&ack, from); + tracing::info!( + cmd_id = format!("0x{cmd_id:04x}"), + seq, + "control REQ -> ACK" + ); + if cmd_id == CMD_WORKMODE { + armed.store(true, Ordering::Relaxed); + tracing::info!("work-mode cmd 0x0100 acked -> arming data stream"); + } + } + }); +} + +fn spawn_stream( + lidar_ip: Ipv4Addr, + host_ip: Ipv4Addr, + packets: Arc>, + rate: f64, + delay: f64, + armed: Arc, + stop: Arc, +) { + std::thread::spawn(move || { + let mk = |sport: u16| -> std::io::Result { + UdpSocket::bind(SocketAddrV4::new(lidar_ip, sport)) + }; + let (point, imu, status) = match (mk(PORT_POINT), mk(PORT_IMU), mk(PORT_STATUS)) { + (Ok(a), Ok(b), Ok(c)) => (a, b, c), + _ => { + tracing::error!("failed to bind data-plane source ports on {lidar_ip}"); + return; + } + }; + // Wait for handshake to arm streaming, with `delay` as a startup floor + fallback. + let waited = Instant::now(); + while !armed.load(Ordering::Relaxed) && !stop.load(Ordering::Relaxed) { + if waited.elapsed().as_secs_f64() >= delay.max(0.0) && delay > 0.0 { + tracing::warn!("no handshake within delay={delay}s — arming stream anyway"); + break; + } + std::thread::sleep(Duration::from_millis(50)); + } + std::thread::sleep(Duration::from_secs_f64(delay.max(0.0))); + tracing::info!("streaming {} packets at {rate}x", packets.len()); + + // Shift every packet's Livox sensor timestamp by a constant so the first + // emitted packet reads ≈ now and the original inter-packet spacing (used for + // intra-scan deskew) is preserved — the stream looks current/live. + let now_ns = SystemTime::now() + .duration_since(UNIX_EPOCH) + .unwrap() + .as_nanos() as u64; + let first_orig = packets + .iter() + .find(|p| matches!(p.src_port, PORT_POINT | PORT_IMU)) + .map(|p| read_ts_ns(&p.payload)) + .unwrap_or(0); + let ts_shift = now_ns.wrapping_sub(first_orig); + + let t_wall0 = Instant::now(); + let mut t_cap0: Option = None; + for p in packets.iter() { + if stop.load(Ordering::Relaxed) { + break; + } + // The Mid-360 MULTICASTS point/IMU to MCAST_DATA:port (the SDK joins that + // group — confirmed via `ss -ulnp` showing 224.1.1.5:56301/56401); status + // is unicast to the host. Sending point/IMU unicast is silently dropped. + let (sock, dst_ip, dst) = match p.src_port { + PORT_POINT => (&point, MCAST_DATA, DST_POINT), + PORT_IMU => (&imu, MCAST_DATA, DST_IMU), + PORT_STATUS => (&status, host_ip, DST_STATUS), + _ => continue, + }; + let t0 = *t_cap0.get_or_insert(p.ts); + let target = (p.ts - t0) / rate; + let elapsed = t_wall0.elapsed().as_secs_f64(); + if target > elapsed { + std::thread::sleep(Duration::from_secs_f64(target - elapsed)); + } + let mut out = p.payload.clone(); + if matches!(p.src_port, PORT_POINT | PORT_IMU) { + rewrite_ts(&mut out, ts_shift); + } + let _ = sock.send_to(&out, SocketAddrV4::new(dst_ip, dst)); + } + tracing::info!("data stream finished"); + }); +} + +// ---- payload synthesizers (layouts from Livox-SDK2 sdk_core/comm/define.h) ---- +// Mid-360 device type (livox_lidar_def.h: kLivoxLidarTypeMid360 = 9). +const DEV_TYPE_MID360: u8 = 9; + +/// Detection/search (0x0000) ACK body == `DetectionData`: +/// ret_code:u8, dev_type:u8, sn[16], lidar_ip[4], cmd_port:u16 LE. +/// The SDK's VerifyNetSegment requires lidar_ip on the host's /24 (192.168.1.x). +fn discovery_ack_payload(lidar_ip: Ipv4Addr) -> Vec { + let mut d = Vec::with_capacity(24); + d.push(0); // ret_code = success + d.push(DEV_TYPE_MID360); + // sn[16] MUST be null-terminated within 16 bytes — the SDK treats it as a + // C-string (strcpy), so a full-16 SN with no NUL overruns its buffer. + let mut sn = [0u8; 16]; + sn[..10].copy_from_slice(b"FAKEMID360"); // sn[10..]=0 -> NUL-terminated + d.extend_from_slice(&sn); + d.extend_from_slice(&lidar_ip.octets()); + d.extend_from_slice(&CMD_PORT.to_le_bytes()); + d +} + +// kKeyFwType (livox_lidar_def.h ParamKeyName = 0x8010); fw_type != 0 => app +// firmware (not loader/upgrade mode), so the SDK proceeds to normal operation. +const KEY_FW_TYPE: u16 = 0x8010; +const FW_TYPE_APP: u8 = 1; + +/// Control-plane ACK bodies. The SDK casts the SdkPacket data[] directly to the +/// per-cmd response struct, which are #pragma pack(1) (packed, no padding). +fn control_ack_payload(cmd_id: u16) -> Vec { + match cmd_id { + // GetInternalInfo (0x0101): LivoxLidarDiagInternalInfoResponse (packed) — + // ret_code:u8 @0, param_num:u16 @1, data @3 (= LivoxLidarKeyValueParam: + // key:u16 @0, length:u16 @2, value @4). QueryFwType expects one param + // keyed kKeyFwType (0x8010) with a 1-byte fw_type value (non-zero = app). + 0x0101 => { + let mut d = vec![0u8; 8]; + // d[0] ret_code = 0 + d[1..3].copy_from_slice(&1u16.to_le_bytes()); // param_num = 1 + d[3..5].copy_from_slice(&KEY_FW_TYPE.to_le_bytes()); + d[5..7].copy_from_slice(&1u16.to_le_bytes()); // value length = 1 + d[7] = FW_TYPE_APP; + d + } + // Others: LivoxLidarAsyncControlResponse (packed) { ret_code:u8 @0, + // error_key:u16 @1 } = 3 bytes. ret_code=0 (success), error_key=0. + _ => vec![0u8; 3], + } +} +// LivoxLidarEthernetPacket.timestamp[8] sits at payload offset 28 (packed: +// version@0,len@1,time_interval@3,dot_num@5,udp_cnt@7,frame_cnt@9,data_type@10, +// time_type@11,rsvd@12,crc32@24,timestamp@28). The SDK casts the UDP payload +// directly to LivoxLidarEthernetPacket*, so offset 28 is in the payload. +const TS_OFFSET: usize = 28; + +fn read_ts_ns(payload: &[u8]) -> u64 { + if payload.len() >= TS_OFFSET + 8 { + u64::from_le_bytes(payload[TS_OFFSET..TS_OFFSET + 8].try_into().unwrap()) + } else { + 0 + } +} + +fn rewrite_ts(payload: &mut [u8], shift: u64) { + if payload.len() >= TS_OFFSET + 8 { + let orig = u64::from_le_bytes(payload[TS_OFFSET..TS_OFFSET + 8].try_into().unwrap()); + let new = orig.wrapping_add(shift); + payload[TS_OFFSET..TS_OFFSET + 8].copy_from_slice(&new.to_le_bytes()); + } +} + +#[tokio::main] +async fn main() { + let transport = LcmTransport::new() + .await + .expect("Failed to create transport"); + run::(transport).await; +} From 0504aae6bba264d913861bfdc16d6d08c1d03026 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sat, 13 Jun 2026 07:18:04 -0700 Subject: [PATCH 055/185] feat(pointlio): publish body-frame cloud, split frame_ids, remove global_map - Cloud now published in the sensor frame (mid360_link): use fastlio2 get_body_cloud() (undistorted scan, no world registration) instead of inverse-transforming the world cloud. No transform in publish_lidar. - Split frames: frame_id (mid360_link) on both the cloud + odometry headers; odom_parent_frame_id (odom) -> odom_frame_id (base_link) for the TF publish. - Remove global_map / voxel_map.hpp entirely (deleted file, config, blueprint + pcap_to_db references). - Bump fast-lio pin to fcbd1c2 (adds get_body_cloud). --- .../sensors/lidar/pointlio/cpp/flake.lock | 6 +- .../sensors/lidar/pointlio/cpp/main.cpp | 102 ++---- .../sensors/lidar/pointlio/cpp/voxel_map.hpp | 297 ------------------ .../hardware/sensors/lidar/pointlio/module.py | 31 +- .../lidar/pointlio/pointlio_blueprints.py | 2 +- .../lidar/pointlio/tools/pcap_to_db.py | 1 - 6 files changed, 40 insertions(+), 399 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/pointlio/cpp/voxel_map.hpp diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock index a26ebea325..b319cd9875 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock @@ -37,11 +37,11 @@ "fast-lio": { "flake": false, "locked": { - "lastModified": 1781136428, - "narHash": "sha256-rqzroZAzhQ7A+wOGsN7Qzk64p7m3+4doyb92fyLAOZE=", + "lastModified": 1781343224, + "narHash": "sha256-1CBt6felqK7VUbiDijAcjLzNI8A0sMieJdb1NQ1l3yk=", "owner": "dimensionalOS", "repo": "dimos-module-fastlio2", - "rev": "02d506674b8e532d6fc094214ea76c958dbfcbcb", + "rev": "fcbd1c229b6f7eb221df33c00e1ed53fd5c03126", "type": "github" }, "original": { diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 77d87a04a8..7360f75ebc 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -4,8 +4,8 @@ // FAST-LIO2 + Livox Mid-360 native module for dimos NativeModule framework. // // Binds Livox SDK2 directly into FAST-LIO-NON-ROS: SDK callbacks feed -// CustomMsg/Imu to FastLio, which performs EKF-LOAM SLAM. Registered -// (world-frame) point clouds and odometry are published on LCM. +// CustomMsg/Imu to FastLio, which performs EKF-LOAM SLAM. Sensor-frame +// (mid360_link) point clouds and odometry are published on LCM. // // Usage: // ./fastlio2_native \ @@ -35,7 +35,6 @@ #include "dimos_native_module.hpp" #include "pcap_replay.hpp" #include "timing.hpp" -#include "voxel_map.hpp" // dimos LCM message headers #include "geometry_msgs/Quaternion.hpp" @@ -123,7 +122,6 @@ static std::optional virtual_now() { static std::string g_lidar_topic; static std::string g_odometry_topic; -static std::string g_map_topic; static std::string g_frame_id; // required via --frame_id static std::string g_child_frame_id; // required via --child_frame_id static float g_frequency = 10.0f; @@ -184,9 +182,11 @@ using dimos::time_from_seconds; using dimos::make_header; // --------------------------------------------------------------------------- -// Publish lidar (world-frame point cloud) +// Publish lidar point cloud in the sensor body frame (g_frame_id / mid360_link) // --------------------------------------------------------------------------- - +// +// `cloud` is FAST-LIO's undistorted scan in the sensor's own frame +// (get_body_cloud), so points are published as-is with no world registration. static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, const std::string& topic = "") { const std::string& chan = topic.empty() ? g_lidar_topic : topic; @@ -225,23 +225,11 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, pc.data_length = pc.row_step; pc.data.resize(pc.data_length); - // Apply full init_pose (rotation+translation) to match the odometry frame. - const bool apply_init_pose = has_init_pose(); for (int i = 0; i < num_points; ++i) { float* dst = reinterpret_cast(pc.data.data() + i * 16); - if (apply_init_pose) { - double rx, ry, rz; - quat_rotate(g_init_qx, g_init_qy, g_init_qz, g_init_qw, - cloud->points[i].x, cloud->points[i].y, cloud->points[i].z, - rx, ry, rz); - dst[0] = static_cast(rx + g_init_x); - dst[1] = static_cast(ry + g_init_y); - dst[2] = static_cast(rz + g_init_z); - } else { - dst[0] = cloud->points[i].x; - dst[1] = cloud->points[i].y; - dst[2] = cloud->points[i].z; - } + dst[0] = cloud->points[i].x; + dst[1] = cloud->points[i].y; + dst[2] = cloud->points[i].z; dst[3] = cloud->points[i].intensity; } @@ -499,7 +487,6 @@ int main(int argc, char** argv) { // Required: LCM topics for output ports g_lidar_topic = mod.has("lidar") ? mod.topic("lidar") : ""; g_odometry_topic = mod.has("odometry") ? mod.topic("odometry") : ""; - g_map_topic = mod.has("global_map") ? mod.topic("global_map") : ""; if (g_lidar_topic.empty() && g_odometry_topic.empty()) { fprintf(stderr, "Error: at least one of --lidar or --odometry is required\n"); @@ -522,16 +509,13 @@ int main(int argc, char** argv) { std::string lidar_ip = mod.arg("lidar_ip", "192.168.1.155"); g_frequency = mod.arg_float("frequency", 10.0f); g_frame_id = mod.arg_required("frame_id"); - g_child_frame_id = mod.arg_required("child_frame_id"); + g_child_frame_id = mod.arg_required("odom_frame_id"); float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f); float odom_freq = mod.arg_float("odom_freq", 50.0f); CloudFilterConfig filter_cfg; filter_cfg.voxel_size = mod.arg_float("voxel_size", 0.1f); filter_cfg.sor_mean_k = mod.arg_int("sor_mean_k", 50); filter_cfg.sor_stddev = mod.arg_float("sor_stddev", 1.0f); - float map_voxel_size = mod.arg_float("map_voxel_size", 0.1f); - float map_max_range = mod.arg_float("map_max_range", 100.0f); - float map_freq = mod.arg_float("map_freq", 0.0f); // Propagates to the FAST-LIO core via the `fastlio_debug` global. bool debug = mod.arg_bool("debug", false); @@ -608,8 +592,6 @@ int main(int argc, char** argv) { g_lidar_topic.empty() ? "(disabled)" : g_lidar_topic.c_str()); printf("[fastlio2] odometry topic: %s\n", g_odometry_topic.empty() ? "(disabled)" : g_odometry_topic.c_str()); - printf("[fastlio2] global_map topic: %s\n", - g_map_topic.empty() ? "(disabled)" : g_map_topic.c_str()); printf("[fastlio2] config: %s\n", config_path.c_str()); printf("[fastlio2] host_ip: %s lidar_ip: %s frequency: %.1f Hz\n", host_ip.c_str(), lidar_ip.c_str(), g_frequency); @@ -617,9 +599,6 @@ int main(int argc, char** argv) { pointcloud_freq, odom_freq); printf("[fastlio2] voxel_size: %.3f sor_mean_k: %d sor_stddev: %.1f\n", filter_cfg.voxel_size, filter_cfg.sor_mean_k, filter_cfg.sor_stddev); - if (!g_map_topic.empty()) - printf("[fastlio2] map_voxel_size: %.3f map_max_range: %.1f map_freq: %.1f Hz\n", - map_voxel_size, map_max_range, map_freq); } // Signal handlers @@ -653,14 +632,6 @@ int main(int argc, char** argv) { std::optional last_pc_publish; std::optional last_odom_publish; - std::unique_ptr global_map; - std::chrono::microseconds map_interval{0}; - std::optional last_map_publish; - if (!g_map_topic.empty() && map_freq > 0.0f) { - global_map = std::make_unique(map_voxel_size, map_max_range); - map_interval = std::chrono::microseconds( - static_cast(1e6 / map_freq)); - } // Per-section timing for `run_main_iter`, active only with --debug. // maybe_flush() below prints a summary every second. @@ -668,11 +639,9 @@ int main(int argc, char** argv) { static timing::Section t_emit_check{"emit.lock+swap"}; static timing::Section t_feed_lidar{"fast_lio.feed_lidar"}; static timing::Section t_process{"fast_lio.process"}; - static timing::Section t_get_world_cloud{"fast_lio.get_world_cloud"}; + static timing::Section t_get_world_cloud{"fast_lio.get_body_cloud"}; static timing::Section t_filter_cloud{"filter_cloud"}; static timing::Section t_publish_lidar{"publish_lidar"}; - static timing::Section t_map_insert{"global_map.insert"}; - static timing::Section t_map_publish{"global_map.publish"}; static timing::Section t_publish_odom{"publish_odometry"}; auto run_main_iter = [&](std::chrono::steady_clock::time_point now) { @@ -698,9 +667,6 @@ int main(int argc, char** argv) { if (!last_odom_publish.has_value()) { last_odom_publish = seed; } - if (global_map && !last_map_publish.has_value()) { - last_map_publish = seed; - } // At frame rate: drain accumulated points into a CustomMsg and feed // FAST-LIO. Hold g_pc_mutex across the rate-limit check AND swap so the @@ -754,46 +720,22 @@ int main(int argc, char** argv) { const bool lidar_due = !g_lidar_topic.empty() && now - *last_pc_publish >= pc_interval; - const bool map_due = - global_map && now - *last_map_publish >= map_interval; - - // get_world_cloud + filter_cloud (SOR) is the loop's costliest step, - // so build it only when a publish is due. CPU optimization, not the - // divergence fix (that was a deque race in the core, fixed there). - if (lidar_due || map_due) { - auto world_cloud = ([&]() { + + // get_body_cloud + filter_cloud (SOR) is the loop's costliest step, + // so build it only when a publish is due. + if (lidar_due) { + auto body_cloud = ([&]() { timing::Scope s(t_get_world_cloud); - return fast_lio.get_world_cloud(); + return fast_lio.get_body_cloud(); })(); - if (world_cloud && !world_cloud->empty()) { + if (body_cloud && !body_cloud->empty()) { auto filtered = ([&]() { timing::Scope s(t_filter_cloud); - return filter_cloud(world_cloud, filter_cfg); + return filter_cloud(body_cloud, filter_cfg); })(); - - if (lidar_due) { - timing::Scope s(t_publish_lidar); - publish_lidar(filtered, ts); - last_pc_publish = now; - } - - // Global voxel map: insert scan, prune, publish at map_freq. - if (global_map) { - { - timing::Scope s(t_map_insert); - global_map->insert(filtered); - } - if (map_due) { - timing::Scope s(t_map_publish); - global_map->prune( - static_cast(pose[0]), - static_cast(pose[1]), - static_cast(pose[2])); - auto map_cloud = global_map->to_cloud(); - publish_lidar(map_cloud, ts, g_map_topic); - last_map_publish = now; - } - } + timing::Scope s(t_publish_lidar); + publish_lidar(filtered, ts); + last_pc_publish = now; } } diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/voxel_map.hpp b/dimos/hardware/sensors/lidar/pointlio/cpp/voxel_map.hpp deleted file mode 100644 index a50740cd04..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/voxel_map.hpp +++ /dev/null @@ -1,297 +0,0 @@ -// Copyright 2026 Dimensional Inc. -// SPDX-License-Identifier: Apache-2.0 -// -// Efficient global voxel map using a hash map. -// Supports O(1) insert/update, distance-based pruning, and -// raycasting-based free space clearing via Amanatides & Woo 3D DDA. -// FOV is discovered dynamically from incoming point cloud data. - -#ifndef VOXEL_MAP_HPP_ -#define VOXEL_MAP_HPP_ - -#include -#include -#include - -#include -#include - -struct VoxelKey { - int32_t x, y, z; - bool operator==(const VoxelKey& o) const { return x == o.x && y == o.y && z == o.z; } -}; - -struct VoxelKeyHash { - size_t operator()(const VoxelKey& k) const { - // Fast spatial hash — large primes reduce collisions for grid coords - size_t h = static_cast(k.x) * 73856093u; - h ^= static_cast(k.y) * 19349669u; - h ^= static_cast(k.z) * 83492791u; - return h; - } -}; - -struct Voxel { - float x, y, z; // running centroid - float intensity; - uint32_t count; // points merged into this voxel - uint8_t miss_count; // consecutive scans where a ray passed through without hitting -}; - -/// Config for raycast-based free space clearing. -struct RaycastConfig { - int subsample = 4; // raycast every Nth point - int max_misses = 3; // erase after this many consecutive misses - float fov_margin_rad = 0.035f; // ~2° safety margin added to discovered FOV -}; - -class VoxelMap { -public: - explicit VoxelMap(float voxel_size, float max_range = 100.0f) - : voxel_size_(voxel_size), max_range_(max_range) { - map_.reserve(500000); - } - - /// Insert a point cloud into the map, merging into existing voxels. - /// Resets miss_count for hit voxels. - template - void insert(const typename pcl::PointCloud::Ptr& cloud) { - if (!cloud) return; - float inv = 1.0f / voxel_size_; - for (const auto& pt : cloud->points) { - VoxelKey key{ - static_cast(std::floor(pt.x * inv)), - static_cast(std::floor(pt.y * inv)), - static_cast(std::floor(pt.z * inv))}; - - auto it = map_.find(key); - if (it != map_.end()) { - // Running average update - auto& v = it->second; - float n = static_cast(v.count); - float n1 = n + 1.0f; - v.x = (v.x * n + pt.x) / n1; - v.y = (v.y * n + pt.y) / n1; - v.z = (v.z * n + pt.z) / n1; - v.intensity = (v.intensity * n + pt.intensity) / n1; - v.count++; - v.miss_count = 0; - } else { - map_.emplace(key, Voxel{pt.x, pt.y, pt.z, pt.intensity, 1, 0}); - } - } - } - - /// Cast rays from sensor origin through each point in the cloud. - /// Discovers the sensor FOV from the cloud's elevation angle range, - /// then marks intermediate voxels as missed and erases those exceeding - /// the miss threshold within the discovered FOV. - /// - /// Orientation quaternion (qx,qy,qz,qw) is body→world. - template - void raycast_clear(float ox, float oy, float oz, - float qx, float qy, float qz, float qw, - const typename pcl::PointCloud::Ptr& cloud, - const RaycastConfig& cfg) { - if (!cloud || cloud->empty() || cfg.max_misses <= 0) return; - - // Phase 0: discover FOV from this scan's elevation angles in sensor-local frame - update_fov(ox, oy, oz, qx, qy, qz, qw, cloud); - - // Skip raycasting until we have a valid FOV (need at least a few scans) - if (!fov_valid_) return; - - float inv = 1.0f / voxel_size_; - int n_pts = static_cast(cloud->size()); - float fov_up = fov_up_ + cfg.fov_margin_rad; - float fov_down = fov_down_ - cfg.fov_margin_rad; - - // Phase 1: walk rays, increment miss_count for intermediate voxels - for (int i = 0; i < n_pts; i += cfg.subsample) { - const auto& pt = cloud->points[i]; - raycast_single(ox, oy, oz, pt.x, pt.y, pt.z, inv); - } - - // Phase 2: erase voxels that exceeded miss threshold and are within FOV - for (auto it = map_.begin(); it != map_.end();) { - if (it->second.miss_count > static_cast(cfg.max_misses)) { - if (in_sensor_fov(ox, oy, oz, qx, qy, qz, qw, - it->second.x, it->second.y, it->second.z, - fov_up, fov_down)) { - it = map_.erase(it); - continue; - } - } - ++it; - } - } - - /// Remove voxels farther than max_range from the given position. - void prune(float px, float py, float pz) { - float r2 = max_range_ * max_range_; - for (auto it = map_.begin(); it != map_.end();) { - float dx = it->second.x - px; - float dy = it->second.y - py; - float dz = it->second.z - pz; - if (dx * dx + dy * dy + dz * dz > r2) - it = map_.erase(it); - else - ++it; - } - } - - /// Export all voxel centroids as a point cloud. - template - typename pcl::PointCloud::Ptr to_cloud() const { - typename pcl::PointCloud::Ptr cloud( - new pcl::PointCloud(map_.size(), 1)); - size_t i = 0; - for (const auto& [key, v] : map_) { - auto& pt = cloud->points[i++]; - pt.x = v.x; - pt.y = v.y; - pt.z = v.z; - pt.intensity = v.intensity; - } - return cloud; - } - - size_t size() const { return map_.size(); } - void clear() { map_.clear(); } - void set_max_range(float r) { max_range_ = r; } - float fov_up_deg() const { return fov_up_ * 180.0f / static_cast(M_PI); } - float fov_down_deg() const { return fov_down_ * 180.0f / static_cast(M_PI); } - bool fov_valid() const { return fov_valid_; } - -private: - std::unordered_map map_; - float voxel_size_; - float max_range_; - - // Dynamically discovered sensor FOV (accumulated over scans) - float fov_up_ = -static_cast(M_PI); // start narrow, expand from data - float fov_down_ = static_cast(M_PI); - int fov_scan_count_ = 0; - bool fov_valid_ = false; - static constexpr int FOV_WARMUP_SCANS = 5; // require N scans before trusting FOV - - /// Update discovered FOV from a scan's elevation angles in sensor-local frame. - template - void update_fov(float ox, float oy, float oz, - float qx, float qy, float qz, float qw, - const typename pcl::PointCloud::Ptr& cloud) { - // Inverse quaternion for world→sensor rotation - float nqx = -qx, nqy = -qy, nqz = -qz; - - for (const auto& pt : cloud->points) { - float wx = pt.x - ox, wy = pt.y - oy, wz = pt.z - oz; - - // Rotate to sensor-local frame - float tx = 2.0f * (nqy * wz - nqz * wy); - float ty = 2.0f * (nqz * wx - nqx * wz); - float tz = 2.0f * (nqx * wy - nqy * wx); - float lx = wx + qw * tx + (nqy * tz - nqz * ty); - float ly = wy + qw * ty + (nqz * tx - nqx * tz); - float lz = wz + qw * tz + (nqx * ty - nqy * tx); - - float horiz_dist = std::sqrt(lx * lx + ly * ly); - if (horiz_dist < 1e-6f) continue; - float elevation = std::atan2(lz, horiz_dist); - - if (elevation > fov_up_) fov_up_ = elevation; - if (elevation < fov_down_) fov_down_ = elevation; - } - - if (++fov_scan_count_ >= FOV_WARMUP_SCANS && !fov_valid_) { - fov_valid_ = true; - printf("[voxel_map] FOV discovered: [%.1f, %.1f] deg\n", - fov_down_deg(), fov_up_deg()); - } - } - - /// Amanatides & Woo 3D DDA: walk from (ox,oy,oz) to (px,py,pz), - /// incrementing miss_count for all intermediate voxels. - void raycast_single(float ox, float oy, float oz, - float px, float py, float pz, float inv) { - float dx = px - ox, dy = py - oy, dz = pz - oz; - float len = std::sqrt(dx * dx + dy * dy + dz * dz); - if (len < 1e-6f) return; - dx /= len; dy /= len; dz /= len; - - int32_t cx = static_cast(std::floor(ox * inv)); - int32_t cy = static_cast(std::floor(oy * inv)); - int32_t cz = static_cast(std::floor(oz * inv)); - int32_t ex = static_cast(std::floor(px * inv)); - int32_t ey = static_cast(std::floor(py * inv)); - int32_t ez = static_cast(std::floor(pz * inv)); - - int sx = (dx >= 0) ? 1 : -1; - int sy = (dy >= 0) ? 1 : -1; - int sz = (dz >= 0) ? 1 : -1; - - // tMax: parametric distance along ray to next voxel boundary per axis - // tDelta: parametric distance to cross one full voxel per axis - float tMaxX = (std::abs(dx) < 1e-10f) ? 1e30f - : (((dx > 0 ? cx + 1 : cx) * voxel_size_ - ox) / dx); - float tMaxY = (std::abs(dy) < 1e-10f) ? 1e30f - : (((dy > 0 ? cy + 1 : cy) * voxel_size_ - oy) / dy); - float tMaxZ = (std::abs(dz) < 1e-10f) ? 1e30f - : (((dz > 0 ? cz + 1 : cz) * voxel_size_ - oz) / dz); - - float tDeltaX = (std::abs(dx) < 1e-10f) ? 1e30f : std::abs(voxel_size_ / dx); - float tDeltaY = (std::abs(dy) < 1e-10f) ? 1e30f : std::abs(voxel_size_ / dy); - float tDeltaZ = (std::abs(dz) < 1e-10f) ? 1e30f : std::abs(voxel_size_ / dz); - - // Walk through voxels (skip endpoint — it was hit) - int max_steps = static_cast(len * inv) + 3; // safety bound - for (int step = 0; step < max_steps; ++step) { - if (cx == ex && cy == ey && cz == ez) break; // reached endpoint - - VoxelKey key{cx, cy, cz}; - auto it = map_.find(key); - if (it != map_.end() && it->second.miss_count < 255) { - it->second.miss_count++; - } - - // Step to next voxel on the axis with smallest tMax - if (tMaxX < tMaxY && tMaxX < tMaxZ) { - cx += sx; tMaxX += tDeltaX; - } else if (tMaxY < tMaxZ) { - cy += sy; tMaxY += tDeltaY; - } else { - cz += sz; tMaxZ += tDeltaZ; - } - } - } - - /// Check if a voxel centroid falls within the sensor's vertical FOV. - /// Rotates the vector (sensor→voxel) into sensor-local frame using the - /// inverse of the body→world quaternion, then checks elevation angle. - static bool in_sensor_fov(float ox, float oy, float oz, - float qx, float qy, float qz, float qw, - float vx, float vy, float vz, - float fov_up_rad, float fov_down_rad) { - // Vector from sensor origin to voxel in world frame - float wx = vx - ox, wy = vy - oy, wz = vz - oz; - - // Rotate by quaternion inverse (conjugate): q* = (-qx,-qy,-qz,qw) - float nqx = -qx, nqy = -qy, nqz = -qz; - // t = 2 * cross(q.xyz, v) - float tx = 2.0f * (nqy * wz - nqz * wy); - float ty = 2.0f * (nqz * wx - nqx * wz); - float tz = 2.0f * (nqx * wy - nqy * wx); - // v' = v + qw * t + cross(q.xyz, t) - float lx = wx + qw * tx + (nqy * tz - nqz * ty); - float ly = wy + qw * ty + (nqz * tx - nqx * tz); - float lz = wz + qw * tz + (nqx * ty - nqy * tx); - - // Elevation angle in sensor-local frame - float horiz_dist = std::sqrt(lx * lx + ly * ly); - if (horiz_dist < 1e-6f) return true; // directly above/below, treat as in FOV - float elevation = std::atan2(lz, horiz_dist); - - return elevation >= fov_down_rad && elevation <= fov_up_rad; - } -}; - -#endif diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index 59d6e897ae..9f0c7db28d 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -61,8 +61,8 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.navigation.nav_stack.frames import FRAME_BODY, FRAME_ODOM -from dimos.spec import mapping, perception +from dimos.navigation.nav_stack.frames import FRAME_ODOM +from dimos.spec import perception from dimos.utils.generic import get_local_ips from dimos.utils.logging_config import setup_logger @@ -83,11 +83,14 @@ class PointLioConfig(NativeModuleConfig): # Converted to init_pose CLI arg [x, y, z, qx, qy, qz, qw] in model_post_init. mount: Pose = Pose() - # Frame IDs for output messages. "odom" reflects that PointLio provides - # locally-smooth, continuous odometry (no loop-closure jumps). PGO - # publishes the map→odom correction via TF. - frame_id: str = FRAME_ODOM - child_frame_id: str = FRAME_BODY + # frame_id is the header frame for BOTH the point cloud and the odometry + # message (the Mid-360 sensor frame). The TF published by the module is a + # separate odom_parent_frame_id -> odom_frame_id transform. + frame_id: str = "mid360_link" + # TF publish frames (odom -> base_link): the sensor pose expressed as the + # base_link pose in the odom frame. + odom_parent_frame_id: str = FRAME_ODOM + odom_frame_id: str = "base_link" # FAST-LIO internal processing rates msr_freq: float = 50.0 @@ -102,11 +105,6 @@ class PointLioConfig(NativeModuleConfig): sor_mean_k: int = 50 sor_stddev: float = 1.0 - # Global voxel map (disabled when map_freq <= 0) - map_freq: float = 0.0 - map_voxel_size: float = 0.1 - map_max_range: float = 100.0 - # FAST-LIO YAML config (relative to config/ dir, or absolute path) # C++ binary reads YAML directly via yaml-cpp config: Annotated[ @@ -147,7 +145,7 @@ class PointLioConfig(NativeModuleConfig): # init_pose is computed from mount; config is resolved to config_path init_pose: list[float] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] - cli_exclude: frozenset[str] = frozenset({"config", "mount"}) + cli_exclude: frozenset[str] = frozenset({"config", "mount", "odom_parent_frame_id"}) def model_post_init(self, __context: object) -> None: """Resolve config_path and compute init_pose from mount.""" @@ -168,12 +166,11 @@ def model_post_init(self, __context: object) -> None: ] -class PointLio(NativeModule, perception.Lidar, perception.Odometry, mapping.GlobalPointcloud): +class PointLio(NativeModule, perception.Lidar, perception.Odometry): config: PointLioConfig lidar: Out[PointCloud2] odometry: Out[Odometry] - global_map: Out[PointCloud2] @rpc def start(self) -> None: @@ -187,8 +184,8 @@ def start(self) -> None: def _on_odom_for_tf(self, msg: Odometry) -> None: self.tf.publish( Transform( - frame_id=FRAME_ODOM, - child_frame_id=FRAME_BODY, + frame_id=self.config.odom_parent_frame_id, + child_frame_id=self.config.odom_frame_id, translation=Vector3( msg.pose.position.x, msg.pose.position.y, diff --git a/dimos/hardware/sensors/lidar/pointlio/pointlio_blueprints.py b/dimos/hardware/sensors/lidar/pointlio/pointlio_blueprints.py index 7a413404f4..bbd2cc4272 100644 --- a/dimos/hardware/sensors/lidar/pointlio/pointlio_blueprints.py +++ b/dimos/hardware/sensors/lidar/pointlio/pointlio_blueprints.py @@ -22,7 +22,7 @@ mid360_pointlio = autoconnect( - PointLio.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=-1), + PointLio.blueprint(voxel_size=voxel_size), vis_module("rerun"), ).global_config(n_workers=2, robot_model="mid360_pointlio") diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py index d81ddab5d2..2055b0a0e1 100644 --- a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py @@ -264,7 +264,6 @@ def _run(args: argparse.Namespace) -> int: blueprint = autoconnect( PointLio.blueprint( frame_id="world", - map_freq=-1, odom_freq=args.odom_freq, replay_pcap=pcap_path, deterministic_clock=True, From 8bb723bb23729b5f060d06bb126829c08d72f725 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sat, 13 Jun 2026 07:22:25 -0700 Subject: [PATCH 056/185] feat(virtual_mid360): Python NativeModule wrapper for blueprints --- .../lidar/livox/virtual_mid360/module.py | 68 +++++++++++++++++++ 1 file changed, 68 insertions(+) create mode 100644 dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py new file mode 100644 index 0000000000..d50cf2cd9a --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py @@ -0,0 +1,68 @@ +# 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. + +"""Python NativeModule wrapper for the virtual_mid360 Rust binary. + +virtual_mid360 replays a recorded Livox Mid-360 pcap onto a virtual network +interface, rewriting packet timestamps to current-time and synthesizing the +Livox SDK2 control protocol so an unmodified consumer (e.g. PointLio) connects +to it as if it were a real sensor. It carries no dimos streams; it speaks the +Livox wire protocol over UDP, so consumers reach it by host_ip/lidar_ip, not by +stream wiring. + +Usage:: + + from dimos.hardware.sensors.lidar.livox.virtual_mid360.module import VirtualMid360 + from dimos.hardware.sensors.lidar.pointlio.module import PointLio + from dimos.core.coordination.blueprints import autoconnect + + autoconnect( + VirtualMid360.blueprint(pcap="/path/to/ruwik2.pcap"), + PointLio.blueprint(), + ) +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from dimos.core.native_module import NativeModule, NativeModuleConfig + + +class VirtualMid360Config(NativeModuleConfig): + cwd: str | None = "." + executable: str = "result/bin/virtual_mid360" + build_command: str | None = "nix build .#default" + + # Recorded Mid-360 pcap (point/IMU/status UDP). Read fully into RAM. + pcap: str = "" + # Replay-speed multiplier; 1.0 = original inter-packet timing. + rate: float = 1.0 + # Seconds to wait after start before streaming begins. + delay: float = 0.0 + # IP the fake lidar sends from (must be on this netns's veth). + lidar_ip: str = "192.168.1.155" + # Host IP the recorded data is delivered to (where the SDK listens). + host_ip: str = "192.168.1.5" + # Network namespace the fake lidar runs inside. + lidar_netns: str = "lidar" + + +class VirtualMid360(NativeModule): + config: VirtualMid360Config + + +# Verify the module constructs (mirrors the pointlio wrapper's check). +if TYPE_CHECKING: + VirtualMid360() From 32757d6eec9f827272046bd95714f1dc63ca80ed Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sat, 13 Jun 2026 07:32:39 -0700 Subject: [PATCH 057/185] feat(virtual_mid360): add demo_virtual_mid360_pointlio blueprint + register --- .../lidar/livox/virtual_mid360/blueprints.py | 41 +++++++++++++++++++ dimos/robot/all_blueprints.py | 2 + 2 files changed, 43 insertions(+) create mode 100644 dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py new file mode 100644 index 0000000000..0ab2bf5379 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.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. + +"""Blueprint: PointLio fed by a VirtualMid360 replaying a recorded pcap. + +VirtualMid360 stands up a fake Mid-360 on a virtual NIC and replays the pcap +over the Livox wire protocol; PointLio connects to it exactly as it would to +real hardware (no replay_pcap — it runs in live SDK mode and never knows the +sensor is synthetic). Use this to re-run a recorded session through the live +SLAM path, e.g. to confirm a clip does not diverge. + +The two talk over UDP on lidar_ip/host_ip, so they need a network where those +IPs are reachable (the e2e harness runs VirtualMid360 in a `lidar` netns and +PointLio in a `drv` netns joined by a veth carrying lidar_ip). +""" + +from dimos.core.coordination.blueprints import autoconnect +from dimos.hardware.sensors.lidar.livox.virtual_mid360.module import VirtualMid360 +from dimos.hardware.sensors.lidar.pointlio.module import PointLio +from dimos.utils.data import get_data +from dimos.visualization.vis_module import vis_module + +# Default sample: the ruwik2_part3 clip (LFS); override pcap= for other captures. +_RUWIK_PCAP = "ruwik2_part3/ruwik2_part3.pcap" + +demo_virtual_mid360_pointlio = autoconnect( + VirtualMid360.blueprint(pcap=str(get_data(_RUWIK_PCAP))), + PointLio.blueprint(), + vis_module("rerun"), +).global_config(n_workers=3, robot_model="virtual_mid360_pointlio") diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 55a5e3209f..963f5ecb22 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -54,6 +54,7 @@ "demo-object-scene-registration": "dimos.perception.demo_object_scene_registration:demo_object_scene_registration", "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", + "demo-virtual-mid360-pointlio": "dimos.hardware.sensors.lidar.livox.virtual_mid360.blueprints:demo_virtual_mid360_pointlio", "desk-marker-tf": "dimos.perception.fiducial.blueprints.desk_marker_tf:desk_marker_tf", "drone-agentic": "dimos.robot.drone.blueprints.agentic.drone_agentic:drone_agentic", "drone-basic": "dimos.robot.drone.blueprints.basic.drone_basic:drone_basic", @@ -236,6 +237,7 @@ "unitree-skill-container": "dimos.robot.unitree.unitree_skill_container.UnitreeSkillContainer", "unity-bridge-module": "dimos.simulation.unity.module.UnityBridgeModule", "video-arm-teleop-module": "dimos.teleop.quest.quest_extensions.VideoArmTeleopModule", + "virtual-mid360": "dimos.hardware.sensors.lidar.livox.virtual_mid360.module.VirtualMid360", "vlm-agent": "dimos.agents.vlm_agent.VLMAgent", "vlm-stream-tester": "dimos.agents.vlm_stream_tester.VlmStreamTester", "voxel-grid-mapper": "dimos.mapping.voxels.VoxelGridMapper", From 4570885a3d5423fe6c5c21a93baeba3536404eb7 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sat, 13 Jun 2026 09:20:17 -0700 Subject: [PATCH 058/185] fix(virtual_mid360): don't fetch LFS pcap at blueprint import MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit get_data() at module level triggered a git-LFS download during blueprint validation (test_blueprint_is_valid), which CI blocks via git-lfs-guard — failing the whole test matrix. Default pcap to empty; resolve the capture path at run time instead. --- .../sensors/lidar/livox/virtual_mid360/blueprints.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py index 0ab2bf5379..93f7f2ac14 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py @@ -28,14 +28,15 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.hardware.sensors.lidar.livox.virtual_mid360.module import VirtualMid360 from dimos.hardware.sensors.lidar.pointlio.module import PointLio -from dimos.utils.data import get_data from dimos.visualization.vis_module import vis_module -# Default sample: the ruwik2_part3 clip (LFS); override pcap= for other captures. -_RUWIK_PCAP = "ruwik2_part3/ruwik2_part3.pcap" - +# Set pcap to a recorded Mid-360 capture before running, e.g. the ruwik2_part3 +# LFS sample: --VirtualMid360.pcap "$(python -c 'from dimos.utils.data import +# get_data; print(get_data("ruwik2_part3/ruwik2_part3.pcap"))')" +# (Resolved at run time, not import time, so registering this blueprint never +# triggers an LFS pull.) demo_virtual_mid360_pointlio = autoconnect( - VirtualMid360.blueprint(pcap=str(get_data(_RUWIK_PCAP))), + VirtualMid360.blueprint(pcap=""), PointLio.blueprint(), vis_module("rerun"), ).global_config(n_workers=3, robot_model="virtual_mid360_pointlio") From 3aff05f646874e817a41a959b0daa62863856d5d Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 12 Jun 2026 17:29:38 -0700 Subject: [PATCH 059/185] fastlio2: offline pcap replay + pcap_to_db tool (mirror pointlio) Port the minimal pcap-replay subsystem from jeff/feat/go2_record into the clean branch so FAST-LIO can run offline from a Mid-360 pcap, matching the Point-LIO pcap_to_db workflow: - cpp: pcap_replay.hpp + timing.hpp (header-only), main.cpp refactored so the main loop runs from either the live SDK or a pcap feeder thread, with an optional deterministic sensor-clock mode. Keeps the clean branch's velocity-cap (guarded set_max_velocity_norm_ms) and flake (velocity-cap fast-lio); does not pull go2_record's tcpdump record path. - module.py: replay_pcap / replay_skip_until_ns / first_packet_marker / deterministic_clock config fields; skip network validation in replay mode. - tools/pcap_to_db.py: replay a pcap through FastLio2 (real-time, non- deterministic) and append fastlio_odometry + fastlio_lidar into an existing memory2 db, time-aligned onto its clock. --force overwrites. --- .../sensors/lidar/fastlio2/cpp/main.cpp | 432 ++++++++++++++---- .../lidar/fastlio2/cpp/pcap_replay.hpp | 222 +++++++++ .../sensors/lidar/fastlio2/cpp/timing.hpp | 128 ++++++ .../hardware/sensors/lidar/fastlio2/module.py | 20 +- .../lidar/fastlio2/tools/pcap_to_db.py | 361 +++++++++++++++ 5 files changed, 1085 insertions(+), 78 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/pcap_replay.hpp create mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/timing.hpp create mode 100644 dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index 5c53381aa3..1805945451 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -32,6 +33,8 @@ #include "cloud_filter.hpp" #include "dimos_native_module.hpp" +#include "pcap_replay.hpp" +#include "timing.hpp" #include "voxel_map.hpp" // dimos LCM message headers @@ -59,6 +62,79 @@ static std::atomic g_running{true}; static lcm::LCM* g_lcm = nullptr; static FastLio* g_fastlio = nullptr; +// Virtual clock: in replay mode, tracks the pcap timestamp of the packet +// currently being fed so publish_*() reports the original capture time +// instead of replay wall time. Live mode leaves it at 0 and publish_*() +// falls back to system_clock::now(). +static std::atomic g_replay_mode{false}; +static std::atomic g_virtual_clock_ns{0}; + +// Deterministic clock mode. When set, both live and replay drive +// g_virtual_clock_ns from the packet's sensor-clock pkt->timestamp (which +// is identical bit-for-bit between SDK delivery and pcap), and use it as +// the source for scan-boundary rate limits and publish timestamps. This +// removes wall-clock jitter from scan boundaries → live and replay produce +// the same algorithm state. Trade-off: published header.stamp values +// become sensor-boot-relative seconds instead of unix wall time, so this +// is off by default and only flipped on by the record/replay demos. +static std::atomic g_deterministic_clock{false}; + +// First packet's sensor ts (deterministic mode only). Used to seed the +// main loop's rate-limit bookmarks at exactly the first delivered packet, +// independent of when the main loop's first iteration happens to run. +static std::atomic g_first_packet_clock_ns{0}; + +// First-packet marker. Used by record/replay tooling to align the SDK's +// warmup-induced packet drop with replay. The C++ binary writes the wall +// clock of the first on_point_cloud / on_imu_data callback (live mode +// only) to this file; demo_replay reads it back and passes the value as +// --replay_skip_until_ns so pcap_replay drops the same SDK-eaten prefix. +static std::string g_first_packet_marker_path; +static std::atomic g_first_packet_marker_written{false}; + +// The packet's sensor-clock timestamp (pkt->timestamp) is identical bit-for-bit +// between the live SDK delivery path and the recorded pcap, so writing it from +// the first SDK callback gives replay an exact boundary to skip on. Wall clock +// would only let us match within delivery latency (sub-ms). +static void mark_first_packet(uint64_t pkt_timestamp_ns) { + if (g_first_packet_marker_path.empty()) { + return; + } + bool expected = false; + if (!g_first_packet_marker_written.compare_exchange_strong(expected, true)) { + return; + } + FILE* f = std::fopen(g_first_packet_marker_path.c_str(), "w"); + if (f) { + std::fprintf(f, "%lu\n", static_cast(pkt_timestamp_ns)); + std::fclose(f); + } +} + +static double get_publish_ts() { + if (g_deterministic_clock.load() || g_replay_mode.load()) { + return static_cast(g_virtual_clock_ns.load()) / 1e9; + } + return std::chrono::duration( + std::chrono::system_clock::now().time_since_epoch()).count(); +} + +// Virtualized clock for the main loop's frame/publish rate limiters. In +// replay mode this returns a time_point derived from g_virtual_clock_ns so +// scan boundaries are determined by packet arrival, not by wall-clock thread +// scheduling jitter. Returns nullopt if replay hasn't yet seen a packet +// (caller should skip rate-limit checks in that case). +static std::optional virtual_now() { + if (g_deterministic_clock.load() || g_replay_mode.load()) { + uint64_t ns = g_virtual_clock_ns.load(); + if (ns == 0) { + return std::nullopt; + } + return std::chrono::steady_clock::time_point(std::chrono::nanoseconds(ns)); + } + return std::chrono::steady_clock::now(); +} + static std::string g_lidar_topic; static std::string g_odometry_topic; static std::string g_map_topic; @@ -257,6 +333,7 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times g_lcm->publish(g_odometry_topic, &msg); } + // --------------------------------------------------------------------------- // Livox SDK callbacks // --------------------------------------------------------------------------- @@ -268,8 +345,25 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ uint64_t ts_ns = get_timestamp_ns(data); uint16_t dot_num = data->dot_num; + if (!g_replay_mode.load()) { + mark_first_packet(ts_ns); + } + std::lock_guard lock(g_pc_mutex); + // Update the deterministic-mode virtual clock INSIDE the accumulator + // mutex so the main loop can never observe a clock advance without + // also seeing the matching points (race that drifts scan composition). + // Monotonic update: SDK threads can deliver packets slightly out of + // sensor-ts order, and we must not let a later store(lower_ts) undo + // a previous store(higher_ts). + if (g_deterministic_clock.load()) { + uint64_t expected = 0; + g_first_packet_clock_ns.compare_exchange_strong(expected, ts_ns); + uint64_t cur = g_virtual_clock_ns.load(); + while (cur < ts_ns && !g_virtual_clock_ns.compare_exchange_weak(cur, ts_ns)) {} + } + if (!g_frame_has_timestamp) { g_frame_start_ns = ts_ns; g_frame_has_timestamp = true; @@ -308,7 +402,12 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, LivoxLidarEthernetPacket* data, void* /*client_data*/) { if (!g_running.load() || data == nullptr || !g_fastlio) return; - double ts = static_cast(get_timestamp_ns(data)) / 1e9; + uint64_t pkt_ts_ns = get_timestamp_ns(data); + if (!g_replay_mode.load()) { + mark_first_packet(pkt_ts_ns); + } + + double ts = static_cast(pkt_ts_ns) / 1e9; auto* imu_pts = reinterpret_cast(data->data); uint16_t dot_num = data->dot_num; @@ -339,6 +438,18 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, g_fastlio->feed_imu(imu_msg); } + + // Advance the deterministic-mode virtual clock AFTER feed_imu has + // queued the sample, holding g_pc_mutex so this is fully serialized + // with on_point_cloud / the main-loop scan swap. Monotonic CAS so + // out-of-order SDK delivery can't roll the clock backward. + if (g_deterministic_clock.load()) { + std::lock_guard lock(g_pc_mutex); + uint64_t expected = 0; + g_first_packet_clock_ns.compare_exchange_strong(expected, pkt_ts_ns); + uint64_t cur = g_virtual_clock_ns.load(); + while (cur < pkt_ts_ns && !g_virtual_clock_ns.compare_exchange_weak(cur, pkt_ts_ns)) {} + } } static void on_info_change(const uint32_t handle, const LivoxLidarInfo* info, @@ -395,6 +506,14 @@ int main(int argc, char** argv) { double msr_freq = mod.arg_float("msr_freq", 50.0f); double main_freq = mod.arg_float("main_freq", 5000.0f); + // Post-IESKF-update velocity cap. When |v_world| exceeds this value + // the EKF state is restored to the last accepted scan with vel=0 and + // map_incremental is skipped. Breaks the reinforcing-loop divergence + // that gives FAST-LIO multi-km/s velocity runaway on aggressive + // motion or large IMU gaps. Zero disables. Defaults sized to the + // Go2 quadruped envelope (~3.1 m/s); raise for faster platforms. + double max_velocity_norm_ms = mod.arg_float("max_velocity_norm_ms", 0.0f); + // Livox hardware config std::string host_ip = mod.arg("host_ip", "192.168.1.5"); std::string lidar_ip = mod.arg("lidar_ip", "192.168.1.155"); @@ -430,6 +549,34 @@ int main(int argc, char** argv) { ports.host_imu_data = mod.arg_int("host_imu_data_port", port_defaults.host_imu_data); ports.host_log_data = mod.arg_int("host_log_data_port", port_defaults.host_log_data); + // Replay mode (offline). When --replay_pcap is given the SDK is not + // initialized; a feeder thread reads the pcap and calls the existing + // on_point_cloud / on_imu_data callbacks directly. publish_*() uses + // the pcap timestamps as the clock so outputs match the original run. + std::string replay_pcap = mod.arg("replay_pcap", ""); + g_replay_mode.store(!replay_pcap.empty()); + + // Drop pcap packets with pcap_ts < this value. Used in replay to mimic + // the SDK warmup discard that the live run experienced — so the + // algorithm starts from the same first packet in both modes. + uint64_t replay_skip_until_ns = 0; + { + std::string s = mod.arg("replay_skip_until_ns", "0"); + if (!s.empty()) { + replay_skip_until_ns = std::stoull(s); + } + } + + // Live mode: write the wall_clock_ns of the first SDK callback to this + // file. Pair with replay's --replay_skip_until_ns to align packet sets. + g_first_packet_marker_path = mod.arg("first_packet_marker", ""); + + // Drive virtual_now() and get_publish_ts() off the packet's sensor + // clock in both live and replay. Eliminates wall-clock jitter from + // scan boundaries and makes outputs bit-comparable across modes. + // Changes header.stamp from unix wall time to sensor-boot seconds. + g_deterministic_clock.store(mod.arg_bool("deterministic_clock", false)); + // Initial pose offset [x, y, z, qx, qy, qz, qw] { std::string init_str = mod.arg("init_pose", ""); @@ -490,112 +637,158 @@ int main(int argc, char** argv) { g_fastlio = &fast_lio; if (debug) printf("[fastlio2] FAST-LIO initialized.\n"); - // Init Livox SDK (in-memory config, no temp files). - // Pass `debug` so the SDK's spdlog console sink is disabled when off. - if (!livox_common::init_livox_sdk(host_ip, lidar_ip, ports, debug)) { - return 1; - } - - // Register SDK callbacks - SetLivoxLidarPointCloudCallBack(on_point_cloud, nullptr); - SetLivoxLidarImuDataCallback(on_imu_data, nullptr); - SetLivoxLidarInfoChangeCallback(on_info_change, nullptr); - - // Start SDK - if (!LivoxLidarSdkStart()) { - fprintf(stderr, "Error: LivoxLidarSdkStart failed\n"); - LivoxLidarSdkUninit(); - return 1; - } - - if (debug) printf("[fastlio2] SDK started, waiting for device...\n"); - - // Main loop + // Main-loop state. The body lives in `run_main_iter` below so it can be + // invoked from either the wall-clock-paced main thread (live) or the + // pcap-paced feeder thread (replay), with no race on accumulator + // contents in the replay case. auto frame_interval = std::chrono::microseconds( static_cast(1e6 / g_frequency)); - auto last_emit = std::chrono::steady_clock::now(); + std::optional last_emit; const double process_period_ms = 1000.0 / main_freq; - // Rate limiters for output publishing auto pc_interval = std::chrono::microseconds( static_cast(1e6 / pointcloud_freq)); auto odom_interval = std::chrono::microseconds( static_cast(1e6 / odom_freq)); - auto last_pc_publish = std::chrono::steady_clock::now(); - auto last_odom_publish = std::chrono::steady_clock::now(); + std::optional last_pc_publish; + std::optional last_odom_publish; - // Global voxel map (only if map topic is configured AND map_freq > 0) std::unique_ptr global_map; std::chrono::microseconds map_interval{0}; - auto last_map_publish = std::chrono::steady_clock::now(); + std::optional last_map_publish; if (!g_map_topic.empty() && map_freq > 0.0f) { global_map = std::make_unique(map_voxel_size, map_max_range); map_interval = std::chrono::microseconds( static_cast(1e6 / map_freq)); } - while (g_running.load()) { - auto loop_start = std::chrono::high_resolution_clock::now(); - - // At frame rate: build CustomMsg from accumulated points and feed to FAST-LIO - auto now = std::chrono::steady_clock::now(); - if (now - last_emit >= frame_interval) { - std::vector points; - uint64_t frame_start = 0; + // Per-section timing counters for `run_main_iter`. Active only when + // --debug is on (Scope's constructor reads `fastlio_debug` and no-ops + // otherwise). `timing::maybe_flush(now)` at the bottom prints a per- + // section summary every second of wall clock so we can see both how + // often each part fires and how long each call takes. + static timing::Section t_iter{"run_main_iter"}; + static timing::Section t_emit_check{"emit.lock+swap"}; + static timing::Section t_feed_lidar{"fast_lio.feed_lidar"}; + static timing::Section t_process{"fast_lio.process"}; + static timing::Section t_get_world_cloud{"fast_lio.get_world_cloud"}; + static timing::Section t_filter_cloud{"filter_cloud"}; + static timing::Section t_publish_lidar{"publish_lidar"}; + static timing::Section t_map_insert{"global_map.insert"}; + static timing::Section t_map_publish{"global_map.publish"}; + static timing::Section t_publish_odom{"publish_odometry"}; + + auto run_main_iter = [&](std::chrono::steady_clock::time_point now) { + timing::Scope iter_scope(t_iter); + // Lazy-seed all rate-limit bookmarks on the first iteration so they + // line up with the chosen clock (wall in live, pcap in replay) and + // don't fire immediately based on an arbitrary "since program start" + // delta. In deterministic mode we seed from the FIRST packet's + // sensor ts (not the current ts) so both live and replay anchor + // their first scan boundary at the same packet — required for + // bit-for-bit live↔replay parity. + auto seed = now; + if (g_deterministic_clock.load()) { + uint64_t first = g_first_packet_clock_ns.load(); + if (first != 0) { + seed = std::chrono::steady_clock::time_point( + std::chrono::nanoseconds(first)); + } + } + if (!last_emit.has_value()) { + last_emit = seed; + } + if (!last_pc_publish.has_value()) { + last_pc_publish = seed; + } + if (!last_odom_publish.has_value()) { + last_odom_publish = seed; + } + if (global_map && !last_map_publish.has_value()) { + last_map_publish = seed; + } - { - std::lock_guard lock(g_pc_mutex); + // At frame rate: drain accumulated raw points into a CustomMsg + // and feed to FAST-LIO. Hold g_pc_mutex across the rate-limit + // CHECK as well as the swap, so in deterministic mode the + // virtual clock + accumulator are observed atomically with no + // other thread able to slip a packet in between the decision + // and the swap. + std::vector points; + uint64_t frame_start = 0; + { + timing::Scope s(t_emit_check); + std::lock_guard lock(g_pc_mutex); + auto check_now = now; + if (g_deterministic_clock.load()) { + uint64_t ns = g_virtual_clock_ns.load(); + if (ns != 0) { + check_now = std::chrono::steady_clock::time_point( + std::chrono::nanoseconds(ns)); + } + } + if (check_now - *last_emit >= frame_interval) { if (!g_accumulated_points.empty()) { points.swap(g_accumulated_points); frame_start = g_frame_start_ns; g_frame_has_timestamp = false; } + last_emit = check_now; } - - if (!points.empty()) { - // Build CustomMsg - auto lidar_msg = boost::make_shared(); - lidar_msg->header.seq = 0; - lidar_msg->header.stamp = custom_messages::Time().fromSec( - static_cast(frame_start) / 1e9); - lidar_msg->header.frame_id = "livox_frame"; - lidar_msg->timebase = frame_start; - lidar_msg->lidar_id = 0; - for (int i = 0; i < 3; i++) - lidar_msg->rsvd[i] = 0; - lidar_msg->point_num = static_cast(points.size()); - lidar_msg->points = std::move(points); - - fast_lio.feed_lidar(lidar_msg); - } - - last_emit = now; + } + if (!points.empty()) { + // Build CustomMsg + auto lidar_msg = boost::make_shared(); + lidar_msg->header.seq = 0; + lidar_msg->header.stamp = custom_messages::Time().fromSec( + static_cast(frame_start) / 1e9); + lidar_msg->header.frame_id = "livox_frame"; + lidar_msg->timebase = frame_start; + lidar_msg->lidar_id = 0; + for (int i = 0; i < 3; i++) lidar_msg->rsvd[i] = 0; + lidar_msg->point_num = static_cast(points.size()); + lidar_msg->points = std::move(points); + timing::Scope s(t_feed_lidar); + fast_lio.feed_lidar(lidar_msg); } - // Run FAST-LIO processing step (high frequency) - fast_lio.process(); + // Run one FAST-LIO IESKF step. Cheap when the IMU/lidar queues + // are empty; the heavy work happens after a feed_lidar above. + { + timing::Scope s(t_process); + fast_lio.process(); + } - // Check for new results and accumulate/publish (rate-limited) + // Check for new SLAM results and publish (rate-limited). auto pose = fast_lio.get_pose(); if (!pose.empty() && (pose[0] != 0.0 || pose[1] != 0.0 || pose[2] != 0.0)) { - double ts = std::chrono::duration( - std::chrono::system_clock::now().time_since_epoch()).count(); - - auto world_cloud = fast_lio.get_world_cloud(); + double ts = get_publish_ts(); + auto world_cloud = ([&]() { + timing::Scope s(t_get_world_cloud); + return fast_lio.get_world_cloud(); + })(); if (world_cloud && !world_cloud->empty()) { - auto filtered = filter_cloud(world_cloud, filter_cfg); - - // Per-scan publish at pointcloud_freq - if (!g_lidar_topic.empty() && now - last_pc_publish >= pc_interval) { + auto filtered = ([&]() { + timing::Scope s(t_filter_cloud); + return filter_cloud(world_cloud, filter_cfg); + })(); + + // Per-scan world-frame cloud at pointcloud_freq. + if (!g_lidar_topic.empty() && now - *last_pc_publish >= pc_interval) { + timing::Scope s(t_publish_lidar); publish_lidar(filtered, ts); last_pc_publish = now; } - // Global map: insert, prune, and publish at map_freq + // Global voxel map: insert this scan, prune, then publish + // a snapshot at map_freq. if (global_map) { - global_map->insert(filtered); - - if (now - last_map_publish >= map_interval) { + { + timing::Scope s(t_map_insert); + global_map->insert(filtered); + } + if (now - *last_map_publish >= map_interval) { + timing::Scope s(t_map_publish); global_map->prune( static_cast(pose[0]), static_cast(pose[1]), @@ -607,17 +800,97 @@ int main(int argc, char** argv) { } } - // Publish odometry (rate-limited to odom_freq) - if (!g_odometry_topic.empty() && (now - last_odom_publish >= odom_interval)) { + // Pose + covariance, rate-limited to odom_freq. + if (!g_odometry_topic.empty() && now - *last_odom_publish >= odom_interval) { + timing::Scope s(t_publish_odom); publish_odometry(fast_lio.get_odometry(), ts); last_odom_publish = now; } } - // Handle LCM messages + // Periodic per-section summary to stderr (no-op when --debug off). + timing::maybe_flush(std::chrono::steady_clock::now()); + }; + + // Source of point/IMU packets: + // live mode -> Livox SDK opens UDP sockets + dispatches via callbacks + // from its own threads. Main thread drives run_main_iter + // at main_freq, consuming whatever the SDK queued. + // replay mode -> the feeder thread reads the pcap and pushes packets + // through the same on_point/on_imu callbacks (paced at + // realtime via sleep_until). The MAIN thread — same + // one that runs in live mode — owns run_main_iter and + // drains the accumulator. Two threads in both modes, + // matching architectures, so the only difference is + // the source of packets (SDK vs pcap). + std::thread replay_thread; + if (g_replay_mode.load()) { + if (debug) printf("[fastlio2] REPLAY mode, pcap=%s\n", replay_pcap.c_str()); + replay_thread = std::thread([&]() { + pcap_replay::Replayer rep; + rep.path = replay_pcap; + rep.host_point_port = static_cast(ports.host_point_data); + rep.host_imu_port = static_cast(ports.host_imu_data); + rep.on_point = [](LivoxLidarEthernetPacket* p) { + on_point_cloud(0, 0, p, nullptr); + }; + rep.on_imu = [](LivoxLidarEthernetPacket* p) { + on_imu_data(0, 0, p, nullptr); + }; + rep.on_clock = [](uint64_t pcap_ts_ns) { + // In deterministic mode the callbacks already pushed the + // sensor pkt->timestamp into g_virtual_clock_ns — don't + // overwrite with the pcap (wall) ts. + if (g_deterministic_clock.load()) { + return; + } + g_virtual_clock_ns.store(pcap_ts_ns); + }; + // No rep.on_iter — the main thread drives run_main_iter in + // replay mode now, same as in live. This decouples packet + // ingestion from per-iter filter_cloud cost and lets replay + // run at the same wall throughput as live. + rep.running = &g_running; + // Pace the replay feeder at live wall-clock rate. sleep_until + // throttles the feeder so packets land in the accumulator at + // the same wall cadence as the SDK delivers in live mode. + rep.realtime = true; + rep.skip_until_ns = replay_skip_until_ns; + rep.run(); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + g_running.store(false); + }); + } else { + if (!livox_common::init_livox_sdk(host_ip, lidar_ip, ports, debug)) { + return 1; + } + SetLivoxLidarPointCloudCallBack(on_point_cloud, nullptr); + SetLivoxLidarImuDataCallback(on_imu_data, nullptr); + SetLivoxLidarInfoChangeCallback(on_info_change, nullptr); + if (!LivoxLidarSdkStart()) { + fprintf(stderr, "Error: LivoxLidarSdkStart failed\n"); + LivoxLidarSdkUninit(); + return 1; + } + if (debug) printf("[fastlio2] SDK started, waiting for device...\n"); + } + + while (g_running.load()) { + auto loop_start = std::chrono::high_resolution_clock::now(); + auto now_opt = virtual_now(); + if (!now_opt.has_value()) { + // No clock yet — in replay this means the feeder hasn't read + // its first packet; in live it shouldn't happen because + // virtual_now falls back to steady_clock::now(). + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + continue; + } + run_main_iter(*now_opt); + + // Drain LCM messages. lcm.handleTimeout(0); - // Rate control (~5kHz processing) + // Rate control (~main_freq, 5kHz default). auto loop_end = std::chrono::high_resolution_clock::now(); auto elapsed_ms = std::chrono::duration(loop_end - loop_start).count(); if (elapsed_ms < process_period_ms) { @@ -629,7 +902,12 @@ int main(int argc, char** argv) { // Cleanup if (debug) printf("[fastlio2] Shutting down...\n"); g_fastlio = nullptr; - LivoxLidarSdkUninit(); + if (replay_thread.joinable()) { + replay_thread.join(); + } + if (!g_replay_mode.load()) { + LivoxLidarSdkUninit(); + } g_lcm = nullptr; if (debug) printf("[fastlio2] Done.\n"); diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/pcap_replay.hpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/pcap_replay.hpp new file mode 100644 index 0000000000..cfbc0e58c1 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/pcap_replay.hpp @@ -0,0 +1,222 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Read a pcap of recorded Mid-360 UDP traffic and feed each point/imu +// payload to the existing SDK callbacks. Used by `--replay_pcap` to bypass +// the Livox SDK for deterministic offline regression testing. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "livox_lidar_def.h" + +namespace pcap_replay { + +constexpr uint32_t PCAP_MAGIC_LE_US = 0xa1b2c3d4u; +constexpr uint32_t PCAP_MAGIC_LE_NS = 0xa1b23c4du; +constexpr uint32_t LINKTYPE_ETHERNET = 1u; +constexpr uint16_t ETHERTYPE_IPV4 = 0x0800u; +constexpr uint8_t IPPROTO_UDP = 17u; +constexpr size_t ETH_HDR_LEN = 14; +constexpr size_t IP_MIN_HDR_LEN = 20; +constexpr size_t UDP_HDR_LEN = 8; +constexpr size_t LIVOX_ETH_HDR_LEN = 36; + +using PacketCb = std::function; +using ClockCb = std::function; +using IterCb = std::function; + +struct Replayer { + std::string path; + uint16_t host_point_port = 0; + uint16_t host_imu_port = 0; + PacketCb on_point; + PacketCb on_imu; + ClockCb on_clock; + // Called synchronously after every packet, once the payload has been + // appended and the virtual clock advanced. The replay path runs the + // main-loop body here so feeding + processing happen on a single + // thread — eliminates the feeder-vs-main-loop race on accumulator + // contents. + IterCb on_iter; + std::atomic* running = nullptr; + bool realtime = true; + // Drop Livox packets whose sensor timestamp (pkt->timestamp) is + // strictly less than this. Used to mimic the SDK warmup window from a + // paired live run so the algorithm starts from the same first packet + // in both modes. Comparing on sensor ts (which is identical bit-for-bit + // between live SDK delivery and pcap replay) is exact; comparing on + // wall pcap_ts would be off by SDK delivery latency. + uint64_t skip_until_ns = 0; + + bool run() { + std::ifstream f(path, std::ios::binary); + if (!f) { + fprintf(stderr, "[replay] cannot open %s\n", path.c_str()); + return false; + } + + uint8_t global_hdr[24]; + f.read(reinterpret_cast(global_hdr), 24); + if (!f) { + fprintf(stderr, "[replay] short read on pcap global header\n"); + return false; + } + uint32_t magic; + std::memcpy(&magic, global_hdr, 4); + const bool nanos = (magic == PCAP_MAGIC_LE_NS); + if (magic != PCAP_MAGIC_LE_US && magic != PCAP_MAGIC_LE_NS) { + fprintf(stderr, "[replay] unsupported pcap magic 0x%08x\n", magic); + return false; + } + uint32_t linktype; + std::memcpy(&linktype, global_hdr + 20, 4); + if (linktype != LINKTYPE_ETHERNET) { + fprintf(stderr, "[replay] unsupported linktype %u (need ETHERNET=1)\n", linktype); + return false; + } + + printf("[replay] reading %s (port=%u imu=%u realtime=%d)\n", + path.c_str(), host_point_port, host_imu_port, realtime ? 1 : 0); + + uint64_t first_pcap_ts_ns = 0; + std::chrono::steady_clock::time_point start_wall; + bool seeded = false; + + size_t pkts = 0, pts = 0, imu = 0, other = 0; + uint8_t rec_hdr[16]; + std::vector buf; + + while (running == nullptr || running->load()) { + f.read(reinterpret_cast(rec_hdr), 16); + if (!f) { + break; + } + + uint32_t ts_sec, ts_sub, incl_len, orig_len; + std::memcpy(&ts_sec, rec_hdr + 0, 4); + std::memcpy(&ts_sub, rec_hdr + 4, 4); + std::memcpy(&incl_len, rec_hdr + 8, 4); + std::memcpy(&orig_len, rec_hdr + 12, 4); + (void)orig_len; + + const uint64_t pcap_ts_ns = + static_cast(ts_sec) * 1'000'000'000ULL + + (nanos ? static_cast(ts_sub) : static_cast(ts_sub) * 1000ULL); + + buf.resize(incl_len); + f.read(reinterpret_cast(buf.data()), incl_len); + if (!f) { + break; + } + pkts++; + + if (buf.size() < ETH_HDR_LEN) { + continue; + } + uint16_t ethertype = (static_cast(buf[12]) << 8) | buf[13]; + if (ethertype != ETHERTYPE_IPV4) { + continue; + } + size_t ip_off = ETH_HDR_LEN; + if (buf.size() < ip_off + IP_MIN_HDR_LEN) { + continue; + } + uint8_t vihl = buf[ip_off]; + if ((vihl >> 4) != 4) { + continue; + } + int ihl = (vihl & 0x0f) * 4; + if (ihl < static_cast(IP_MIN_HDR_LEN)) { + continue; + } + if (buf[ip_off + 9] != IPPROTO_UDP) { + continue; + } + size_t udp_off = ip_off + ihl; + if (buf.size() < udp_off + UDP_HDR_LEN) { + continue; + } + uint16_t dst_port = (static_cast(buf[udp_off + 2]) << 8) | buf[udp_off + 3]; + uint16_t udp_len = (static_cast(buf[udp_off + 4]) << 8) | buf[udp_off + 5]; + size_t payload_off = udp_off + UDP_HDR_LEN; + size_t payload_end = std::min(buf.size(), static_cast(udp_off + udp_len)); + if (payload_end <= payload_off) { + continue; + } + size_t payload_len = payload_end - payload_off; + if (payload_len < LIVOX_ETH_HDR_LEN) { + continue; + } + + auto* livox_pkt = + reinterpret_cast(buf.data() + payload_off); + + // Sensor-clock skip: drop packets the live SDK wouldn't have + // seen (those before its first delivered callback) so the + // algorithm processes the same input set in both modes. + if (skip_until_ns > 0) { + uint64_t pkt_ts; + std::memcpy(&pkt_ts, livox_pkt->timestamp, sizeof(uint64_t)); + if (pkt_ts < skip_until_ns) { + continue; + } + } + + if (realtime) { + if (!seeded) { + first_pcap_ts_ns = pcap_ts_ns; + start_wall = std::chrono::steady_clock::now(); + seeded = true; + } else { + auto target = start_wall + std::chrono::nanoseconds(pcap_ts_ns - first_pcap_ts_ns); + auto now = std::chrono::steady_clock::now(); + if (target > now) { + std::this_thread::sleep_until(target); + } + } + } + + if (dst_port == host_point_port) { + if (on_point) { + on_point(livox_pkt); + } + pts++; + } else if (dst_port == host_imu_port) { + if (on_imu) { + on_imu(livox_pkt); + } + imu++; + } else { + other++; + } + // Advance the virtual clock AFTER the payload has been added to + // accumulators. Reverse order would let the main-loop thread see + // the clock advance and emit a scan that's missing this packet. + if (on_clock) { + on_clock(pcap_ts_ns); + } + + // Run one main-loop iteration synchronously so feeding and + // processing are strictly serialized in replay mode. + if (on_iter) { + on_iter(); + } + } + + printf("[replay] done: %zu pcap records (point=%zu imu=%zu other=%zu)\n", + pkts, pts, imu, other); + return true; + } +}; + +} // namespace pcap_replay diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/timing.hpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/timing.hpp new file mode 100644 index 0000000000..d1fbe8ded4 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/timing.hpp @@ -0,0 +1,128 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Lightweight per-section timing for diagnosing where wall time goes in +// `run_main_iter`. Active only when --debug is on (i.e. the global +// `fastlio_debug` flag is true) so non-debug runs pay only a single +// branch per scope. +// +// Usage: +// +// static timing::Section sec{"filter_cloud"}; +// { +// timing::Scope s(sec); +// // ...do work... +// } +// // and periodically: +// timing::maybe_flush(now); +// +// The flush prints one line per section to stderr every flush interval +// (1 second of wall clock) summarising count / mean / max / total, then +// resets the accumulators. The flush is cheap when nothing was recorded. + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "fast_lio_debug.hpp" // for the global `fastlio_debug` flag + +namespace timing { + +struct Section { + const char* name; + std::atomic count{0}; + std::atomic total_ns{0}; + std::atomic max_ns{0}; + + explicit Section(const char* n); + + void add(uint64_t ns) { + count.fetch_add(1, std::memory_order_relaxed); + total_ns.fetch_add(ns, std::memory_order_relaxed); + uint64_t prev = max_ns.load(std::memory_order_relaxed); + while (ns > prev && + !max_ns.compare_exchange_weak(prev, ns, std::memory_order_relaxed)) { + // prev is updated on failure by compare_exchange_weak. + } + } +}; + +inline std::vector& registry() { + static std::vector r; + return r; +} + +inline Section::Section(const char* n) : name(n) { + registry().push_back(this); +} + +struct Scope { + Section& sec; + std::chrono::steady_clock::time_point t0; + bool active; + + explicit Scope(Section& s) : sec(s), active(fastlio_debug) { + if (active) { + t0 = std::chrono::steady_clock::now(); + } + } + + ~Scope() { + if (!active) { + return; + } + auto dt = std::chrono::duration_cast( + std::chrono::steady_clock::now() - t0).count(); + sec.add(static_cast(dt)); + } +}; + +// Print one summary line per section to stderr every FLUSH_INTERVAL wall +// seconds, then reset accumulators. The check is cheap: a single time +// comparison guarded by the fastlio_debug flag. The mutex serialises the +// flush between threads (replay's feeder vs live's main loop) so we +// never see torn output. +inline void maybe_flush(std::chrono::steady_clock::time_point now) { + if (!fastlio_debug) { + return; + } + constexpr auto FLUSH_INTERVAL = std::chrono::seconds(1); + static std::mutex mtx; + static std::chrono::steady_clock::time_point last; + std::lock_guard lock(mtx); + if (last.time_since_epoch().count() == 0) { + last = now; + return; + } + if (now - last < FLUSH_INTERVAL) { + return; + } + auto dt_ms = std::chrono::duration(now - last).count(); + last = now; + + for (Section* s : registry()) { + uint64_t c = s->count.exchange(0, std::memory_order_relaxed); + uint64_t tot = s->total_ns.exchange(0, std::memory_order_relaxed); + uint64_t mx = s->max_ns.exchange(0, std::memory_order_relaxed); + if (c == 0) { + std::fprintf(stderr, "[timing] %-24s n=0\n", s->name); + continue; + } + double mean_us = static_cast(tot) / static_cast(c) / 1e3; + double max_us = static_cast(mx) / 1e3; + double total_ms = static_cast(tot) / 1e6; + double rate_hz = static_cast(c) * 1000.0 / dt_ms; + std::fprintf(stderr, + "[timing] %-24s n=%5lu rate=%7.1fHz mean=%8.3fus max=%9.3fus tot=%7.2fms\n", + s->name, + static_cast(c), + rate_hz, mean_us, max_us, total_ms); + } +} + +} // namespace timing diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 7c7ff59ff8..7113a5de3a 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -130,6 +130,23 @@ class FastLio2Config(NativeModuleConfig): # Resolved in __post_init__, passed as --config_path to the binary config_path: str | None = None + # Offline replay. When set, the C++ binary skips SDK init and feeds + # packets from this pcap into the same callbacks the SDK would, with + # publish timestamps driven by the pcap clock. + replay_pcap: Path | None = None + # Replay-only: drop pcap records with pcap_ts < this. Used to mimic the + # SDK warmup window from the paired live run. + replay_skip_until_ns: int | None = None + # Live-only: file path where the C++ binary writes the wall_ns of the + # first SDK callback, so a later replay can align its first packet. + first_packet_marker: Path | None = None + # Drive scan boundaries + publish ts off the sensor packet timestamp in + # both live and replay so they produce bit-for-bit identical outputs. + # Side effect: published header.stamp is sensor-boot seconds, not unix + # wall time. Off by default; only the deterministic record/replay path + # flips it on (real-time replay leaves it False). + deterministic_clock: bool = False + # init_pose is computed from mount; config is resolved to config_path init_pose: list[float] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] cli_exclude: frozenset[str] = frozenset({"config", "mount"}) @@ -162,7 +179,8 @@ class FastLio2(NativeModule, perception.Lidar, perception.Odometry, mapping.Glob @rpc def start(self) -> None: - self._validate_network() + if self.config.replay_pcap is None: + self._validate_network() super().start() self.register_disposable( Disposable(self.odometry.transport.subscribe(self._on_odom_for_tf, self.odometry)) diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py new file mode 100644 index 0000000000..50685f8fec --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -0,0 +1,361 @@ +# 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. + +"""Run FAST-LIO over a .pcap and append its outputs into an existing .db. + +Given a Livox Mid-360 pcap capture and a memory2 SQLite database, this streams +the pcap through the FastLio2 native module and writes two streams into the +database, both time-aligned onto the db's existing clock: + +* ``fastlio_odometry`` -- the IESKF pose at the native odom rate (~30 Hz). +* ``fastlio_lidar`` -- the registered (deskewed, odom-frame) point cloud at the + native pointcloud rate (~10 Hz). + +This mirrors the Point-LIO ``pcap_to_db.py`` tool, with one deliberate +difference: FAST-LIO is *not* bit-deterministic (OpenMP reduction order), so the +replay runs ``deterministic_clock=False`` -- the feeder paces packets at +wall-clock realtime, exactly as the live SDK delivers them, and publish +timestamps come from the pcap's capture clock. A 20-minute recording therefore +takes ~20 minutes of wall time to replay. + +If either stream already exists in the db the run aborts, unless ``--force`` is +given, in which case the existing ``fastlio_odometry`` and ``fastlio_lidar`` +streams are dropped before the new ones are written. + +Timing conversion +----------------- +With ``deterministic_clock=False`` FAST-LIO publishes with the pcap packet +clock, which for a real recording is the original capture's *unix wall time* -- +the same clock the db's other streams already use. So the common case needs no +shift. The offset is auto-derived from the two clocks: + +* db + replay on the same clock family (both wall, or both sensor): offset 0. +* cross-clock (e.g. a deterministic sensor-clock replay into a wall-clock db): + start-align by shifting the replay's first ts onto the db's earliest ts. +* db has no existing timestamped rows: offset 0. + +Pass ``--time-offset`` to override the auto choice. + +Usage (from the dimos5 venv):: + + source .venv/bin/activate + python -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db \ + --pcap /path/to/capture.pcap --db /path/to/memory.db +""" + +from __future__ import annotations + +import argparse +from collections.abc import AsyncIterator +import math +from pathlib import Path +import sqlite3 +import sys +import time + +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +# Below this an absolute timestamp is sensor-boot seconds, not unix wall time. +_SENSOR_CLOCK_MAX = 1e8 +# Strictly-increasing tie-breaker so two samples never collide on ts. +_EPS = 1e-9 +# Poll the db on this cadence while the replay drains the pcap. +_POLL_SEC = 1.0 +# Stop after the odom stream has been stagnant this long (pcap fully drained). +_STAGNANT_SEC = 6.0 +# Go2 quadruped post-update velocity cap (m/s). Breaks the FAST-LIO velocity +# runaway on aggressive motion; the dog cannot physically exceed this, so it +# only ever clamps divergence. Zero disables. See FastLio2Config. +_GO2_MAX_VELOCITY_MS = 3.1 + + +class RecConfig(ModuleConfig): + """Configures the recorder with the target db and timing conversion.""" + + db_path: str = "" + # Earliest existing ts in the db, or -1.0 if the db has no timestamped rows. + ref_start_ts: float = -1.0 + # Explicit offset override; NaN means auto-derive from ref_start_ts. + time_offset: float = float("nan") + + +class Rec(Module): + """Append FAST-LIO odometry + lidar into an existing SQLite db with ts conversion.""" + + config: RecConfig + fastlio_odometry: In[Odometry] + fastlio_lidar: In[PointCloud2] + _offset: float | None = None + _last_odom_ts: float = 0.0 + _last_lidar_ts: float = 0.0 + _last_pose: object = None + _odom_count: int = 0 + _lidar_count: int = 0 + + async def main(self) -> AsyncIterator[None]: + from dimos.memory2.store.sqlite import SqliteStore + + self._store = SqliteStore(path=self.config.db_path) + self._os = self._store.stream("fastlio_odometry", Odometry) + self._ls = self._store.stream("fastlio_lidar", PointCloud2) + yield + self._store.stop() + + def _resolve_offset(self, first_ts: float) -> float: + override = self.config.time_offset + if not math.isnan(override): + return override + ref = self.config.ref_start_ts + if ref < 0.0: + return 0.0 + # Same clock family (both wall, or both sensor) -> already aligned. + # Cross-clock -> start-align the replay's first ts onto the db's first. + if (first_ts > _SENSOR_CLOCK_MAX) == (ref > _SENSOR_CLOCK_MAX): + return 0.0 + return ref - first_ts + + def _aligned_ts(self, raw_ts: float, last_ts: float) -> float: + """Convert a replay ts onto the db clock, kept strictly above last_ts.""" + if self._offset is None: + self._offset = self._resolve_offset(raw_ts) + return max(raw_ts + self._offset, last_ts + _EPS) + + async def handle_fastlio_odometry(self, v: Odometry) -> None: + raw_ts = getattr(v, "ts", None) or time.time() + ts = self._aligned_ts(raw_ts, self._last_odom_ts) + self._last_odom_ts = ts + pose = getattr(v, "pose", None) + self._last_pose = getattr(pose, "pose", None) if pose is not None else None + self._os.append(v, ts=ts, pose=self._last_pose) + self._odom_count += 1 + + async def handle_fastlio_lidar(self, v: PointCloud2) -> None: + raw_ts = getattr(v, "ts", None) or time.time() + ts = self._aligned_ts(raw_ts, self._last_lidar_ts) + self._last_lidar_ts = ts + self._ls.append(v, ts=ts, pose=self._last_pose) + self._lidar_count += 1 + + +def _db_ref_start_ts(db_path: Path) -> float: + """Min ts across the db's existing streams, or -1.0 if none/absent.""" + if not db_path.exists(): + return -1.0 + con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) + try: + tables = [ + row[0] + for row in con.execute("SELECT name FROM sqlite_master WHERE type='table'").fetchall() + ] + best: float | None = None + for table in tables: + if table.startswith("_") or table.startswith("sqlite_"): + continue + try: + # vec0/rtree virtual tables (sqlite-vec etc.) raise "no such + # module" here when the extension isn't loaded -- skip them. + cols = [c[1] for c in con.execute(f"PRAGMA table_info('{table}')").fetchall()] + if "ts" not in cols: + continue + row = con.execute(f"SELECT MIN(ts) FROM '{table}'").fetchone() + except sqlite3.OperationalError: + continue + if row and row[0] is not None: + best = row[0] if best is None else min(best, row[0]) + return best if best is not None else -1.0 + finally: + con.close() + + +def _table_stats(db_path: Path, table: str) -> tuple[int, float, float]: + """(count, min_ts, max_ts) for a stream table; zeros if absent.""" + if not db_path.exists(): + return 0, 0.0, 0.0 + con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) + try: + try: + row = con.execute(f"SELECT COUNT(*), MIN(ts), MAX(ts) FROM '{table}'").fetchone() + except sqlite3.OperationalError: + return 0, 0.0, 0.0 + cnt = row[0] or 0 + return cnt, row[1] or 0.0, row[2] or 0.0 + finally: + con.close() + + +def _run(args: argparse.Namespace) -> int: + pcap_path = Path(args.pcap).expanduser().resolve() + db_path = Path(args.db).expanduser().resolve() + if not pcap_path.exists(): + print(f"[pcap_to_db] missing pcap: {pcap_path}", file=sys.stderr) + return 2 + if args.max_sensor_sec < 0: + print("[pcap_to_db] --max-sensor-sec must be >= 0", file=sys.stderr) + return 2 + + from dimos.core.coordination.blueprints import autoconnect + from dimos.core.coordination.module_coordinator import ModuleCoordinator + from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 + from dimos.memory2.store.sqlite import SqliteStore + + fastlio_streams = ("fastlio_odometry", "fastlio_lidar") + store = SqliteStore(path=str(db_path)) + try: + existing = sorted(set(store.list_streams()) & set(fastlio_streams)) + if existing and not args.force: + print( + f"[pcap_to_db] {db_path.name} already has {existing}; pass --force to overwrite", + file=sys.stderr, + ) + return 2 + for name in existing: + store.delete_stream(name) + if existing: + print(f"[pcap_to_db] --force: dropped existing {existing}", flush=True) + finally: + store.stop() + + ref_start_ts = _db_ref_start_ts(db_path) + time_offset = float("nan") if args.time_offset is None else args.time_offset + if not math.isnan(time_offset): + offset_desc = f"explicit {time_offset:+.3f}s" + elif ref_start_ts < 0.0: + offset_desc = "auto: db empty -> 0" + elif ref_start_ts < _SENSOR_CLOCK_MAX: + offset_desc = f"auto: db sensor-clock (R0={ref_start_ts:.2f})" + else: + offset_desc = f"auto: db wall-clock (R0={ref_start_ts:.2f})" + print( + f"[pcap_to_db] pcap={pcap_path.name} db={db_path.name} " + f"odom_freq={args.odom_freq}Hz vmax={args.max_velocity_norm_ms}m/s offset={offset_desc}", + flush=True, + ) + + fastlio_kwargs: dict[str, object] = dict( + frame_id="world", + map_freq=-1, + odom_freq=args.odom_freq, + max_velocity_norm_ms=args.max_velocity_norm_ms, + replay_pcap=pcap_path, + deterministic_clock=False, + debug=False, + ) + # Omit config to fall back to the module default (config/mid360.yaml). + if args.config: + fastlio_kwargs["config"] = Path(args.config) + fastlio = FastLio2.blueprint(**fastlio_kwargs).remappings( + [ + (FastLio2, "odometry", "fastlio_odometry"), + (FastLio2, "lidar", "fastlio_lidar"), + ] + ) + blueprint = autoconnect( + fastlio, + Rec.blueprint( + db_path=str(db_path), + ref_start_ts=ref_start_ts, + time_offset=time_offset, + ), + ).global_config(n_workers=4, robot_model="mid360_fastlio2_pcap_to_db") + coord = ModuleCoordinator.build(blueprint) + + t0 = time.time() + last_max = 0.0 + first_max: float | None = None + stagnant_since: float | None = None + try: + while True: + time.sleep(_POLL_SEC) + cnt, min_ts, max_ts = _table_stats(db_path, "fastlio_odometry") + if cnt == 0: + continue + if first_max is None: + first_max = min_ts + if args.max_sensor_sec > 0 and (max_ts - first_max) >= args.max_sensor_sec: + print( + f"[pcap_to_db] reached --max-sensor-sec={args.max_sensor_sec:.1f}s", + flush=True, + ) + break + if max_ts == last_max: + if stagnant_since is None: + stagnant_since = time.time() + elif time.time() - stagnant_since > _STAGNANT_SEC: + break + else: + last_max = max_ts + stagnant_since = None + finally: + coord.stop() + + o_cnt, o_min, o_max = _table_stats(db_path, "fastlio_odometry") + l_cnt = _table_stats(db_path, "fastlio_lidar")[0] + span = o_max - o_min + print( + f"[pcap_to_db] done odom={o_cnt} lidar={l_cnt} " + f"ts=[{o_min:.3f}, {o_max:.3f}] span={span:.1f}s " + f"wall={time.time() - t0:.1f}s", + flush=True, + ) + return 0 + + +def main(argv: list[str]) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--pcap", required=True, help="Livox Mid-360 pcap capture") + parser.add_argument("--db", required=True, help="target memory2 SQLite db (appended to)") + parser.add_argument( + "--odom-freq", + type=float, + default=30.0, + help="FAST-LIO odometry publish rate in Hz (default 30)", + ) + parser.add_argument( + "--max-velocity-norm-ms", + type=float, + default=_GO2_MAX_VELOCITY_MS, + help=f"post-update velocity cap in m/s, anti-divergence (default {_GO2_MAX_VELOCITY_MS} " + "for go2; 0 disables)", + ) + parser.add_argument( + "--config", + type=str, + default=None, + help="FAST-LIO yaml (relative to config/ or absolute); omit for the module default", + ) + parser.add_argument( + "--max-sensor-sec", + type=float, + default=0.0, + help="stop after this many seconds of sensor time (0 = whole pcap)", + ) + parser.add_argument( + "--time-offset", + type=float, + default=None, + help="seconds added to every output ts; omit to auto-derive from the db clock", + ) + parser.add_argument( + "--force", + action="store_true", + help="overwrite existing fastlio_odometry/fastlio_lidar streams in the db", + ) + return _run(parser.parse_args(argv)) + + +if __name__ == "__main__": + raise SystemExit(main(sys.argv[1:])) From 6dbb3917fe4dcd1107fe4ccf996faa3f7db1e482 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sat, 13 Jun 2026 16:21:30 -0700 Subject: [PATCH 060/185] fastlio2: pcap_to_db builds a db from scratch + privatize recorder --- .../lidar/fastlio2/tools/pcap_to_db.py | 57 +++++++++++-------- 1 file changed, 34 insertions(+), 23 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index 50685f8fec..478085308b 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -12,16 +12,20 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Run FAST-LIO over a .pcap and append its outputs into an existing .db. +"""Run FAST-LIO over a .pcap and write its outputs into a .db. -Given a Livox Mid-360 pcap capture and a memory2 SQLite database, this streams -the pcap through the FastLio2 native module and writes two streams into the -database, both time-aligned onto the db's existing clock: +Given a Livox Mid-360 pcap capture, this streams the pcap through the FastLio2 +native module and writes two streams into a memory2 SQLite database: * ``fastlio_odometry`` -- the IESKF pose at the native odom rate (~30 Hz). * ``fastlio_lidar`` -- the registered (deskewed, odom-frame) point cloud at the native pointcloud rate (~10 Hz). +The ``--db`` is optional. With no existing db the tool builds one **from +scratch** (omit ``--db`` and it defaults to ``.db`` next to the pcap). +With an existing db the two streams are appended and time-aligned onto the db's +clock, so FAST-LIO output can be compared against whatever it already holds. + This mirrors the Point-LIO ``pcap_to_db.py`` tool, with one deliberate difference: FAST-LIO is *not* bit-deterministic (OpenMP reduction order), so the replay runs ``deterministic_clock=False`` -- the feeder paces packets at @@ -50,6 +54,13 @@ Usage (from the dimos5 venv):: source .venv/bin/activate + + # Build a fresh db from scratch (no existing db needed); defaults to + # .db next to the pcap. + python -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db \ + --pcap /path/to/capture.pcap + + # Or append into an existing recording db for comparison. python -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db \ --pcap /path/to/capture.pcap --db /path/to/memory.db """ @@ -77,13 +88,9 @@ _POLL_SEC = 1.0 # Stop after the odom stream has been stagnant this long (pcap fully drained). _STAGNANT_SEC = 6.0 -# Go2 quadruped post-update velocity cap (m/s). Breaks the FAST-LIO velocity -# runaway on aggressive motion; the dog cannot physically exceed this, so it -# only ever clamps divergence. Zero disables. See FastLio2Config. -_GO2_MAX_VELOCITY_MS = 3.1 -class RecConfig(ModuleConfig): +class _RecConfig(ModuleConfig): """Configures the recorder with the target db and timing conversion.""" db_path: str = "" @@ -93,10 +100,10 @@ class RecConfig(ModuleConfig): time_offset: float = float("nan") -class Rec(Module): +class _Rec(Module): """Append FAST-LIO odometry + lidar into an existing SQLite db with ts conversion.""" - config: RecConfig + config: _RecConfig fastlio_odometry: In[Odometry] fastlio_lidar: In[PointCloud2] _offset: float | None = None @@ -199,13 +206,18 @@ def _table_stats(db_path: Path, table: str) -> tuple[int, float, float]: def _run(args: argparse.Namespace) -> int: pcap_path = Path(args.pcap).expanduser().resolve() - db_path = Path(args.db).expanduser().resolve() if not pcap_path.exists(): print(f"[pcap_to_db] missing pcap: {pcap_path}", file=sys.stderr) return 2 if args.max_sensor_sec < 0: print("[pcap_to_db] --max-sensor-sec must be >= 0", file=sys.stderr) return 2 + # --db is optional: with no existing db, build one from scratch. When + # omitted the output defaults to .db next to the pcap, so a fresh + # db can be generated with just --pcap. + db_path = Path(args.db).expanduser().resolve() if args.db else pcap_path.with_suffix(".db") + db_existed = db_path.exists() + db_path.parent.mkdir(parents=True, exist_ok=True) from dimos.core.coordination.blueprints import autoconnect from dimos.core.coordination.module_coordinator import ModuleCoordinator @@ -241,7 +253,8 @@ def _run(args: argparse.Namespace) -> int: offset_desc = f"auto: db wall-clock (R0={ref_start_ts:.2f})" print( f"[pcap_to_db] pcap={pcap_path.name} db={db_path.name} " - f"odom_freq={args.odom_freq}Hz vmax={args.max_velocity_norm_ms}m/s offset={offset_desc}", + f"({'append' if db_existed else 'new'}) " + f"odom_freq={args.odom_freq}Hz offset={offset_desc}", flush=True, ) @@ -249,7 +262,6 @@ def _run(args: argparse.Namespace) -> int: frame_id="world", map_freq=-1, odom_freq=args.odom_freq, - max_velocity_norm_ms=args.max_velocity_norm_ms, replay_pcap=pcap_path, deterministic_clock=False, debug=False, @@ -265,7 +277,7 @@ def _run(args: argparse.Namespace) -> int: ) blueprint = autoconnect( fastlio, - Rec.blueprint( + _Rec.blueprint( db_path=str(db_path), ref_start_ts=ref_start_ts, time_offset=time_offset, @@ -317,20 +329,19 @@ def _run(args: argparse.Namespace) -> int: def main(argv: list[str]) -> int: parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("--pcap", required=True, help="Livox Mid-360 pcap capture") - parser.add_argument("--db", required=True, help="target memory2 SQLite db (appended to)") + parser.add_argument( + "--db", + default=None, + help="target memory2 SQLite db. If it exists, fastlio streams are appended/aligned " + "onto its clock; if it doesn't, a fresh db is built from scratch. " + "Omit to default to .db next to the pcap.", + ) parser.add_argument( "--odom-freq", type=float, default=30.0, help="FAST-LIO odometry publish rate in Hz (default 30)", ) - parser.add_argument( - "--max-velocity-norm-ms", - type=float, - default=_GO2_MAX_VELOCITY_MS, - help=f"post-update velocity cap in m/s, anti-divergence (default {_GO2_MAX_VELOCITY_MS} " - "for go2; 0 disables)", - ) parser.add_argument( "--config", type=str, From 24c19fae9ec75a90a6917c61019f641332a973e8 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sat, 13 Jun 2026 16:21:31 -0700 Subject: [PATCH 061/185] fastlio2: set mid360 acc_cov=0.5 to bound velocity divergence --- .../sensors/lidar/fastlio2/config/mid360.yaml | 20 +++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml b/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml index b710afd29e..b3337d9ed3 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml +++ b/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml @@ -11,15 +11,23 @@ preprocess: blind: 0.5 mapping: - acc_cov: 0.1 + # acc_cov is the divergence guard. At the upstream default (0.1) FAST-LIO + # odometry runs away to km/s on aggressive Go2 motion — the IMU acceleration + # prediction dominates the IESKF and diverges. Raising acc_cov down-weights + # that prediction. On the ruwik2_pt3 raw pcap the threshold is sharp between + # 0.3 (diverges) and 0.35 (bounded); 0.5 holds with margin (reliably bounded + # across reps, peak ~5 m/s, matching Point-LIO). See jhist + # dimos-fastlio-velocity-spike.md (2026-06-13 correction). + acc_cov: 0.5 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 - # Scan downsampling that feeds the IESKF. The upstream default (0.5 m) is too - # sparse on aggressive Go2 motion: the point-to-plane update becomes - # ill-conditioned and odometry velocity diverges to km/s. Replaying ruwik2_pt3 - # shows a sharp threshold between 0.38 m (diverges) and 0.35 m (bounded); 0.1 m - # stays bounded with margin. See jhist dimos-fastlio-velocity-spike.md (2026-06-10). + # Scan voxel that feeds the IESKF. NOTE: an earlier finding that voxel 0.1 m + # bounds the velocity divergence turned out to be a re-encoding artifact + # (measured on a bag with synthesized per-point timestamps). On the raw pcap, + # voxel size does NOT bound divergence at any value — acc_cov above is the + # real guard. 0.1 m retained only for scan density, not for stability. + # See jhist dimos-fastlio-velocity-spike.md (2026-06-13 correction). filter_size_surf: 0.1 filter_size_map: 0.1 fov_degree: 360 From 8d9a4618da7f67d5d1dd9732f3d1497a68b3c503 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sat, 13 Jun 2026 16:29:55 -0700 Subject: [PATCH 062/185] =?UTF-8?q?lidar:=20split=20recording=20=E2=80=94?= =?UTF-8?q?=20standalone=20LivoxPcapRecorder=20+=20ts-rewriting=20FastLio2?= =?UTF-8?q?Recorder?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - livox/pcap_recorder.py: standalone tcpdump pcap capture (LivoxPcapRecorder), decoupled from FAST-LIO. The lidar/SLAM module no longer owns packet capture. - fastlio2/recorder.py: FastLio2Recorder records fastlio_odometry + fastlio_lidar and rewrites ONLY those streams' timestamps onto the db clock (promoted from pcap_to_db's inline recorder; fixes the ts==0.0 falsy-fallback bug). - pcap_to_db.py now imports FastLio2Recorder instead of an inline copy. --- .../sensors/lidar/fastlio2/recorder.py | 124 +++++++ .../lidar/fastlio2/tools/pcap_to_db.py | 78 +---- .../sensors/lidar/livox/pcap_recorder.py | 318 ++++++++++++++++++ dimos/robot/all_blueprints.py | 2 + 4 files changed, 446 insertions(+), 76 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/fastlio2/recorder.py create mode 100644 dimos/hardware/sensors/lidar/livox/pcap_recorder.py diff --git a/dimos/hardware/sensors/lidar/fastlio2/recorder.py b/dimos/hardware/sensors/lidar/fastlio2/recorder.py new file mode 100644 index 0000000000..24ec96e473 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/recorder.py @@ -0,0 +1,124 @@ +# 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. + +"""FAST-LIO output recorder with timestamp rewriting. + +Records the two FastLio2 output streams — ``fastlio_odometry`` and +``fastlio_lidar`` — into a memory2 SQLite db, rewriting *only those streams'* +timestamps onto the db's clock. This is what makes offline pcap replay line up +with a live recording: FAST-LIO replayed from a pcap publishes on the pcap's +capture clock, and this recorder shifts that onto whatever clock the db already +uses, while leaving every other stream in the db untouched. + +The timestamp conversion is the only thing it does beyond a plain recorder, and +it applies it to fastlio messages exclusively (the two declared inputs), so a +recording that mixes fastlio replay with already-correct streams stays +coherent. Used by ``tools/pcap_to_db.py``; also usable directly in a blueprint +that wires FastLio2's ``odometry``/``lidar`` into this module's inputs. +""" + +from __future__ import annotations + +from collections.abc import AsyncIterator +import math +import time + +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +# Below this an absolute timestamp is sensor-boot seconds, not unix wall time. +_SENSOR_CLOCK_MAX = 1e8 +# Strictly-increasing tie-breaker so two samples never collide on ts. +_EPS = 1e-9 + + +class FastLio2RecorderConfig(ModuleConfig): + """Target db and the timestamp-conversion policy for fastlio streams.""" + + db_path: str = "" + # Earliest existing ts in the db, or -1.0 if the db has no timestamped rows. + ref_start_ts: float = -1.0 + # Explicit offset override; NaN means auto-derive from ref_start_ts. + time_offset: float = float("nan") + + +class FastLio2Recorder(Module): + """Record FAST-LIO odometry + lidar into a SQLite db, rewriting their ts. + + Only the fastlio streams declared here are time-converted; nothing else in + the db is touched. The offset is auto-derived (same clock family -> 0; + cross-clock -> start-align the replay's first ts onto the db's first), or + forced via ``time_offset``. + """ + + config: FastLio2RecorderConfig + fastlio_odometry: In[Odometry] + fastlio_lidar: In[PointCloud2] + _offset: float | None = None + _last_odom_ts: float = 0.0 + _last_lidar_ts: float = 0.0 + _last_pose: object = None + _odom_count: int = 0 + _lidar_count: int = 0 + + async def main(self) -> AsyncIterator[None]: + from dimos.memory2.store.sqlite import SqliteStore + + self._store = SqliteStore(path=self.config.db_path) + self._os = self._store.stream("fastlio_odometry", Odometry) + self._ls = self._store.stream("fastlio_lidar", PointCloud2) + yield + self._store.stop() + + def _resolve_offset(self, first_ts: float) -> float: + override = self.config.time_offset + if not math.isnan(override): + return override + ref = self.config.ref_start_ts + if ref < 0.0: + return 0.0 + # Same clock family (both wall, or both sensor) -> already aligned. + # Cross-clock -> start-align the replay's first ts onto the db's first. + if (first_ts > _SENSOR_CLOCK_MAX) == (ref > _SENSOR_CLOCK_MAX): + return 0.0 + return ref - first_ts + + def _aligned_ts(self, raw_ts: float, last_ts: float) -> float: + """Convert a replay ts onto the db clock, kept strictly above last_ts.""" + if self._offset is None: + self._offset = self._resolve_offset(raw_ts) + return max(raw_ts + self._offset, last_ts + _EPS) + + @staticmethod + def _raw_ts(v: object) -> float: + # A genuine sensor ts of 0.0 is falsy, so guard on None explicitly rather + # than `ts or time.time()` (which would misclassify a 0.0 stamp as wall). + ts = getattr(v, "ts", None) + return ts if ts is not None else time.time() + + async def handle_fastlio_odometry(self, v: Odometry) -> None: + ts = self._aligned_ts(self._raw_ts(v), self._last_odom_ts) + self._last_odom_ts = ts + pose = getattr(v, "pose", None) + self._last_pose = getattr(pose, "pose", None) if pose is not None else None + self._os.append(v, ts=ts, pose=self._last_pose) + self._odom_count += 1 + + async def handle_fastlio_lidar(self, v: PointCloud2) -> None: + ts = self._aligned_ts(self._raw_ts(v), self._last_lidar_ts) + self._last_lidar_ts = ts + self._ls.append(v, ts=ts, pose=self._last_pose) + self._lidar_count += 1 diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index 478085308b..375b3817fd 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -68,96 +68,22 @@ from __future__ import annotations import argparse -from collections.abc import AsyncIterator import math from pathlib import Path import sqlite3 import sys import time -from dimos.core.module import Module, ModuleConfig -from dimos.core.stream import In -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.hardware.sensors.lidar.fastlio2.recorder import FastLio2Recorder # Below this an absolute timestamp is sensor-boot seconds, not unix wall time. _SENSOR_CLOCK_MAX = 1e8 -# Strictly-increasing tie-breaker so two samples never collide on ts. -_EPS = 1e-9 # Poll the db on this cadence while the replay drains the pcap. _POLL_SEC = 1.0 # Stop after the odom stream has been stagnant this long (pcap fully drained). _STAGNANT_SEC = 6.0 -class _RecConfig(ModuleConfig): - """Configures the recorder with the target db and timing conversion.""" - - db_path: str = "" - # Earliest existing ts in the db, or -1.0 if the db has no timestamped rows. - ref_start_ts: float = -1.0 - # Explicit offset override; NaN means auto-derive from ref_start_ts. - time_offset: float = float("nan") - - -class _Rec(Module): - """Append FAST-LIO odometry + lidar into an existing SQLite db with ts conversion.""" - - config: _RecConfig - fastlio_odometry: In[Odometry] - fastlio_lidar: In[PointCloud2] - _offset: float | None = None - _last_odom_ts: float = 0.0 - _last_lidar_ts: float = 0.0 - _last_pose: object = None - _odom_count: int = 0 - _lidar_count: int = 0 - - async def main(self) -> AsyncIterator[None]: - from dimos.memory2.store.sqlite import SqliteStore - - self._store = SqliteStore(path=self.config.db_path) - self._os = self._store.stream("fastlio_odometry", Odometry) - self._ls = self._store.stream("fastlio_lidar", PointCloud2) - yield - self._store.stop() - - def _resolve_offset(self, first_ts: float) -> float: - override = self.config.time_offset - if not math.isnan(override): - return override - ref = self.config.ref_start_ts - if ref < 0.0: - return 0.0 - # Same clock family (both wall, or both sensor) -> already aligned. - # Cross-clock -> start-align the replay's first ts onto the db's first. - if (first_ts > _SENSOR_CLOCK_MAX) == (ref > _SENSOR_CLOCK_MAX): - return 0.0 - return ref - first_ts - - def _aligned_ts(self, raw_ts: float, last_ts: float) -> float: - """Convert a replay ts onto the db clock, kept strictly above last_ts.""" - if self._offset is None: - self._offset = self._resolve_offset(raw_ts) - return max(raw_ts + self._offset, last_ts + _EPS) - - async def handle_fastlio_odometry(self, v: Odometry) -> None: - raw_ts = getattr(v, "ts", None) or time.time() - ts = self._aligned_ts(raw_ts, self._last_odom_ts) - self._last_odom_ts = ts - pose = getattr(v, "pose", None) - self._last_pose = getattr(pose, "pose", None) if pose is not None else None - self._os.append(v, ts=ts, pose=self._last_pose) - self._odom_count += 1 - - async def handle_fastlio_lidar(self, v: PointCloud2) -> None: - raw_ts = getattr(v, "ts", None) or time.time() - ts = self._aligned_ts(raw_ts, self._last_lidar_ts) - self._last_lidar_ts = ts - self._ls.append(v, ts=ts, pose=self._last_pose) - self._lidar_count += 1 - - def _db_ref_start_ts(db_path: Path) -> float: """Min ts across the db's existing streams, or -1.0 if none/absent.""" if not db_path.exists(): @@ -277,7 +203,7 @@ def _run(args: argparse.Namespace) -> int: ) blueprint = autoconnect( fastlio, - _Rec.blueprint( + FastLio2Recorder.blueprint( db_path=str(db_path), ref_start_ts=ref_start_ts, time_offset=time_offset, diff --git a/dimos/hardware/sensors/lidar/livox/pcap_recorder.py b/dimos/hardware/sensors/lidar/livox/pcap_recorder.py new file mode 100644 index 0000000000..954c46ca3c --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/pcap_recorder.py @@ -0,0 +1,318 @@ +# 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. + +"""Standalone Livox pcap recorder. + +Captures the raw Livox Mid-360 UDP packets (point + IMU) to a libpcap file via +tcpdump. This is the ground-truth sensor input the FastLio2 binary can be +replayed against bit-for-bit (see fastlio2/tools/pcap_to_db.py). + +Decoupled from FAST-LIO on purpose: the lidar driver / SLAM module should not +own a packet capture. Drop this module into any blueprint that needs the raw +wire recorded alongside the live run:: + + from dimos.hardware.sensors.lidar.livox.pcap_recorder import LivoxPcapRecorder + autoconnect(Mid360.blueprint(...), LivoxPcapRecorder.blueprint(pcap_path="raw.pcap")) + +tcpdump needs capture capability once per host: + sudo setcap cap_net_raw,cap_net_admin=eip $(which tcpdump) +""" + +from __future__ import annotations + +import asyncio +from collections.abc import AsyncIterator +import os +from pathlib import Path +import re +import shlex +import shutil +import signal +import subprocess +import textwrap +import time + +from pydantic import Field + +from dimos.core.module import Module, ModuleConfig +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +def _stop_when_parent_dies(cmd: list[str], grace_sec: float) -> list[str]: + """Reap cmd if the recorder dies, including via SIGKILL (which it can't + intercept) — otherwise tcpdump's own session would outlive it.""" + parent_pid = os.getpid() + quoted = " ".join(shlex.quote(arg) for arg in cmd) + # Foreground waits on tcpdump so a startup failure propagates its exit code. + script = textwrap.dedent(f""" + {quoted} & + child=$! + ( + while kill -0 {parent_pid} 2>/dev/null; do + sleep 0.5 + done + kill -INT "$child" 2>/dev/null + sleep {grace_sec} + kill -KILL "$child" 2>/dev/null + ) & + watcher=$! + wait "$child" + code=$? + kill "$watcher" 2>/dev/null + exit $code + """).strip() + return ["bash", "-c", script] + + +class LivoxPcapRecorderConfig(ModuleConfig): + """Where and how to capture the raw Livox UDP stream.""" + + pcap_path: str | Path = "raw_mid360.pcap" + # Capture interface for tcpdump. Machine-specific, so it defaults from the + # DIMOS_PCAP_IFACE env var (falling back to enp2s0) to avoid hardcoding a + # value that's only correct on one host. + record_pcap_iface: str = Field( + default_factory=lambda: os.environ.get("DIMOS_PCAP_IFACE", "enp2s0") + ) + record_pcap_snaplen: int = 2048 + lidar_ip: str = "192.168.1.107" + # Grace period for each stop signal (SIGINT→SIGTERM→SIGKILL) when tearing + # down the tcpdump capture. + pcap_stop_timeout: float = 5.0 + + +class LivoxPcapRecorder(Module): + """Owns a tcpdump process capturing raw Mid-360 UDP packets to a pcap. + + Single responsibility: write the wire. Pairs with a memory2 recorder that + captures the decoded/derived streams; together they make a session that can + be replayed offline. + """ + + config: LivoxPcapRecorderConfig + + # tcpdump fails fast (EPERM, bad iface) within a few ms; pause briefly so poll() catches that. + _TCPDUMP_STARTUP_PROBE_SEC: float = 0.3 + # How long to let tcpdump run before declaring the capture dead if nothing landed. + _PCAP_WATCHDOG_SEC: float = 5.0 + # A libpcap file with zero packets is just its 24-byte global header. + _PCAP_GLOBAL_HEADER_BYTES: int = 24 + # How long the diagnostic sniff listens for *any* UDP source on the iface. + _PCAP_DIAGNOSTIC_SNIFF_SEC: float = 3.0 + + _pcap_proc: subprocess.Popen[bytes] | None = None + + async def main(self) -> AsyncIterator[None]: + self._start_pcap() + if self._pcap_proc is not None: + watchdog = asyncio.create_task(self._pcap_watchdog()) + else: + watchdog = None + yield + if watchdog is not None: + watchdog.cancel() + self._stop_pcap() + + def _start_pcap(self) -> None: + cfg = self.config + path = Path(cfg.pcap_path).expanduser() + path.parent.mkdir(parents=True, exist_ok=True) + + # Capture every UDP packet originating from the lidar. + packet_filter_expression = f"src host {cfg.lidar_ip} and udp" + tcpdump = shutil.which("tcpdump") or "tcpdump" + cmd = [ + tcpdump, + "-i", + cfg.record_pcap_iface, + "-w", + str(path), + "-s", + str(cfg.record_pcap_snaplen), + "-U", + "-n", + packet_filter_expression, + ] + + # Own session/group so _stop_pcap can signal the wrapper + tcpdump + # without touching the recorder, and Ctrl-C doesn't race shutdown. + proc = subprocess.Popen( + _stop_when_parent_dies(cmd, cfg.pcap_stop_timeout), + stdout=subprocess.DEVNULL, + stderr=subprocess.PIPE, + start_new_session=True, + ) + # tcpdump exits within a few ms on EPERM; wait briefly so we can detect that. + time.sleep(self._TCPDUMP_STARTUP_PROBE_SEC) + if proc.poll() is not None: + stderr = proc.stderr.read().decode(errors="replace") if proc.stderr else "" + self._pcap_proc = None + logger.error( + f"LivoxPcapRecorder failed to start — tcpdump exited" + f" rc={proc.returncode} stderr={stderr.strip()}" + ) + print( + "[livox_pcap] pcap recording is enabled but tcpdump cannot capture.\n" + " Grant capture capability once with:\n" + f" sudo setcap cap_net_raw,cap_net_admin=eip {tcpdump}\n" + " then restart. (tcpdump stderr above.)", + flush=True, + ) + return + + logger.info( + f"LivoxPcapRecorder capturing path={path} " + f"iface={cfg.record_pcap_iface} filter={packet_filter_expression!r}" + ) + self._pcap_proc = proc + + async def _pcap_watchdog(self) -> None: + """If tcpdump captured nothing after a few seconds, dump everything we + know about why — almost always a wrong lidar_ip or interface.""" + await asyncio.sleep(self._PCAP_WATCHDOG_SEC) + proc = self._pcap_proc + if proc is None: + return + path = Path(self.config.pcap_path).expanduser() + try: + size = path.stat().st_size + except OSError: + size = -1 + if size > self._PCAP_GLOBAL_HEADER_BYTES: + logger.info( + f"LivoxPcapRecorder pcap healthy — {size} bytes captured in " + f"{self._PCAP_WATCHDOG_SEC:.0f}s path={path}" + ) + return + report = await asyncio.to_thread(self._build_empty_pcap_report, size, proc) + logger.error(report) + print(report, flush=True) + + def _build_empty_pcap_report(self, size: int, proc: subprocess.Popen[bytes]) -> str: + cfg = self.config + packet_filter_expression = f"src host {cfg.lidar_ip} and udp" + proc_alive = proc.poll() is None + stderr_text = "" + if not proc_alive and proc.stderr is not None: + try: + stderr_text = proc.stderr.read().decode(errors="replace").strip() + except (OSError, ValueError): + stderr_text = "" + + observed = self._observed_udp_sources() + if observed: + listing = "\n".join( + f" {source} ({count} pkts)" + for source, count in sorted(observed.items(), key=lambda kv: kv[1], reverse=True) + ) + diagnosis = ( + f" UDP traffic IS flowing on {cfg.record_pcap_iface}, but from other source(s):\n" + f"{listing}\n" + f" None matched 'src host {cfg.lidar_ip}'. The lidar_ip is almost certainly\n" + f" wrong — set lidar_ip to whichever address above is the lidar and restart." + ) + else: + diagnosis = ( + f" NO UDP traffic at all was seen on {cfg.record_pcap_iface} during a " + f"{self._PCAP_DIAGNOSTIC_SNIFF_SEC:.0f}s probe.\n" + f" Wrong interface, unplugged cable, or the lidar is powered off." + ) + + neigh = self._run_quiet(["ip", "neigh", "show", cfg.lidar_ip]).strip() + return textwrap.dedent(f""" + ============================================================================ + [livox_pcap] PCAP WATCHDOG: 0 packets captured after {self._PCAP_WATCHDOG_SEC:.0f}s + ============================================================================ + Recording is enabled but tcpdump wrote an EMPTY pcap (size={size} bytes; an + empty libpcap file is {self._PCAP_GLOBAL_HEADER_BYTES} bytes of global header). + + Capture config: + interface : {cfg.record_pcap_iface} + lidar_ip : {cfg.lidar_ip} + filter : {packet_filter_expression!r} + pcap_path : {cfg.pcap_path} + tcpdump : alive={proc_alive} pid={proc.pid}{f" stderr={stderr_text!r}" if stderr_text else ""} + + Diagnosis: + {diagnosis} + + arp/neigh for {cfg.lidar_ip}: {neigh or ""} + ============================================================================ + """).strip() + + def _observed_udp_sources(self) -> dict[str, int]: + """Sniff the interface briefly and tally which source IPs are sending UDP.""" + tcpdump = shutil.which("tcpdump") or "tcpdump" + cmd = [tcpdump, "-i", self.config.record_pcap_iface, "-nn", "-c", "60", "udp"] + try: + result = subprocess.run( + cmd, + capture_output=True, + text=True, + timeout=self._PCAP_DIAGNOSTIC_SNIFF_SEC, + ) + output = result.stdout + except subprocess.TimeoutExpired as expired: + stdout = expired.stdout + output = ( + stdout.decode(errors="replace") if isinstance(stdout, bytes) else (stdout or "") + ) + except OSError: + return {} + counts: dict[str, int] = {} + for line in output.splitlines(): + match = re.search(r"\bIP6?\s+(\S+?)\.\d+\s+>", line) + if match: + source = match.group(1) + counts[source] = counts.get(source, 0) + 1 + return counts + + @staticmethod + def _run_quiet(cmd: list[str]) -> str: + try: + return subprocess.run(cmd, capture_output=True, text=True, timeout=2.0).stdout + except (OSError, subprocess.TimeoutExpired): + return "" + + def _stop_pcap(self) -> None: + proc = self._pcap_proc + if proc is None: + return + self._pcap_proc = None + if proc.poll() is not None: + return + # Signal the group so tcpdump gets it directly. SIGINT is its + # clean-stop signal (flushes the pcap); escalate if it hangs. + try: + pgid = os.getpgid(proc.pid) + except ProcessLookupError: + return + timeout = self.config.pcap_stop_timeout + for sig in (signal.SIGINT, signal.SIGTERM, signal.SIGKILL): + try: + os.killpg(pgid, sig) + except ProcessLookupError: + break + try: + proc.wait(timeout=timeout) + break + except subprocess.TimeoutExpired: + logger.warning( + f"tcpdump did not exit on {sig.name}; escalating path={self.config.pcap_path}" + ) + else: + proc.wait() + logger.info(f"LivoxPcapRecorder stopped path={self.config.pcap_path}") diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 24d90dd7fe..eb1793f984 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -155,6 +155,7 @@ "evaluator": "dimos.navigation.nav_3d.evaluator.evaluator.Evaluator", "far-planner": "dimos.navigation.nav_stack.modules.far_planner.far_planner.FarPlanner", "fast-lio2": "dimos.hardware.sensors.lidar.fastlio2.module.FastLio2", + "fast-lio2-recorder": "dimos.hardware.sensors.lidar.fastlio2.recorder.FastLio2Recorder", "g1-connection": "dimos.robot.unitree.g1.connection.G1Connection", "g1-connection-base": "dimos.robot.unitree.g1.connection.G1ConnectionBase", "g1-high-level-dds-sdk": "dimos.robot.unitree.g1.effectors.high_level.dds_sdk.G1HighLevelDdsSdk", @@ -177,6 +178,7 @@ "joystick-module": "dimos.robot.unitree.b1.joystick_module.JoystickModule", "keyboard-teleop": "dimos.robot.unitree.keyboard_teleop.KeyboardTeleop", "keyboard-teleop-module": "dimos.teleop.keyboard.keyboard_teleop_module.KeyboardTeleopModule", + "livox-pcap-recorder": "dimos.hardware.sensors.lidar.livox.pcap_recorder.LivoxPcapRecorder", "local-planner": "dimos.navigation.nav_stack.modules.local_planner.local_planner.LocalPlanner", "manipulation-module": "dimos.manipulation.manipulation_module.ManipulationModule", "map": "dimos.robot.unitree.type.map.Map", From 1088fe3f647f4bfab286dcb5671118a3363b2136 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sat, 13 Jun 2026 16:36:48 -0700 Subject: [PATCH 063/185] fastlio2: rip out the global_map output (Python) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit FastLio2 no longer produces a global voxel map — odometry + registered lidar only. Removed global_map Out, map config, and the mapping.GlobalPointcloud spec. Updated all consumers (fastlio_blueprints, alfred_nav, g1_onboard, g1_nav_onboard, mobile, pcap_to_db) to drop map args + the global_map remap. Nav blueprints lose their fastlio map; full nav wants a separate mapper wired in (follow-up). --- dimos/control/blueprints/mobile.py | 2 -- .../sensors/lidar/fastlio2/fastlio_blueprints.py | 6 +++--- dimos/hardware/sensors/lidar/fastlio2/module.py | 10 ++-------- .../sensors/lidar/fastlio2/tools/pcap_to_db.py | 1 - dimos/robot/diy/alfred/blueprints/alfred_nav.py | 2 -- .../g1/blueprints/navigation/unitree_g1_nav_onboard.py | 1 - .../g1/blueprints/primitive/unitree_g1_onboard.py | 3 +-- 7 files changed, 6 insertions(+), 19 deletions(-) diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py index c5065ea8d4..f16a277734 100644 --- a/dimos/control/blueprints/mobile.py +++ b/dimos/control/blueprints/mobile.py @@ -146,7 +146,6 @@ def _flowbase_twist_base( host_ip=os.getenv("LIDAR_HOST_IP", "192.168.1.5"), lidar_ip=os.getenv("LIDAR_IP", "192.168.1.189"), mount=_flowbase_mid360_mount, - map_freq=1.0, config="default.yaml", ), create_nav_stack( @@ -199,7 +198,6 @@ def _flowbase_twist_base( .remappings( [ (FastLio2, "lidar", "registered_scan"), - (FastLio2, "global_map", "global_map_fastlio"), # SimplePlanner / FarPlanner owns way_point — disconnect MovementManager's # redundant pass-through copy (matches unitree-g1-nav-onboard). (MovementManager, "way_point", "_mgr_way_point_unused"), diff --git a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py index 89e8ec2c14..6c848233f2 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py +++ b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py @@ -23,7 +23,7 @@ mid360_fastlio = autoconnect( - FastLio2.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=-1), + FastLio2.blueprint(voxel_size=voxel_size), vis_module("rerun"), ).global_config(n_workers=2, robot_model="mid360_fastlio2") @@ -41,7 +41,7 @@ ).global_config(n_workers=3, robot_model="mid360_fastlio2_voxels") mid360_fastlio_voxels_native = autoconnect( - FastLio2.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=3.0), + FastLio2.blueprint(voxel_size=voxel_size), vis_module( "rerun", rerun_config={ @@ -54,7 +54,7 @@ mid360_fastlio_ray_trace = autoconnect( - FastLio2.blueprint(voxel_size=voxel_size, map_voxel_size=voxel_size, map_freq=-1), + FastLio2.blueprint(voxel_size=voxel_size), RayTracingVoxelMap.blueprint(voxel_size=voxel_size), vis_module( "rerun", diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 7113a5de3a..f9a4b02e33 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -62,7 +62,7 @@ from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.navigation.nav_stack.frames import FRAME_BODY, FRAME_ODOM -from dimos.spec import mapping, perception +from dimos.spec import perception from dimos.utils.generic import get_local_ips from dimos.utils.logging_config import setup_logger @@ -102,11 +102,6 @@ class FastLio2Config(NativeModuleConfig): sor_mean_k: int = 50 sor_stddev: float = 1.0 - # Global voxel map (disabled when map_freq <= 0) - map_freq: float = 0.0 - map_voxel_size: float = 0.1 - map_max_range: float = 100.0 - # FAST-LIO YAML config (relative to config/ dir, or absolute path) # C++ binary reads YAML directly via yaml-cpp config: Annotated[ @@ -170,12 +165,11 @@ def model_post_init(self, __context: object) -> None: ] -class FastLio2(NativeModule, perception.Lidar, perception.Odometry, mapping.GlobalPointcloud): +class FastLio2(NativeModule, perception.Lidar, perception.Odometry): config: FastLio2Config lidar: Out[PointCloud2] odometry: Out[Odometry] - global_map: Out[PointCloud2] @rpc def start(self) -> None: diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index 375b3817fd..16d75b952b 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -186,7 +186,6 @@ def _run(args: argparse.Namespace) -> int: fastlio_kwargs: dict[str, object] = dict( frame_id="world", - map_freq=-1, odom_freq=args.odom_freq, replay_pcap=pcap_path, deterministic_clock=False, diff --git a/dimos/robot/diy/alfred/blueprints/alfred_nav.py b/dimos/robot/diy/alfred/blueprints/alfred_nav.py index 91a8db9bec..d0853d7d08 100644 --- a/dimos/robot/diy/alfred/blueprints/alfred_nav.py +++ b/dimos/robot/diy/alfred/blueprints/alfred_nav.py @@ -55,7 +55,6 @@ host_ip=os.getenv("LIDAR_HOST_IP", "192.168.1.5"), lidar_ip=os.getenv("LIDAR_IP", "192.168.1.189"), mount=ALFRED.internal_odom_offsets["mid360_link"], - map_freq=1.0, config="default.yaml", ), create_nav_stack(**nav_config), @@ -73,7 +72,6 @@ [ # nav stack needs "registered_scan" (FastLio2, "lidar", "registered_scan"), - (FastLio2, "global_map", "global_map_fastlio"), # SimplePlanner / FarPlanner owns way_point — disconnect MovementManager's (MovementManager, "way_point", "_mgr_way_point_unused"), ] diff --git a/dimos/robot/unitree/g1/blueprints/navigation/unitree_g1_nav_onboard.py b/dimos/robot/unitree/g1/blueprints/navigation/unitree_g1_nav_onboard.py index 1fe25fb40d..d424f11ea4 100644 --- a/dimos/robot/unitree/g1/blueprints/navigation/unitree_g1_nav_onboard.py +++ b/dimos/robot/unitree/g1/blueprints/navigation/unitree_g1_nav_onboard.py @@ -73,7 +73,6 @@ [ # FastLio2 outputs "lidar"; SmartNav modules expect "registered_scan" (FastLio2, "lidar", "registered_scan"), - (FastLio2, "global_map", "global_map_fastlio"), # Planner owns way_point — disconnect MovementManager's click relay (MovementManager, "way_point", "_mgr_way_point_unused"), ] diff --git a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py index 96f97146b1..fce8fe4efa 100644 --- a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py +++ b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py @@ -29,9 +29,8 @@ host_ip=os.getenv("LIDAR_HOST_IP", "192.168.123.164"), lidar_ip=os.getenv("LIDAR_IP", "192.168.123.120"), mount=G1.internal_odom_offsets["mid360_link"], - map_freq=1.0, config="default.yaml", - ).remappings([(FastLio2, "global_map", "global_map_fastlio")]), + ), G1HighLevelDdsSdk.blueprint(), unitree_g1_vis, ).global_config(n_workers=12, robot_model="unitree_g1") From c64047dca2ed4730eea8ca161bf38eb9262ae75e Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sat, 13 Jun 2026 16:41:15 -0700 Subject: [PATCH 064/185] fastlio2: rip out the global_map output (C++) Remove the VoxelMap global-map machinery from the native binary: the voxel_map.hpp include, g_map_topic, map_freq/map_voxel_size/map_max_range args, the VoxelMap instance + insert/prune/publish loop, and its timing sections. FAST-LIO now only publishes registered lidar + odometry. --- .../sensors/lidar/fastlio2/cpp/main.cpp | 44 ------------------- 1 file changed, 44 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index 1805945451..86e8bb139e 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -35,7 +35,6 @@ #include "dimos_native_module.hpp" #include "pcap_replay.hpp" #include "timing.hpp" -#include "voxel_map.hpp" // dimos LCM message headers #include "geometry_msgs/Quaternion.hpp" @@ -137,7 +136,6 @@ static std::optional virtual_now() { static std::string g_lidar_topic; static std::string g_odometry_topic; -static std::string g_map_topic; static std::string g_frame_id; // required via --frame_id static std::string g_child_frame_id; // required via --child_frame_id static float g_frequency = 10.0f; @@ -488,7 +486,6 @@ int main(int argc, char** argv) { // Required: LCM topics for output ports g_lidar_topic = mod.has("lidar") ? mod.topic("lidar") : ""; g_odometry_topic = mod.has("odometry") ? mod.topic("odometry") : ""; - g_map_topic = mod.has("global_map") ? mod.topic("global_map") : ""; if (g_lidar_topic.empty() && g_odometry_topic.empty()) { fprintf(stderr, "Error: at least one of --lidar or --odometry is required\n"); @@ -526,9 +523,6 @@ int main(int argc, char** argv) { filter_cfg.voxel_size = mod.arg_float("voxel_size", 0.1f); filter_cfg.sor_mean_k = mod.arg_int("sor_mean_k", 50); filter_cfg.sor_stddev = mod.arg_float("sor_stddev", 1.0f); - float map_voxel_size = mod.arg_float("map_voxel_size", 0.1f); - float map_max_range = mod.arg_float("map_max_range", 100.0f); - float map_freq = mod.arg_float("map_freq", 0.0f); // Verbose logging — propagates to the FAST-LIO C++ core via the // `fastlio_debug` global. Default false → only real errors print. @@ -605,8 +599,6 @@ int main(int argc, char** argv) { g_lidar_topic.empty() ? "(disabled)" : g_lidar_topic.c_str()); printf("[fastlio2] odometry topic: %s\n", g_odometry_topic.empty() ? "(disabled)" : g_odometry_topic.c_str()); - printf("[fastlio2] global_map topic: %s\n", - g_map_topic.empty() ? "(disabled)" : g_map_topic.c_str()); printf("[fastlio2] config: %s\n", config_path.c_str()); printf("[fastlio2] host_ip: %s lidar_ip: %s frequency: %.1f Hz\n", host_ip.c_str(), lidar_ip.c_str(), g_frequency); @@ -614,9 +606,6 @@ int main(int argc, char** argv) { pointcloud_freq, odom_freq); printf("[fastlio2] voxel_size: %.3f sor_mean_k: %d sor_stddev: %.1f\n", filter_cfg.voxel_size, filter_cfg.sor_mean_k, filter_cfg.sor_stddev); - if (!g_map_topic.empty()) - printf("[fastlio2] map_voxel_size: %.3f map_max_range: %.1f map_freq: %.1f Hz\n", - map_voxel_size, map_max_range, map_freq); } // Signal handlers @@ -653,15 +642,6 @@ int main(int argc, char** argv) { std::optional last_pc_publish; std::optional last_odom_publish; - std::unique_ptr global_map; - std::chrono::microseconds map_interval{0}; - std::optional last_map_publish; - if (!g_map_topic.empty() && map_freq > 0.0f) { - global_map = std::make_unique(map_voxel_size, map_max_range); - map_interval = std::chrono::microseconds( - static_cast(1e6 / map_freq)); - } - // Per-section timing counters for `run_main_iter`. Active only when // --debug is on (Scope's constructor reads `fastlio_debug` and no-ops // otherwise). `timing::maybe_flush(now)` at the bottom prints a per- @@ -674,8 +654,6 @@ int main(int argc, char** argv) { static timing::Section t_get_world_cloud{"fast_lio.get_world_cloud"}; static timing::Section t_filter_cloud{"filter_cloud"}; static timing::Section t_publish_lidar{"publish_lidar"}; - static timing::Section t_map_insert{"global_map.insert"}; - static timing::Section t_map_publish{"global_map.publish"}; static timing::Section t_publish_odom{"publish_odometry"}; auto run_main_iter = [&](std::chrono::steady_clock::time_point now) { @@ -704,9 +682,6 @@ int main(int argc, char** argv) { if (!last_odom_publish.has_value()) { last_odom_publish = seed; } - if (global_map && !last_map_publish.has_value()) { - last_map_publish = seed; - } // At frame rate: drain accumulated raw points into a CustomMsg // and feed to FAST-LIO. Hold g_pc_mutex across the rate-limit @@ -779,25 +754,6 @@ int main(int argc, char** argv) { publish_lidar(filtered, ts); last_pc_publish = now; } - - // Global voxel map: insert this scan, prune, then publish - // a snapshot at map_freq. - if (global_map) { - { - timing::Scope s(t_map_insert); - global_map->insert(filtered); - } - if (now - *last_map_publish >= map_interval) { - timing::Scope s(t_map_publish); - global_map->prune( - static_cast(pose[0]), - static_cast(pose[1]), - static_cast(pose[2])); - auto map_cloud = global_map->to_cloud(); - publish_lidar(map_cloud, ts, g_map_topic); - last_map_publish = now; - } - } } // Pose + covariance, rate-limited to odom_freq. From 4725bcb1341a51e6dc80e6feadc8bde5d40e5871 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sat, 13 Jun 2026 17:16:27 -0700 Subject: [PATCH 065/185] fastlio2: bump mid360 acc_cov 0.5 -> 1.0 for reliable bounding MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Testing on the ruwik2_pt3 replay showed 0.5 is borderline — it bounds most runs but still diverged ~1 in 3 (the divergence is stochastic; FAST-LIO isn't bit-exact). acc_cov=1.0 held bounded across every rep (~4 m/s, matching Point-LIO), so default to 1.0 for margin. --- .../sensors/lidar/fastlio2/config/mid360.yaml | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml b/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml index b3337d9ed3..dd6ddab0c2 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml +++ b/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml @@ -14,11 +14,13 @@ mapping: # acc_cov is the divergence guard. At the upstream default (0.1) FAST-LIO # odometry runs away to km/s on aggressive Go2 motion — the IMU acceleration # prediction dominates the IESKF and diverges. Raising acc_cov down-weights - # that prediction. On the ruwik2_pt3 raw pcap the threshold is sharp between - # 0.3 (diverges) and 0.35 (bounded); 0.5 holds with margin (reliably bounded - # across reps, peak ~5 m/s, matching Point-LIO). See jhist - # dimos-fastlio-velocity-spike.md (2026-06-13 correction). - acc_cov: 0.5 + # that prediction. The divergence is stochastic (FAST-LIO is not bit-exact), + # so acc_cov shifts the bounded *probability* rather than being a hard switch: + # on the ruwik2_pt3 raw pcap, 0.3 diverges, 0.5 is borderline (bounds most + # runs but still diverged ~1 in 3), and 1.0 held bounded across every rep + # (peak ~4 m/s, matching Point-LIO). 1.0 for margin. A hard guarantee would + # need the velocity guardrail. See jhist dimos-fastlio-velocity-spike.md. + acc_cov: 1.0 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 From 9bb831558a8c0bc4f5d8df00d2a8a4aeacf8b351 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 20:21:03 -0700 Subject: [PATCH 066/185] refactor(pointlio): remove in-process pcap replay; use virtual_mid360 only MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Rip out the in-process replay path now that the rust virtual_mid360 module covers pcap replay over the wire (unmodified live SDK, no replay code in the consumer). Removed: cpp/pcap_replay.hpp; the replay/virtual-clock/ deterministic-clock/first-packet-marker machinery + branches in main.cpp; the replay_pcap/replay_skip_until_ns/deterministic_clock/replay_dual_thread/ first_packet_marker config in module.py (start() now always validates the network); tools/pcap_to_db.py (drove the in-process replay). Live SDK path unchanged — verified it still handshakes + publishes odometry via virtual_mid360. --- .../sensors/lidar/pointlio/cpp/main.cpp | 347 ++--------------- .../lidar/pointlio/cpp/pcap_replay.hpp | 347 ----------------- .../sensors/lidar/pointlio/cpp/timing.hpp | 2 +- .../hardware/sensors/lidar/pointlio/module.py | 18 +- .../lidar/pointlio/tools/pcap_to_db.py | 364 ------------------ 5 files changed, 28 insertions(+), 1050 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/pointlio/cpp/pcap_replay.hpp delete mode 100644 dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 7360f75ebc..8d645031cf 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -33,7 +33,6 @@ #include "cloud_filter.hpp" #include "dimos_native_module.hpp" -#include "pcap_replay.hpp" #include "timing.hpp" // dimos LCM message headers @@ -61,65 +60,11 @@ static std::atomic g_running{true}; static lcm::LCM* g_lcm = nullptr; static FastLio* g_fastlio = nullptr; -// Replay: virtual clock holds the pcap timestamp of the packet being fed so -// publish_*() reports capture time. Live leaves it 0 → system_clock::now(). -static std::atomic g_replay_mode{false}; -static std::atomic g_virtual_clock_ns{0}; - -// Deterministic mode: drive the virtual clock from pkt->timestamp (identical -// live vs pcap) for rate limits + publish ts, removing wall-clock jitter so -// live and replay produce identical state. Cost: header.stamp becomes -// sensor-boot seconds, not unix time. Off by default; record/replay demos only. -static std::atomic g_deterministic_clock{false}; - -// First packet's sensor ts (deterministic mode only): seeds the main loop's -// rate-limit bookmarks at the first delivered packet regardless of loop timing. -static std::atomic g_first_packet_clock_ns{0}; - -// First-packet marker: live writes the first callback's pkt->timestamp here; -// demo_replay reads it back as --replay_skip_until_ns to drop the same -// SDK-eaten warmup prefix. pkt->timestamp is bit-identical live vs pcap. -static std::string g_first_packet_marker_path; -static std::atomic g_first_packet_marker_written{false}; - -static void mark_first_packet(uint64_t pkt_timestamp_ns) { - if (g_first_packet_marker_path.empty()) { - return; - } - bool expected = false; - if (!g_first_packet_marker_written.compare_exchange_strong(expected, true)) { - return; - } - FILE* f = std::fopen(g_first_packet_marker_path.c_str(), "w"); - if (f) { - std::fprintf(f, "%lu\n", static_cast(pkt_timestamp_ns)); - std::fclose(f); - } -} - static double get_publish_ts() { - if (g_deterministic_clock.load() || g_replay_mode.load()) { - return static_cast(g_virtual_clock_ns.load()) / 1e9; - } return std::chrono::duration( std::chrono::system_clock::now().time_since_epoch()).count(); } -// Clock for the main loop's rate limiters. In deterministic mode returns a -// time_point from g_virtual_clock_ns (sensor-paced), else wall clock — the -// latter keeps the feeder/main scan-composition race needed to reproduce live -// divergence offline. nullopt = no packet seen yet; skip rate-limit checks. -static std::optional virtual_now() { - if (g_deterministic_clock.load()) { - uint64_t ns = g_virtual_clock_ns.load(); - if (ns == 0) { - return std::nullopt; - } - return std::chrono::steady_clock::time_point(std::chrono::nanoseconds(ns)); - } - return std::chrono::steady_clock::now(); -} - static std::string g_lidar_topic; static std::string g_odometry_topic; static std::string g_frame_id; // required via --frame_id @@ -314,22 +259,8 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ const uint64_t point_interval_ns = dot_num > 0 ? static_cast(data->time_interval) * 100 / dot_num : 0; - if (!g_replay_mode.load()) { - mark_first_packet(ts_ns); - } - std::lock_guard lock(g_pc_mutex); - // Advance the virtual clock under the accumulator mutex so the main loop - // can't see a clock advance without the matching points. Monotonic CAS: - // out-of-order SDK delivery must not roll the clock back. - if (g_deterministic_clock.load()) { - uint64_t expected = 0; - g_first_packet_clock_ns.compare_exchange_strong(expected, ts_ns); - uint64_t cur = g_virtual_clock_ns.load(); - while (cur < ts_ns && !g_virtual_clock_ns.compare_exchange_weak(cur, ts_ns)) {} - } - if (!g_frame_has_timestamp) { g_frame_start_ns = ts_ns; g_frame_has_timestamp = true; @@ -369,10 +300,9 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, if (!g_running.load() || data == nullptr || !g_fastlio) return; uint64_t pkt_ts_ns = get_timestamp_ns(data); - if (!g_replay_mode.load()) { - mark_first_packet(pkt_ts_ns); - // Live IMU-drop instrumentation: a dropped datagram shows as a sensor-ts - // jump; wall gaps exceeding sensor gaps mean callback starvation. + // Live IMU-drop instrumentation: a dropped datagram shows as a sensor-ts + // jump; wall gaps exceeding sensor gaps mean callback starvation. + { static std::atomic last_pkt_ts_ns{0}; static std::atomic imu_pkt_count{0}; static std::atomic imu_gap_count{0}; @@ -439,16 +369,6 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, g_fastlio->feed_imu(imu_msg); } - - // Advance the virtual clock after feed_imu, under g_pc_mutex so it's - // serialized with on_point_cloud / the scan swap. Monotonic CAS. - if (g_deterministic_clock.load()) { - std::lock_guard lock(g_pc_mutex); - uint64_t expected = 0; - g_first_packet_clock_ns.compare_exchange_strong(expected, pkt_ts_ns); - uint64_t cur = g_virtual_clock_ns.load(); - while (cur < pkt_ts_ns && !g_virtual_clock_ns.compare_exchange_weak(cur, pkt_ts_ns)) {} - } } static void on_info_change(const uint32_t handle, const LivoxLidarInfo* info, @@ -535,35 +455,6 @@ int main(int argc, char** argv) { ports.host_imu_data = mod.arg_int("host_imu_data_port", port_defaults.host_imu_data); ports.host_log_data = mod.arg_int("host_log_data_port", port_defaults.host_log_data); - // Replay: skip SDK init; a feeder thread reads the pcap and calls - // on_point_cloud / on_imu_data directly, using pcap ts as the clock. - std::string replay_pcap = mod.arg("replay_pcap", ""); - // Alt source: flat binary of driver CustomMsg/Imu frames from a ROS bag - // (tools/dump_bag_frames.py). Bypasses UDP->CustomMsg reconstruction to - // isolate port faithfulness from reconstruction fidelity. - std::string replay_bagframes = mod.arg("replay_bagframes", ""); - g_replay_mode.store(!replay_pcap.empty() || !replay_bagframes.empty()); - - // Drop pcap packets with pcap_ts < this, mimicking the live SDK warmup - // discard so both modes start from the same first packet. - uint64_t replay_skip_until_ns = 0; - { - std::string s = mod.arg("replay_skip_until_ns", "0"); - if (!s.empty()) { - replay_skip_until_ns = std::stoull(s); - } - } - - // Live: write the first callback's ts here; pairs with replay's - // --replay_skip_until_ns to align packet sets. - g_first_packet_marker_path = mod.arg("first_packet_marker", ""); - - // Replay: feed point and IMU on two threads (mimics the SDK's concurrent - // delivery). Only meaningful with deterministic_clock=false. - const bool replay_dual_thread = mod.arg_bool("replay_dual_thread", false); - - g_deterministic_clock.store(mod.arg_bool("deterministic_clock", false)); - // Initial pose offset [x, y, z, qx, qy, qz, qw] { std::string init_str = mod.arg("init_pose", ""); @@ -618,8 +509,8 @@ int main(int argc, char** argv) { g_fastlio = &fast_lio; if (debug) printf("[fastlio2] FAST-LIO initialized.\n"); - // Main-loop state. Body lives in `run_main_iter` so it can run from either - // the wall-paced main thread (live) or the pcap-paced feeder (replay). + // Main-loop state. Body lives in `run_main_iter`, driven by the wall-paced + // main thread. auto frame_interval = std::chrono::microseconds( static_cast(1e6 / g_frequency)); std::optional last_emit; @@ -647,25 +538,15 @@ int main(int argc, char** argv) { auto run_main_iter = [&](std::chrono::steady_clock::time_point now) { timing::Scope iter_scope(t_iter); // Lazy-seed rate-limit bookmarks on the first iteration so they align - // with the chosen clock. In deterministic mode seed from the FIRST - // packet's ts (not now) so live and replay anchor the same scan - // boundary — required for bit-for-bit parity. - auto seed = now; - if (g_deterministic_clock.load()) { - uint64_t first = g_first_packet_clock_ns.load(); - if (first != 0) { - seed = std::chrono::steady_clock::time_point( - std::chrono::nanoseconds(first)); - } - } + // with the wall clock. if (!last_emit.has_value()) { - last_emit = seed; + last_emit = now; } if (!last_pc_publish.has_value()) { - last_pc_publish = seed; + last_pc_publish = now; } if (!last_odom_publish.has_value()) { - last_odom_publish = seed; + last_odom_publish = now; } // At frame rate: drain accumulated points into a CustomMsg and feed @@ -676,21 +557,13 @@ int main(int argc, char** argv) { { timing::Scope s(t_emit_check); std::lock_guard lock(g_pc_mutex); - auto check_now = now; - if (g_deterministic_clock.load()) { - uint64_t ns = g_virtual_clock_ns.load(); - if (ns != 0) { - check_now = std::chrono::steady_clock::time_point( - std::chrono::nanoseconds(ns)); - } - } - if (check_now - *last_emit >= frame_interval) { + if (now - *last_emit >= frame_interval) { if (!g_accumulated_points.empty()) { points.swap(g_accumulated_points); frame_start = g_frame_start_ns; g_frame_has_timestamp = false; } - last_emit = check_now; + last_emit = now; } } if (!points.empty()) { @@ -750,187 +623,24 @@ int main(int argc, char** argv) { timing::maybe_flush(std::chrono::steady_clock::now()); }; - // Packet source: live = Livox SDK callbacks from its own threads; replay = - // feeder thread reads pcap through the same callbacks. Either way the main - // thread owns run_main_iter, so the only difference is SDK vs pcap. - std::thread replay_thread; - if (g_replay_mode.load()) { - if (debug) printf("[fastlio2] REPLAY mode, pcap=%s\n", replay_pcap.c_str()); - replay_thread = std::thread([&]() { - if (!replay_bagframes.empty()) { - // Bag-frame replay: feed driver records straight into the port, - // serialized with the EKF on this thread. No reconstruction, no - // accumulator — deterministic by design. - std::ifstream bf(replay_bagframes, std::ios::binary); - if (!bf) { - fprintf(stderr, "[bagframes] cannot open %s\n", replay_bagframes.c_str()); - g_running.store(false); - return; - } - auto advance_clock = [](uint64_t ts_ns) { - uint64_t expected = 0; - g_first_packet_clock_ns.compare_exchange_strong(expected, ts_ns); - uint64_t cur = g_virtual_clock_ns.load(); - while (cur < ts_ns && - !g_virtual_clock_ns.compare_exchange_weak(cur, ts_ns)) {} - }; - auto step = [&]() { - auto now_opt = virtual_now(); - if (now_opt.has_value()) run_main_iter(*now_opt); - }; - size_t n_imu = 0, n_lid = 0; - uint8_t type = 0; - while (g_running.load() && bf.read(reinterpret_cast(&type), 1)) { - if (type == 0) { - double rec[7]; - if (!bf.read(reinterpret_cast(rec), sizeof(rec))) break; - auto imu_msg = boost::make_shared(); - imu_msg->header.seq = 0; - imu_msg->header.stamp = custom_messages::Time().fromSec(rec[0]); - imu_msg->header.frame_id = "livox_frame"; - imu_msg->orientation.x = 0.0; - imu_msg->orientation.y = 0.0; - imu_msg->orientation.z = 0.0; - imu_msg->orientation.w = 1.0; - imu_msg->linear_acceleration.x = rec[1]; - imu_msg->linear_acceleration.y = rec[2]; - imu_msg->linear_acceleration.z = rec[3]; - imu_msg->angular_velocity.x = rec[4]; - imu_msg->angular_velocity.y = rec[5]; - imu_msg->angular_velocity.z = rec[6]; - for (int j = 0; j < 9; ++j) { - imu_msg->orientation_covariance[j] = 0.0; - imu_msg->angular_velocity_covariance[j] = 0.0; - imu_msg->linear_acceleration_covariance[j] = 0.0; - } - advance_clock(static_cast(rec[0] * 1e9)); - g_fastlio->feed_imu(imu_msg); - step(); - ++n_imu; - } else if (type == 1) { - double stamp_sec = 0.0; - uint64_t timebase = 0; - uint32_t point_num = 0; - if (!bf.read(reinterpret_cast(&stamp_sec), 8)) break; - if (!bf.read(reinterpret_cast(&timebase), 8)) break; - if (!bf.read(reinterpret_cast(&point_num), 4)) break; - auto lidar_msg = boost::make_shared(); - lidar_msg->header.seq = 0; - lidar_msg->header.stamp = custom_messages::Time().fromSec(stamp_sec); - lidar_msg->header.frame_id = "livox_frame"; - lidar_msg->timebase = timebase; - lidar_msg->lidar_id = 0; - for (int j = 0; j < 3; ++j) lidar_msg->rsvd[j] = 0; - lidar_msg->point_num = point_num; - lidar_msg->points.resize(point_num); - for (uint32_t i = 0; i < point_num; ++i) { - uint32_t off = 0; - float xyz[3] = {0, 0, 0}; - uint8_t meta[3] = {0, 0, 0}; - if (!bf.read(reinterpret_cast(&off), 4) || - !bf.read(reinterpret_cast(xyz), 12) || - !bf.read(reinterpret_cast(meta), 3)) { - g_running.store(false); - break; - } - custom_messages::CustomPoint& cp = lidar_msg->points[i]; - cp.offset_time = off; - cp.x = static_cast(xyz[0]); - cp.y = static_cast(xyz[1]); - cp.z = static_cast(xyz[2]); - cp.reflectivity = meta[0]; - cp.tag = meta[1]; - cp.line = meta[2]; - } - advance_clock(static_cast(stamp_sec * 1e9)); - g_fastlio->feed_lidar(lidar_msg); - step(); - ++n_lid; - } else { - fprintf(stderr, "[bagframes] bad record type %u\n", type); - break; - } - } - printf("[bagframes] done: imu=%zu lidar=%zu\n", n_imu, n_lid); - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - g_running.store(false); - return; - } - pcap_replay::Replayer rep; - rep.path = replay_pcap; - rep.host_point_port = static_cast(ports.host_point_data); - rep.host_imu_port = static_cast(ports.host_imu_data); - rep.on_point = [](LivoxLidarEthernetPacket* p) { - on_point_cloud(0, 0, p, nullptr); - }; - rep.on_imu = [](LivoxLidarEthernetPacket* p) { - on_imu_data(0, 0, p, nullptr); - }; - rep.on_clock = [](uint64_t pcap_ts_ns) { - // Deterministic mode already pushed pkt->timestamp; don't - // overwrite with the pcap ts. - if (g_deterministic_clock.load()) { - return; - } - g_virtual_clock_ns.store(pcap_ts_ns); - }; - rep.running = &g_running; - if (g_deterministic_clock.load() && !replay_dual_thread) { - // Serial replay: feeder drives the EKF synchronously per packet, - // unpaced. Feed+process strictly serialized → reproducible, - // matching Point-LIO's single-executor semantics. The realtime - // path's interleaving race makes even clean data diverge. - rep.realtime = false; - rep.on_iter = [&]() { - auto now_opt = virtual_now(); - if (now_opt.has_value()) run_main_iter(*now_opt); - }; - } else { - // Realtime path: feeder paced at wall clock, main thread drives - // run_main_iter. For wall-clock replay and live-race repro. - rep.realtime = true; - } - rep.skip_until_ns = replay_skip_until_ns; - rep.dual_thread = replay_dual_thread; - rep.run(); - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - g_running.store(false); - }); - } else { - if (!livox_common::init_livox_sdk(host_ip, lidar_ip, ports, debug)) { - return 1; - } - SetLivoxLidarPointCloudCallBack(on_point_cloud, nullptr); - SetLivoxLidarImuDataCallback(on_imu_data, nullptr); - SetLivoxLidarInfoChangeCallback(on_info_change, nullptr); - if (!LivoxLidarSdkStart()) { - fprintf(stderr, "Error: LivoxLidarSdkStart failed\n"); - LivoxLidarSdkUninit(); - return 1; - } - if (debug) printf("[fastlio2] SDK started, waiting for device...\n"); + // Packet source: Livox SDK callbacks from its own threads feed the + // accumulator/EKF; the main thread below owns run_main_iter. + if (!livox_common::init_livox_sdk(host_ip, lidar_ip, ports, debug)) { + return 1; + } + SetLivoxLidarPointCloudCallBack(on_point_cloud, nullptr); + SetLivoxLidarImuDataCallback(on_imu_data, nullptr); + SetLivoxLidarInfoChangeCallback(on_info_change, nullptr); + if (!LivoxLidarSdkStart()) { + fprintf(stderr, "Error: LivoxLidarSdkStart failed\n"); + LivoxLidarSdkUninit(); + return 1; } + if (debug) printf("[fastlio2] SDK started, waiting for device...\n"); - // Bag-frame replay drives run_main_iter from the feeder, so the main thread - // must stay out of the EKF regardless of deterministic_clock — else both - // co-drive run_main_iter and race on the shared measurement cloud. - const bool serial_replay = - g_replay_mode.load() && !replay_dual_thread && - (g_deterministic_clock.load() || !replay_bagframes.empty()); while (g_running.load()) { - if (serial_replay) { - // Feeder drives run_main_iter; main thread only services LCM. - lcm.handleTimeout(10); - continue; - } auto loop_start = std::chrono::high_resolution_clock::now(); - auto now_opt = virtual_now(); - if (!now_opt.has_value()) { - // No clock yet (replay feeder hasn't read a packet). - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - continue; - } - run_main_iter(*now_opt); + run_main_iter(std::chrono::steady_clock::now()); lcm.handleTimeout(0); @@ -946,12 +656,7 @@ int main(int argc, char** argv) { // Cleanup if (debug) printf("[fastlio2] Shutting down...\n"); g_fastlio = nullptr; - if (replay_thread.joinable()) { - replay_thread.join(); - } - if (!g_replay_mode.load()) { - LivoxLidarSdkUninit(); - } + LivoxLidarSdkUninit(); g_lcm = nullptr; if (debug) printf("[fastlio2] Done.\n"); diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/pcap_replay.hpp b/dimos/hardware/sensors/lidar/pointlio/cpp/pcap_replay.hpp deleted file mode 100644 index d4dd2d3b73..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/pcap_replay.hpp +++ /dev/null @@ -1,347 +0,0 @@ -// Copyright 2026 Dimensional Inc. -// SPDX-License-Identifier: Apache-2.0 -// -// Read a pcap of recorded Mid-360 UDP traffic and feed each point/imu -// payload to the existing SDK callbacks. Used by `--replay_pcap` to bypass -// the Livox SDK for deterministic offline regression testing. - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "livox_lidar_def.h" - -namespace pcap_replay { - -constexpr uint32_t PCAP_MAGIC_LE_US = 0xa1b2c3d4u; -constexpr uint32_t PCAP_MAGIC_LE_NS = 0xa1b23c4du; -constexpr uint32_t LINKTYPE_ETHERNET = 1u; -constexpr uint16_t ETHERTYPE_IPV4 = 0x0800u; -constexpr uint8_t IPPROTO_UDP = 17u; -constexpr size_t ETH_HDR_LEN = 14; -constexpr size_t IP_MIN_HDR_LEN = 20; -constexpr size_t UDP_HDR_LEN = 8; -constexpr size_t LIVOX_ETH_HDR_LEN = 36; - -using PacketCb = std::function; -using ClockCb = std::function; -using IterCb = std::function; - -struct Replayer { - std::string path; - uint16_t host_point_port = 0; - uint16_t host_imu_port = 0; - PacketCb on_point; - PacketCb on_imu; - ClockCb on_clock; - // Runs the main-loop body inline after each packet so feed+process stay on - // one thread, avoiding a feeder-vs-main-loop race on accumulator contents. - IterCb on_iter; - std::atomic* running = nullptr; - bool realtime = true; - // Drop packets with sensor ts < this to mimic the SDK warmup window from a - // paired live run. Sensor ts is bit-identical live vs replay; wall pcap_ts - // would be off by SDK delivery latency. - uint64_t skip_until_ns = 0; - // Feed point/IMU on two threads (vs one serial feeder) to reproduce the - // live SDK's independent delivery threads, exposing point/IMU overlap. - // Requires wall clock (deterministic_clock=false). - bool dual_thread = false; - - struct Pkt { - uint64_t pcap_ts_ns = 0; - bool is_point = false; - std::vector payload; - }; - - // Parse pcap into point/IMU streams, applying skip window. False on bad file. - bool prebuffer(std::vector& point_pkts, std::vector& imu_pkts) { - std::ifstream f(path, std::ios::binary); - if (!f) { - fprintf(stderr, "[replay] cannot open %s\n", path.c_str()); - return false; - } - uint8_t global_hdr[24]; - f.read(reinterpret_cast(global_hdr), 24); - if (!f) { - fprintf(stderr, "[replay] short read on pcap global header\n"); - return false; - } - uint32_t magic; - std::memcpy(&magic, global_hdr, 4); - const bool nanos = (magic == PCAP_MAGIC_LE_NS); - if (magic != PCAP_MAGIC_LE_US && magic != PCAP_MAGIC_LE_NS) { - fprintf(stderr, "[replay] unsupported pcap magic 0x%08x\n", magic); - return false; - } - uint32_t linktype; - std::memcpy(&linktype, global_hdr + 20, 4); - if (linktype != LINKTYPE_ETHERNET) { - fprintf(stderr, "[replay] unsupported linktype %u (need ETHERNET=1)\n", linktype); - return false; - } - uint8_t rec_hdr[16]; - std::vector buf; - while (true) { - f.read(reinterpret_cast(rec_hdr), 16); - if (!f) break; - uint32_t ts_sec, ts_sub, incl_len, orig_len; - std::memcpy(&ts_sec, rec_hdr + 0, 4); - std::memcpy(&ts_sub, rec_hdr + 4, 4); - std::memcpy(&incl_len, rec_hdr + 8, 4); - std::memcpy(&orig_len, rec_hdr + 12, 4); - (void)orig_len; - const uint64_t pcap_ts_ns = - static_cast(ts_sec) * 1'000'000'000ULL + - (nanos ? static_cast(ts_sub) : static_cast(ts_sub) * 1000ULL); - buf.resize(incl_len); - f.read(reinterpret_cast(buf.data()), incl_len); - if (!f) break; - if (buf.size() < ETH_HDR_LEN) continue; - uint16_t ethertype = (static_cast(buf[12]) << 8) | buf[13]; - if (ethertype != ETHERTYPE_IPV4) continue; - size_t ip_off = ETH_HDR_LEN; - if (buf.size() < ip_off + IP_MIN_HDR_LEN) continue; - uint8_t vihl = buf[ip_off]; - if ((vihl >> 4) != 4) continue; - int ihl = (vihl & 0x0f) * 4; - if (ihl < static_cast(IP_MIN_HDR_LEN)) continue; - if (buf[ip_off + 9] != IPPROTO_UDP) continue; - size_t udp_off = ip_off + ihl; - if (buf.size() < udp_off + UDP_HDR_LEN) continue; - uint16_t dst_port = (static_cast(buf[udp_off + 2]) << 8) | buf[udp_off + 3]; - uint16_t udp_len = (static_cast(buf[udp_off + 4]) << 8) | buf[udp_off + 5]; - size_t payload_off = udp_off + UDP_HDR_LEN; - size_t payload_end = std::min(buf.size(), static_cast(udp_off + udp_len)); - if (payload_end <= payload_off) continue; - size_t payload_len = payload_end - payload_off; - if (payload_len < LIVOX_ETH_HDR_LEN) continue; - const bool is_point = (dst_port == host_point_port); - const bool is_imu = (dst_port == host_imu_port); - if (!is_point && !is_imu) continue; - if (skip_until_ns > 0) { - auto* lp = reinterpret_cast(buf.data() + payload_off); - uint64_t pkt_ts; - std::memcpy(&pkt_ts, lp->timestamp, sizeof(uint64_t)); - if (pkt_ts < skip_until_ns) continue; - } - Pkt p; - p.pcap_ts_ns = pcap_ts_ns; - p.is_point = is_point; - p.payload.assign(buf.begin() + static_cast(payload_off), - buf.begin() + static_cast(payload_end)); - (is_point ? point_pkts : imu_pkts).emplace_back(std::move(p)); - } - return true; - } - - // Pace one stream against the shared wall anchor; own thread in dual mode. - void feed_stream(const std::vector& pkts, const PacketCb& cb, - std::chrono::steady_clock::time_point start_wall, - uint64_t first_pcap_ts_ns) { - for (const auto& p : pkts) { - if (running != nullptr && !running->load()) return; - if (realtime) { - auto target = start_wall + - std::chrono::nanoseconds(p.pcap_ts_ns - first_pcap_ts_ns); - auto now = std::chrono::steady_clock::now(); - if (target > now) std::this_thread::sleep_until(target); - } - auto* livox_pkt = reinterpret_cast( - const_cast(p.payload.data())); - if (cb) cb(livox_pkt); - } - } - - // Two-thread feeder; main loop drains accumulator as in live. Wall-clock - // mode only (no on_clock/on_iter). - bool run_dual() { - std::vector point_pkts, imu_pkts; - if (!prebuffer(point_pkts, imu_pkts)) return false; - printf("[replay] dual-thread: point=%zu imu=%zu (port=%u imu=%u)\n", - point_pkts.size(), imu_pkts.size(), host_point_port, host_imu_port); - uint64_t first_ts = UINT64_MAX; - if (!point_pkts.empty()) first_ts = std::min(first_ts, point_pkts.front().pcap_ts_ns); - if (!imu_pkts.empty()) first_ts = std::min(first_ts, imu_pkts.front().pcap_ts_ns); - if (first_ts == UINT64_MAX) { - printf("[replay] dual-thread: no packets\n"); - return true; - } - auto start_wall = std::chrono::steady_clock::now(); - std::thread pt([&]() { feed_stream(point_pkts, on_point, start_wall, first_ts); }); - std::thread it([&]() { feed_stream(imu_pkts, on_imu, start_wall, first_ts); }); - pt.join(); - it.join(); - printf("[replay] dual-thread done\n"); - return true; - } - - bool run() { - if (dual_thread) { - return run_dual(); - } - std::ifstream f(path, std::ios::binary); - if (!f) { - fprintf(stderr, "[replay] cannot open %s\n", path.c_str()); - return false; - } - - uint8_t global_hdr[24]; - f.read(reinterpret_cast(global_hdr), 24); - if (!f) { - fprintf(stderr, "[replay] short read on pcap global header\n"); - return false; - } - uint32_t magic; - std::memcpy(&magic, global_hdr, 4); - const bool nanos = (magic == PCAP_MAGIC_LE_NS); - if (magic != PCAP_MAGIC_LE_US && magic != PCAP_MAGIC_LE_NS) { - fprintf(stderr, "[replay] unsupported pcap magic 0x%08x\n", magic); - return false; - } - uint32_t linktype; - std::memcpy(&linktype, global_hdr + 20, 4); - if (linktype != LINKTYPE_ETHERNET) { - fprintf(stderr, "[replay] unsupported linktype %u (need ETHERNET=1)\n", linktype); - return false; - } - - printf("[replay] reading %s (port=%u imu=%u realtime=%d)\n", - path.c_str(), host_point_port, host_imu_port, realtime ? 1 : 0); - - uint64_t first_pcap_ts_ns = 0; - std::chrono::steady_clock::time_point start_wall; - bool seeded = false; - - size_t pkts = 0, pts = 0, imu = 0, other = 0; - uint8_t rec_hdr[16]; - std::vector buf; - - while (running == nullptr || running->load()) { - f.read(reinterpret_cast(rec_hdr), 16); - if (!f) { - break; - } - - uint32_t ts_sec, ts_sub, incl_len, orig_len; - std::memcpy(&ts_sec, rec_hdr + 0, 4); - std::memcpy(&ts_sub, rec_hdr + 4, 4); - std::memcpy(&incl_len, rec_hdr + 8, 4); - std::memcpy(&orig_len, rec_hdr + 12, 4); - (void)orig_len; - - const uint64_t pcap_ts_ns = - static_cast(ts_sec) * 1'000'000'000ULL + - (nanos ? static_cast(ts_sub) : static_cast(ts_sub) * 1000ULL); - - buf.resize(incl_len); - f.read(reinterpret_cast(buf.data()), incl_len); - if (!f) { - break; - } - pkts++; - - if (buf.size() < ETH_HDR_LEN) { - continue; - } - uint16_t ethertype = (static_cast(buf[12]) << 8) | buf[13]; - if (ethertype != ETHERTYPE_IPV4) { - continue; - } - size_t ip_off = ETH_HDR_LEN; - if (buf.size() < ip_off + IP_MIN_HDR_LEN) { - continue; - } - uint8_t vihl = buf[ip_off]; - if ((vihl >> 4) != 4) { - continue; - } - int ihl = (vihl & 0x0f) * 4; - if (ihl < static_cast(IP_MIN_HDR_LEN)) { - continue; - } - if (buf[ip_off + 9] != IPPROTO_UDP) { - continue; - } - size_t udp_off = ip_off + ihl; - if (buf.size() < udp_off + UDP_HDR_LEN) { - continue; - } - uint16_t dst_port = (static_cast(buf[udp_off + 2]) << 8) | buf[udp_off + 3]; - uint16_t udp_len = (static_cast(buf[udp_off + 4]) << 8) | buf[udp_off + 5]; - size_t payload_off = udp_off + UDP_HDR_LEN; - size_t payload_end = std::min(buf.size(), static_cast(udp_off + udp_len)); - if (payload_end <= payload_off) { - continue; - } - size_t payload_len = payload_end - payload_off; - if (payload_len < LIVOX_ETH_HDR_LEN) { - continue; - } - - auto* livox_pkt = - reinterpret_cast(buf.data() + payload_off); - - // Skip packets the live SDK wouldn't have delivered yet. - if (skip_until_ns > 0) { - uint64_t pkt_ts; - std::memcpy(&pkt_ts, livox_pkt->timestamp, sizeof(uint64_t)); - if (pkt_ts < skip_until_ns) { - continue; - } - } - - if (realtime) { - if (!seeded) { - first_pcap_ts_ns = pcap_ts_ns; - start_wall = std::chrono::steady_clock::now(); - seeded = true; - } else { - auto target = start_wall + std::chrono::nanoseconds(pcap_ts_ns - first_pcap_ts_ns); - auto now = std::chrono::steady_clock::now(); - if (target > now) { - std::this_thread::sleep_until(target); - } - } - } - - if (dst_port == host_point_port) { - if (on_point) { - on_point(livox_pkt); - } - pts++; - } else if (dst_port == host_imu_port) { - if (on_imu) { - on_imu(livox_pkt); - } - imu++; - } else { - other++; - } - // Advance clock only after the payload is accumulated, else a scan - // could be emitted missing this packet. - if (on_clock) { - on_clock(pcap_ts_ns); - } - - // Serialize feed and process in replay mode. - if (on_iter) { - on_iter(); - } - } - - printf("[replay] done: %zu pcap records (point=%zu imu=%zu other=%zu)\n", - pkts, pts, imu, other); - return true; - } -}; - -} // namespace pcap_replay diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp b/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp index 8b2f2dc442..ddd72eac47 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp @@ -72,7 +72,7 @@ struct Scope { }; // Print one line per section to stderr every FLUSH_INTERVAL, then reset. -// Mutex serialises flushes across threads (replay feeder vs live main loop). +// Mutex serialises flushes across threads (SDK callbacks vs main loop). inline void maybe_flush(std::chrono::steady_clock::time_point now) { if (!fastlio_debug) { return; diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index 9f0c7db28d..65be5dee6f 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -128,21 +128,6 @@ class PointLioConfig(NativeModuleConfig): # Resolved in __post_init__, passed as --config_path to the binary config_path: str | None = None - # Offline replay. When set, the C++ binary skips SDK init and feeds - # packets from this pcap into the same callbacks the SDK would. - replay_pcap: Path | None = None - # Replay-only: drop pcap records with sensor ts < this. - replay_skip_until_ns: int | None = None - # Live-only: path where the binary writes the first-callback wall_ns. - first_packet_marker: Path | None = None - # Drive scan boundaries + publish ts off the sensor packet timestamp - # for bit-reproducible offline replay. - deterministic_clock: bool = False - # Replay-only: feed point and IMU packets on two separate threads to - # mimic the live Livox SDK's concurrent delivery. Use with - # deterministic_clock=False to reproduce live thread-interleaving. - replay_dual_thread: bool = False - # init_pose is computed from mount; config is resolved to config_path init_pose: list[float] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] cli_exclude: frozenset[str] = frozenset({"config", "mount", "odom_parent_frame_id"}) @@ -174,8 +159,7 @@ class PointLio(NativeModule, perception.Lidar, perception.Odometry): @rpc def start(self) -> None: - if self.config.replay_pcap is None: - self._validate_network() + self._validate_network() super().start() self.register_disposable( Disposable(self.odometry.transport.subscribe(self._on_odom_for_tf, self.odometry)) diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py deleted file mode 100644 index 2055b0a0e1..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py +++ /dev/null @@ -1,364 +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. - -"""Run Point-LIO over a .pcap and write its outputs into a .db. - -Given a Livox Mid-360 pcap capture, this streams the pcap through the Point-LIO -native module (deterministic clock, single feeder -> never loads the whole pcap -into memory) and writes two streams into a memory2 SQLite database: - -* ``pointlio_odometry`` -- the IESKF pose at the native odom rate (~30 Hz). -* ``pointlio_lidar`` -- the registered (deskewed, odom-frame) point cloud at the - native pointcloud rate (~10 Hz). - -The ``--db`` is optional. With no existing db the tool builds one **from -scratch** (omit ``--db`` and it defaults to ``.db`` next to the pcap). -With an existing db the two streams are appended and time-aligned onto the db's -clock, so Point-LIO output can be compared against whatever it already holds -(e.g. a fastlio replay). - -If either stream already exists in the db the run aborts, unless ``--force`` is -given, in which case the existing ``pointlio_odometry`` and ``pointlio_lidar`` -streams are dropped before the new ones are written. - -Timing conversion ------------------ -Point-LIO's deterministic output timestamps are in *sensor-boot seconds* (the -Livox packet clock, small values like 1588.x). The target db may use a different -clock for its existing streams: - -* db already on the sensor clock (e.g. a fastlio replay db, min ts < 1e8): - offset 0 -- both replay the same pcap packet clock, so they line up exactly. -* db on wall-clock unix time (min ts > 1e9): start-align by shifting Point-LIO's - first odom ts onto the db's earliest existing ts. -* db has no existing timestamped rows: offset 0. - -Pass ``--time-offset`` to override the auto choice. - -Usage (from the dimos6 venv):: - - source .venv/bin/activate - - # Build a fresh db from scratch (no existing db needed). The ruwik2_part3 - # sample pcap (120s, includes the velocity-spike segment) is in LFS: - PCAP=$(python -c "from dimos.utils.data import get_data; \ - print(get_data('ruwik2_part3/ruwik2_part3.pcap'))") - python -m dimos.hardware.sensors.lidar.pointlio.tools.pcap_to_db --pcap "$PCAP" - # -> writes ruwik2_part3.db next to the sample with pointlio_odometry - # + pointlio_lidar. - - # Or append into an existing recording db for comparison: - python -m dimos.hardware.sensors.lidar.pointlio.tools.pcap_to_db \ - --pcap /path/to/capture.pcap --db /path/to/memory.db -""" - -from __future__ import annotations - -import argparse -from collections.abc import AsyncIterator -import math -from pathlib import Path -import sqlite3 -import sys -import time - -from dimos.core.module import Module, ModuleConfig -from dimos.core.stream import In -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 - -# Below this the db's existing timestamps are sensor-boot seconds, not unix time. -_SENSOR_CLOCK_MAX = 1e8 -# Strictly-increasing tie-breaker so two odom samples never collide on ts. -_EPS = 1e-9 -# Poll the db on this cadence while the replay drains the pcap. -_POLL_SEC = 1.0 -# Stop after the odom stream has been stagnant this long (pcap fully drained). -_STAGNANT_SEC = 4.0 - - -class _RecConfig(ModuleConfig): - """Configures the recorder with the target db and timing conversion.""" - - db_path: str = "" - # Earliest existing ts in the db, or -1.0 if the db has no timestamped rows. - ref_start_ts: float = -1.0 - # Explicit offset override; NaN means auto-derive from ref_start_ts. - time_offset: float = float("nan") - - -class _Rec(Module): - """Append Point-LIO odometry + lidar into a SQLite db with ts conversion. - - Underscore-prefixed so the blueprint registry generator skips it — this is - an internal helper for the tool, not a public robot module. - """ - - config: _RecConfig - pointlio_odometry: In[Odometry] - pointlio_lidar: In[PointCloud2] - _offset: float | None = None - _last_odom_ts: float = 0.0 - _last_lidar_ts: float = 0.0 - _odom_count: int = 0 - _lidar_count: int = 0 - - async def main(self) -> AsyncIterator[None]: - from dimos.memory2.store.sqlite import SqliteStore - - self._store = SqliteStore(path=self.config.db_path) - self._os = self._store.stream("pointlio_odometry", Odometry) - self._ls = self._store.stream("pointlio_lidar", PointCloud2) - yield - self._store.stop() - - def _resolve_offset(self, first_ts: float) -> float: - override = self.config.time_offset - if not math.isnan(override): - return override - ref = self.config.ref_start_ts - if ref < 0.0 or ref < _SENSOR_CLOCK_MAX: - # Empty db, or db already on the sensor clock -> exact alignment. - return 0.0 - # db on wall-clock time -> start-align Point-LIO onto the db's earliest ts. - return ref - first_ts - - def _aligned_ts(self, raw_ts: float, last_ts: float) -> float: - """Convert a sensor ts onto the db clock, kept strictly above last_ts.""" - if self._offset is None: - self._offset = self._resolve_offset(raw_ts) - return max(raw_ts + self._offset, last_ts + _EPS) - - async def handle_pointlio_odometry(self, v: Odometry) -> None: - # `is not None`, not `or`: a real sensor ts of 0.0 must not fall back to - # wall time (would misclassify the stream's clock in _resolve_offset). - raw_ts_raw = getattr(v, "ts", None) - raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() - ts = self._aligned_ts(raw_ts, self._last_odom_ts) - self._last_odom_ts = ts - pose = getattr(v, "pose", None) - pose_inner = getattr(pose, "pose", None) if pose is not None else None - self._os.append(v, ts=ts, pose=pose_inner) - self._odom_count += 1 - - async def handle_pointlio_lidar(self, v: PointCloud2) -> None: - raw_ts_raw = getattr(v, "ts", None) - raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() - ts = self._aligned_ts(raw_ts, self._last_lidar_ts) - self._last_lidar_ts = ts - self._ls.append(v, ts=ts) - self._lidar_count += 1 - - -def _db_ref_start_ts(db_path: Path) -> float: - """Min ts across the db's existing streams, or -1.0 if none/absent.""" - if not db_path.exists(): - return -1.0 - con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) - try: - tables = [ - row[0] - for row in con.execute("SELECT name FROM sqlite_master WHERE type='table'").fetchall() - ] - best: float | None = None - for table in tables: - if table.startswith("_") or table.startswith("sqlite_"): - continue - try: - # vec0/rtree virtual tables (sqlite-vec etc.) raise "no such - # module" here when the extension isn't loaded -- skip them. - cols = [c[1] for c in con.execute(f"PRAGMA table_info('{table}')").fetchall()] - if "ts" not in cols: - continue - row = con.execute(f"SELECT MIN(ts) FROM '{table}'").fetchone() - except sqlite3.OperationalError: - continue - if row and row[0] is not None: - best = row[0] if best is None else min(best, row[0]) - return best if best is not None else -1.0 - finally: - con.close() - - -def _table_stats(db_path: Path, table: str) -> tuple[int, float, float]: - """(count, min_ts, max_ts) for a stream table; zeros if absent.""" - if not db_path.exists(): - return 0, 0.0, 0.0 - con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) - try: - try: - row = con.execute(f"SELECT COUNT(*), MIN(ts), MAX(ts) FROM '{table}'").fetchone() - except sqlite3.OperationalError: - return 0, 0.0, 0.0 - cnt = row[0] or 0 - return cnt, row[1] or 0.0, row[2] or 0.0 - finally: - con.close() - - -def _run(args: argparse.Namespace) -> int: - pcap_path = Path(args.pcap).expanduser().resolve() - if not pcap_path.exists(): - print(f"[pcap_to_db] missing pcap: {pcap_path}", file=sys.stderr) - return 2 - if args.max_sensor_sec < 0: - print("[pcap_to_db] --max-sensor-sec must be >= 0", file=sys.stderr) - return 2 - # --db is optional: with no existing db, build one from scratch. When - # omitted the output defaults to .db next to the pcap, so a fresh - # db can be generated with just --pcap. - db_path = Path(args.db).expanduser().resolve() if args.db else pcap_path.with_suffix(".db") - db_existed = db_path.exists() - db_path.parent.mkdir(parents=True, exist_ok=True) - - from dimos.core.coordination.blueprints import autoconnect - from dimos.core.coordination.module_coordinator import ModuleCoordinator - from dimos.hardware.sensors.lidar.pointlio.module import PointLio - from dimos.memory2.store.sqlite import SqliteStore - - pointlio_streams = ("pointlio_odometry", "pointlio_lidar") - store = SqliteStore(path=str(db_path)) - try: - existing = sorted(set(store.list_streams()) & set(pointlio_streams)) - if existing and not args.force: - print( - f"[pcap_to_db] {db_path.name} already has {existing}; pass --force to overwrite", - file=sys.stderr, - ) - return 2 - for name in existing: - store.delete_stream(name) - if existing: - print(f"[pcap_to_db] --force: dropped existing {existing}", flush=True) - finally: - store.stop() - - ref_start_ts = _db_ref_start_ts(db_path) - time_offset = float("nan") if args.time_offset is None else args.time_offset - if not math.isnan(time_offset): - offset_desc = f"explicit {time_offset:+.3f}s" - elif ref_start_ts < 0.0: - offset_desc = "auto: db empty -> 0" - elif ref_start_ts < _SENSOR_CLOCK_MAX: - offset_desc = f"auto: db sensor-clock (R0={ref_start_ts:.2f}) -> 0" - else: - offset_desc = f"auto: db wall-clock (R0={ref_start_ts:.2f}) -> start-align" - print( - f"[pcap_to_db] pcap={pcap_path.name} db={db_path.name} " - f"({'append' if db_existed else 'new'}) " - f"odom_freq={args.odom_freq}Hz offset={offset_desc}", - flush=True, - ) - - blueprint = autoconnect( - PointLio.blueprint( - frame_id="world", - odom_freq=args.odom_freq, - replay_pcap=pcap_path, - deterministic_clock=True, - replay_dual_thread=False, - debug=False, - ).remappings( - [ - (PointLio, "odometry", "pointlio_odometry"), - (PointLio, "lidar", "pointlio_lidar"), - ] - ), - _Rec.blueprint( - db_path=str(db_path), - ref_start_ts=ref_start_ts, - time_offset=time_offset, - ), - ).global_config(n_workers=4, robot_model="mid360_pointlio_pcap_to_db") - coord = ModuleCoordinator.build(blueprint) - - t0 = time.time() - last_max = 0.0 - first_max: float | None = None - stagnant_since: float | None = None - try: - while True: - time.sleep(_POLL_SEC) - cnt, min_ts, max_ts = _table_stats(db_path, "pointlio_odometry") - if cnt == 0: - continue - if first_max is None: - first_max = min_ts - if args.max_sensor_sec > 0 and (max_ts - first_max) >= args.max_sensor_sec: - print( - f"[pcap_to_db] reached --max-sensor-sec={args.max_sensor_sec:.1f}s", - flush=True, - ) - break - if max_ts == last_max: - if stagnant_since is None: - stagnant_since = time.time() - elif time.time() - stagnant_since > _STAGNANT_SEC: - break - else: - last_max = max_ts - stagnant_since = None - finally: - coord.stop() - - o_cnt, o_min, o_max = _table_stats(db_path, "pointlio_odometry") - l_cnt = _table_stats(db_path, "pointlio_lidar")[0] - span = o_max - o_min - print( - f"[pcap_to_db] done odom={o_cnt} lidar={l_cnt} " - f"ts=[{o_min:.3f}, {o_max:.3f}] span={span:.1f}s " - f"wall={time.time() - t0:.1f}s", - flush=True, - ) - return 0 - - -def main(argv: list[str]) -> int: - parser = argparse.ArgumentParser(description=__doc__) - parser.add_argument("--pcap", required=True, help="Livox Mid-360 pcap capture") - parser.add_argument( - "--db", - default=None, - help="target memory2 SQLite db. If it exists, pointlio streams are appended/aligned " - "onto its clock; if it doesn't, a fresh db is built from scratch. " - "Omit to default to .db next to the pcap.", - ) - parser.add_argument( - "--odom-freq", - type=float, - default=30.0, - help="Point-LIO odometry publish rate in Hz (default 30)", - ) - parser.add_argument( - "--max-sensor-sec", - type=float, - default=0.0, - help="stop after this many seconds of sensor time (0 = whole pcap)", - ) - parser.add_argument( - "--time-offset", - type=float, - default=None, - help="seconds added to every output ts; omit to auto-derive from the db clock", - ) - parser.add_argument( - "--force", - action="store_true", - help="overwrite existing pointlio_odometry/pointlio_lidar streams in the db", - ) - return _run(parser.parse_args(argv)) - - -if __name__ == "__main__": - raise SystemExit(main(sys.argv[1:])) From a24e29f0b5e86ec325decef04cd435487d2d5444 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 20:44:25 -0700 Subject: [PATCH 067/185] feat(pointlio): new pcap_to_db driven by the rust virtual_mid360 replay Replaces the removed in-process pcap_to_db. Orchestrates the over-the-wire replay: stands up a drv/lidar veth pair (via sudo), runs virtual_mid360 in the lidar netns streaming the pcap, and a live Point-LIO + recorder coordinator in the drv netns, recording pointlio_odometry + pointlio_lidar into a memory2 SQLite db. Keeps the old db ts-alignment / --force / from- scratch behavior. Needs CAP_NET_ADMIN (shells out to sudo). Verified e2e on ruwik2_part3: odom ~30Hz, lidar ~10Hz. --- .../lidar/pointlio/tools/pcap_to_db.py | 536 ++++++++++++++++++ 1 file changed, 536 insertions(+) create mode 100644 dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py new file mode 100644 index 0000000000..286f592110 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py @@ -0,0 +1,536 @@ +# 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. + +"""Run Point-LIO over a .pcap (via the rust virtual_mid360 replay) → .db. + +Point-LIO has no in-process replay anymore; the only replay path is the +``virtual_mid360`` rust module, which replays a recorded Mid-360 pcap *over the +wire* so an unmodified live Point-LIO connects to it as real hardware. This tool +orchestrates that end to end and records Point-LIO's outputs into a memory2 +SQLite database: + +* ``pointlio_odometry`` — the IESKF pose at the native odom rate. +* ``pointlio_lidar`` — the sensor-frame point cloud at the native rate. + +It stands up two network namespaces joined by a veth: ``virtual_mid360`` runs in +the ``lidar`` ns and streams the pcap; live Point-LIO + the recorder run together +in the ``drv`` ns (one coordinator, so their LCM streams wire up normally). Since +this creates network namespaces + veths, **it needs CAP_NET_ADMIN** — it shells +out to ``sudo`` for those steps (so passwordless sudo, or running as root, is +required). Replay is real time (Point-LIO is not deterministic), so two runs over +the same pcap will differ slightly. + +The ``--db`` is optional: with no existing db a fresh one is built from scratch +(defaults to ``.db`` next to the pcap). With an existing db the two streams +are appended and time-aligned onto its clock, so Point-LIO output can be compared +against whatever it already holds (e.g. a fastlio replay). If either stream +already exists the run aborts unless ``--force`` drops them first. + +Run it as your normal user from the dimos6 venv — it shells out to ``sudo`` +for the privileged netns/veth bits itself:: + + source .venv/bin/activate + PCAP=$(python -c "from dimos.utils.data import get_data; \ + print(get_data('ruwik2_part3/ruwik2_part3.pcap'))") + python -m dimos.hardware.sensors.lidar.pointlio.tools.pcap_to_db --pcap "$PCAP" + # -> writes ruwik2_part3.db next to the sample. + +Two simultaneous runs (e.g. alongside a fastlio replay) must use distinct +namespaces/IPs — see --drv-ns / --lidar-ns / --host-ip / --lidar-ip. +""" + +from __future__ import annotations + +import argparse +from collections.abc import AsyncIterator +import json +import math +import os +from pathlib import Path +import signal +import sqlite3 +import subprocess +import sys +import time + +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +# Below this the db's existing timestamps are sensor-boot seconds, not unix time. +_SENSOR_CLOCK_MAX = 1e8 +# Strictly-increasing tie-breaker so two samples never collide on ts. +_EPS = 1e-9 +# Poll the db on this cadence while the replay drains the pcap. +_POLL_SEC = 1.0 +# Stop after the odom stream has been stagnant this long (pcap fully drained). +_STAGNANT_SEC = 5.0 +# virtual_mid360 crate dir (its `nix build .#default` produces result/bin/virtual_mid360). +# .../sensors/lidar/pointlio/tools/pcap_to_db.py -> parents[2] == .../sensors/lidar +_VM_DIR = Path(__file__).resolve().parents[2] / "livox" / "virtual_mid360" + + +class _RecConfig(ModuleConfig): + """Configures the recorder with the target db and timing conversion.""" + + db_path: str = "" + # Earliest existing ts in the db, or -1.0 if the db has no timestamped rows. + ref_start_ts: float = -1.0 + # Explicit offset override; NaN means auto-derive from ref_start_ts. + time_offset: float = float("nan") + + +class _Rec(Module): + """Append Point-LIO odometry + lidar into a SQLite db with ts conversion. + + Underscore-prefixed so the blueprint registry generator skips it — this is + an internal helper for the tool, not a public robot module. + """ + + config: _RecConfig + pointlio_odometry: In[Odometry] + pointlio_lidar: In[PointCloud2] + _offset: float | None = None + _last_odom_ts: float = 0.0 + _last_lidar_ts: float = 0.0 + _odom_count: int = 0 + _lidar_count: int = 0 + + async def main(self) -> AsyncIterator[None]: + from dimos.memory2.store.sqlite import SqliteStore + + self._store = SqliteStore(path=self.config.db_path) + self._os = self._store.stream("pointlio_odometry", Odometry) + self._ls = self._store.stream("pointlio_lidar", PointCloud2) + yield + self._store.stop() + + def _resolve_offset(self, first_ts: float) -> float: + override = self.config.time_offset + if not math.isnan(override): + return override + ref = self.config.ref_start_ts + if ref < 0.0 or ref < _SENSOR_CLOCK_MAX: + # Empty db, or db already on the sensor clock -> exact alignment. + return 0.0 + # db on wall-clock time -> start-align Point-LIO onto the db's earliest ts. + return ref - first_ts + + def _aligned_ts(self, raw_ts: float, last_ts: float) -> float: + """Convert a sensor ts onto the db clock, kept strictly above last_ts.""" + if self._offset is None: + self._offset = self._resolve_offset(raw_ts) + return max(raw_ts + self._offset, last_ts + _EPS) + + async def handle_pointlio_odometry(self, v: Odometry) -> None: + # `is not None`, not `or`: a real sensor ts of 0.0 must not fall back to + # wall time (would misclassify the stream's clock in _resolve_offset). + raw_ts_raw = getattr(v, "ts", None) + raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() + ts = self._aligned_ts(raw_ts, self._last_odom_ts) + self._last_odom_ts = ts + pose = getattr(v, "pose", None) + pose_inner = getattr(pose, "pose", None) if pose is not None else None + self._os.append(v, ts=ts, pose=pose_inner) + self._odom_count += 1 + + async def handle_pointlio_lidar(self, v: PointCloud2) -> None: + raw_ts_raw = getattr(v, "ts", None) + raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() + ts = self._aligned_ts(raw_ts, self._last_lidar_ts) + self._last_lidar_ts = ts + self._ls.append(v, ts=ts) + self._lidar_count += 1 + + +def _db_ref_start_ts(db_path: Path) -> float: + """Min ts across the db's existing streams, or -1.0 if none/absent.""" + if not db_path.exists(): + return -1.0 + con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) + try: + tables = [ + row[0] + for row in con.execute("SELECT name FROM sqlite_master WHERE type='table'").fetchall() + ] + best: float | None = None + for table in tables: + if table.startswith("_") or table.startswith("sqlite_"): + continue + try: + cols = [c[1] for c in con.execute(f"PRAGMA table_info('{table}')").fetchall()] + if "ts" not in cols: + continue + row = con.execute(f"SELECT MIN(ts) FROM '{table}'").fetchone() + except sqlite3.OperationalError: + continue + if row and row[0] is not None: + best = row[0] if best is None else min(best, row[0]) + return best if best is not None else -1.0 + finally: + con.close() + + +def _table_stats(db_path: Path, table: str) -> tuple[int, float, float]: + """(count, min_ts, max_ts) for a stream table; zeros if absent.""" + if not db_path.exists(): + return 0, 0.0, 0.0 + con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) + try: + try: + row = con.execute(f"SELECT COUNT(*), MIN(ts), MAX(ts) FROM '{table}'").fetchone() + except sqlite3.OperationalError: + return 0, 0.0, 0.0 + cnt = row[0] or 0 + return cnt, row[1] or 0.0, row[2] or 0.0 + finally: + con.close() + + +# --------------------------------------------------------------------------- +# Network-namespace orchestration (outer process; needs CAP_NET_ADMIN via sudo) +# --------------------------------------------------------------------------- + + +def _sudo() -> list[str]: + """Privilege-escalation prefix for the netns/veth commands.""" + return ["sudo"] + + +def _ns(args: list[str], check: bool = True) -> subprocess.CompletedProcess[bytes]: + return subprocess.run(_sudo() + args, check=check, capture_output=True) + + +def _teardown(drv: str, lidar: str, veth: str) -> None: + for cmd in ( + ["ip", "netns", "del", drv], + ["ip", "netns", "del", lidar], + ["ip", "link", "del", veth], + ): + _ns(cmd, check=False) + + +def _setup_netns( + drv: str, lidar: str, veth_drv: str, veth_lidar: str, host_ip: str, lidar_ip: str +) -> None: + """Recreate the drv/lidar veth pair with the Livox multicast routing.""" + _teardown(drv, lidar, veth_drv) + steps = [ + ["ip", "netns", "add", drv], + ["ip", "netns", "add", lidar], + ["ip", "link", "add", veth_drv, "type", "veth", "peer", "name", veth_lidar], + ["ip", "link", "set", veth_drv, "netns", drv], + ["ip", "link", "set", veth_lidar, "netns", lidar], + ["ip", "netns", "exec", drv, "ip", "addr", "add", f"{host_ip}/24", "dev", veth_drv], + ["ip", "netns", "exec", lidar, "ip", "addr", "add", f"{lidar_ip}/24", "dev", veth_lidar], + ] + for ns in (drv, lidar): + steps += [ + ["ip", "netns", "exec", ns, "ip", "link", "set", "lo", "up"], + ["ip", "netns", "exec", ns, "ip", "link", "set", "lo", "multicast", "on"], + ["ip", "netns", "exec", ns, "ip", "route", "add", "224.0.0.0/4", "dev", "lo"], + ] + steps += [ + ["ip", "netns", "exec", drv, "ip", "link", "set", veth_drv, "up"], + ["ip", "netns", "exec", lidar, "ip", "link", "set", veth_lidar, "up"], + ["ip", "netns", "exec", drv, "ip", "link", "set", veth_drv, "multicast", "on"], + ["ip", "netns", "exec", lidar, "ip", "link", "set", veth_lidar, "multicast", "on"], + # Mid-360 multicasts point/IMU to 224.1.1.5; broadcast detection to 255.255.255.255. + [ + "ip", + "netns", + "exec", + lidar, + "ip", + "route", + "add", + "255.255.255.255/32", + "dev", + veth_lidar, + ], + ["ip", "netns", "exec", lidar, "ip", "route", "add", "224.1.1.5/32", "dev", veth_lidar], + ] + for cmd in steps: + _ns(cmd) + + +def _resolve_vm_binary() -> str: + """Path to the virtual_mid360 binary; build it via nix if not present.""" + env = os.environ.get("VIRTUAL_MID360_BIN") + if env: + return env + out = _VM_DIR / "result" / "bin" / "virtual_mid360" + if out.exists(): + return str(out) + print("[pcap_to_db] building virtual_mid360 (nix build .#default)...", flush=True) + subprocess.run(["nix", "build", ".#default"], cwd=_VM_DIR, check=True) + return str(out) + + +def _run_outer(args: argparse.Namespace) -> int: + pcap_path = Path(args.pcap).expanduser().resolve() + if not pcap_path.exists(): + print(f"[pcap_to_db] missing pcap: {pcap_path}", file=sys.stderr) + return 2 + db_path = Path(args.db).expanduser().resolve() if args.db else pcap_path.with_suffix(".db") + db_existed = db_path.exists() + db_path.parent.mkdir(parents=True, exist_ok=True) + + # Fail fast on stream conflicts before touching the network. Only open an + # *existing* db here — a new db is created by the (root) inner so it owns it + # outright; SQLite refuses WAL writes when the file owner != the process uid. + pointlio_streams = ("pointlio_odometry", "pointlio_lidar") + if db_existed: + from dimos.memory2.store.sqlite import SqliteStore + + store = SqliteStore(path=str(db_path)) + try: + existing = sorted(set(store.list_streams()) & set(pointlio_streams)) + if existing and not args.force: + print( + f"[pcap_to_db] {db_path.name} already has {existing}; pass --force to overwrite", + file=sys.stderr, + ) + return 2 + for name in existing: + store.delete_stream(name) + if existing: + print(f"[pcap_to_db] --force: dropped existing {existing}", flush=True) + finally: + store.stop() + + ref_start_ts = _db_ref_start_ts(db_path) + vm_bin = _resolve_vm_binary() + print( + f"[pcap_to_db] pcap={pcap_path.name} db={db_path.name} " + f"({'append' if db_existed else 'new'}) rate={args.rate} " + f"ns={args.drv_ns}/{args.lidar_ns} ips={args.host_ip}/{args.lidar_ip}", + flush=True, + ) + + # An existing db is user-owned; hand it (and its WAL sidecars) to root so the + # root inner can write it (SQLite WAL refuses cross-uid writes). Restored below. + if db_existed: + for suffix in ("", "-wal", "-shm"): + p = Path(f"{db_path}{suffix}") + if p.exists(): + _ns(["chown", "0:0", str(p)], check=False) + + _setup_netns( + args.drv_ns, args.lidar_ns, args.veth_drv, args.veth_lidar, args.host_ip, args.lidar_ip + ) + vm_proc: subprocess.Popen[bytes] | None = None + inner: subprocess.Popen[bytes] | None = None + try: + # Recorder + live Point-LIO run together in the drv ns (one coordinator). + inner_cmd = [ + *_sudo(), + "ip", + "netns", + "exec", + args.drv_ns, + sys.executable, + "-m", + "dimos.hardware.sensors.lidar.pointlio.tools.pcap_to_db", + "--inner", + "--db", + str(db_path), + "--odom-freq", + str(args.odom_freq), + "--ref-start-ts", + repr(ref_start_ts), + "--time-offset", + "nan" if args.time_offset is None else repr(args.time_offset), + "--max-sensor-sec", + str(args.max_sensor_sec), + "--host-ip", + args.host_ip, + "--lidar-ip", + args.lidar_ip, + ] + inner = subprocess.Popen(inner_cmd, cwd=os.getcwd()) + + # Give Point-LIO a moment to come up before the sensor starts streaming. + time.sleep(args.warmup_sec) + vm_cfg = json.dumps( + { + "topics": {}, + "config": { + "pcap": str(pcap_path), + "rate": args.rate, + "delay": 0.0, + "lidar_ip": args.lidar_ip, + "host_ip": args.host_ip, + "lidar_netns": args.lidar_ns, + }, + } + ).encode() + vm_proc = subprocess.Popen( + [*_sudo(), "ip", "netns", "exec", args.lidar_ns, vm_bin], + stdin=subprocess.PIPE, + ) + assert vm_proc.stdin is not None + vm_proc.stdin.write(vm_cfg) + vm_proc.stdin.close() + + # The inner exits itself once the odom stream goes stagnant (pcap drained). + inner.wait() + finally: + for proc in (vm_proc, inner): + if proc and proc.poll() is None: + _ns(["pkill", "-9", "-f", "virtual_mid360"], check=False) + _ns(["pkill", "-9", "-f", "pointlio_native"], check=False) + try: + proc.wait(timeout=5) + except subprocess.TimeoutExpired: + proc.kill() + _teardown(args.drv_ns, args.lidar_ns, args.veth_drv) + # The inner coordinator ran as root (netns entry needs it) → hand the db + # files back to the invoking user. + uid, gid = os.getuid(), os.getgid() + for suffix in ("", "-wal", "-shm"): + p = Path(f"{db_path}{suffix}") + if p.exists(): + _ns(["chown", f"{uid}:{gid}", str(p)], check=False) + + o_cnt, o_min, o_max = _table_stats(db_path, "pointlio_odometry") + l_cnt = _table_stats(db_path, "pointlio_lidar")[0] + if o_cnt == 0: + print("[pcap_to_db] no odometry recorded — check the run above", file=sys.stderr) + return 1 + print( + f"[pcap_to_db] done odom={o_cnt} lidar={l_cnt} " + f"ts=[{o_min:.3f}, {o_max:.3f}] span={o_max - o_min:.1f}s", + flush=True, + ) + return 0 + + +# --------------------------------------------------------------------------- +# Inner process: live Point-LIO + recorder, already inside the drv netns +# --------------------------------------------------------------------------- + + +def _run_inner(args: argparse.Namespace) -> int: + from dimos.core.coordination.blueprints import autoconnect + from dimos.core.coordination.module_coordinator import ModuleCoordinator + from dimos.hardware.sensors.lidar.pointlio.module import PointLio + + db_path = Path(args.db) + time_offset = float("nan") if args.time_offset == "nan" else float(args.time_offset) + + blueprint = autoconnect( + PointLio.blueprint( + host_ip=args.host_ip, + lidar_ip=args.lidar_ip, + odom_freq=args.odom_freq, + debug=False, + ).remappings( + [ + (PointLio, "odometry", "pointlio_odometry"), + (PointLio, "lidar", "pointlio_lidar"), + ] + ), + _Rec.blueprint( + db_path=str(db_path), + ref_start_ts=args.ref_start_ts, + time_offset=time_offset, + ), + ).global_config(n_workers=4, robot_model="mid360_pointlio_pcap_to_db") + coord = ModuleCoordinator.build(blueprint) + + last_max = 0.0 + first_max: float | None = None + stagnant_since: float | None = None + try: + while True: + time.sleep(_POLL_SEC) + cnt, min_ts, max_ts = _table_stats(db_path, "pointlio_odometry") + if cnt == 0: + continue + if first_max is None: + first_max = min_ts + if args.max_sensor_sec > 0 and (max_ts - first_max) >= args.max_sensor_sec: + print( + f"[pcap_to_db] reached --max-sensor-sec={args.max_sensor_sec:.1f}s", flush=True + ) + break + if max_ts == last_max: + if stagnant_since is None: + stagnant_since = time.time() + elif time.time() - stagnant_since > _STAGNANT_SEC: + break + else: + last_max = max_ts + stagnant_since = None + finally: + coord.stop() + return 0 + + +def main(argv: list[str]) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--pcap", help="Livox Mid-360 pcap capture") + parser.add_argument( + "--db", + default=None, + help="target memory2 SQLite db. Existing -> append/align; missing -> built from " + "scratch. Omit to default to .db next to the pcap.", + ) + parser.add_argument( + "--rate", type=float, default=1.0, help="replay-speed multiplier (default 1.0)" + ) + parser.add_argument( + "--odom-freq", type=float, default=30.0, help="Point-LIO odometry rate Hz (default 30)" + ) + parser.add_argument( + "--max-sensor-sec", + type=float, + default=0.0, + help="stop after N sensor seconds (0 = whole pcap)", + ) + parser.add_argument( + "--time-offset", + type=float, + default=None, + help="seconds added to every output ts (auto if omitted)", + ) + parser.add_argument("--force", action="store_true", help="overwrite existing pointlio streams") + parser.add_argument( + "--warmup-sec", type=float, default=4.0, help="wait before streaming starts" + ) + # Namespace / addressing knobs (override to run two replays at once). + parser.add_argument("--drv-ns", default="drv") + parser.add_argument("--lidar-ns", default="lidar") + parser.add_argument("--veth-drv", default="veth-drv") + parser.add_argument("--veth-lidar", default="veth-lidar") + parser.add_argument("--host-ip", default="192.168.1.5") + parser.add_argument("--lidar-ip", default="192.168.1.155") + # Internal: re-exec inside the drv netns to run the coordinator. + parser.add_argument("--inner", action="store_true", help=argparse.SUPPRESS) + parser.add_argument("--ref-start-ts", type=float, default=-1.0, help=argparse.SUPPRESS) + + args = parser.parse_args(argv) + if args.inner: + return _run_inner(args) + if not args.pcap: + parser.error("--pcap is required") + # Ignore SIGINT in the parent so the finally-block teardown always runs. + signal.signal(signal.SIGINT, signal.SIG_IGN) + return _run_outer(args) + + +if __name__ == "__main__": + raise SystemExit(main(sys.argv[1:])) From 05aa9d9707e29dcb68612029381d0a7e6dbaaf5e Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 21:12:37 -0700 Subject: [PATCH 068/185] =?UTF-8?q?refactor(pointlio):=20drop=20mount/init?= =?UTF-8?q?=5Fpose=20offset=20=E2=80=94=20publish=20in=20the=20sensor=20fr?= =?UTF-8?q?ame?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Remove the 'mount' Pose config and the init_pose offset it drove. The cloud already publishes in the sensor frame (mid360_link, via get_body_cloud); now odometry is the raw SLAM pose too, with no mount/world offset applied. Drops the init_pose CLI arg + g_init_*/has_init_pose/quat_mul/quat_rotate (now unused) from main.cpp. (global_map was already removed earlier.) --- .../sensors/lidar/pointlio/cpp/main.cpp | 98 ++----------------- .../hardware/sensors/lidar/pointlio/module.py | 23 +---- 2 files changed, 11 insertions(+), 110 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 8d645031cf..2b7676b467 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -71,42 +71,6 @@ static std::string g_frame_id; // required via --frame_id static std::string g_child_frame_id; // required via --child_frame_id static float g_frequency = 10.0f; -// Initial pose offset (applied to all SLAM outputs) -static double g_init_x = 0.0; -static double g_init_y = 0.0; -static double g_init_z = 0.0; -static double g_init_qx = 0.0; -static double g_init_qy = 0.0; -static double g_init_qz = 0.0; -static double g_init_qw = 1.0; - -// Hamilton product: q_out = q1 * q2 -static void quat_mul(double ax, double ay, double az, double aw, - double bx, double by, double bz, double bw, - double& ox, double& oy, double& oz, double& ow) { - ow = aw*bw - ax*bx - ay*by - az*bz; - ox = aw*bx + ax*bw + ay*bz - az*by; - oy = aw*by - ax*bz + ay*bw + az*bx; - oz = aw*bz + ax*by - ay*bx + az*bw; -} - -// Rotate vector by quaternion: v_out = q * v * q_inv -static void quat_rotate(double qx, double qy, double qz, double qw, - double vx, double vy, double vz, - double& ox, double& oy, double& oz) { - double tx = 2.0 * (qy*vz - qz*vy); - double ty = 2.0 * (qz*vx - qx*vz); - double tz = 2.0 * (qx*vy - qy*vx); - ox = vx + qw*tx + (qy*tz - qz*ty); - oy = vy + qw*ty + (qz*tx - qx*tz); - oz = vz + qw*tz + (qx*ty - qy*tx); -} - -static bool has_init_pose() { - return g_init_x != 0.0 || g_init_y != 0.0 || g_init_z != 0.0 || - g_init_qx != 0.0 || g_init_qy != 0.0 || g_init_qz != 0.0 || g_init_qw != 1.0; -} - // Frame accumulator (Livox SDK raw → CustomMsg) static std::mutex g_pc_mutex; static std::vector g_accumulated_points; @@ -192,38 +156,14 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times msg.header = make_header(g_frame_id, timestamp); msg.child_frame_id = g_child_frame_id; - // p_out = R_init * p_slam + t_init - if (has_init_pose()) { - double rx, ry, rz; - quat_rotate(g_init_qx, g_init_qy, g_init_qz, g_init_qw, - odom.pose.pose.position.x, - odom.pose.pose.position.y, - odom.pose.pose.position.z, - rx, ry, rz); - msg.pose.pose.position.x = rx + g_init_x; - msg.pose.pose.position.y = ry + g_init_y; - msg.pose.pose.position.z = rz + g_init_z; - - double ox, oy, oz, ow; - quat_mul(g_init_qx, g_init_qy, g_init_qz, g_init_qw, - odom.pose.pose.orientation.x, - odom.pose.pose.orientation.y, - odom.pose.pose.orientation.z, - odom.pose.pose.orientation.w, - ox, oy, oz, ow); - msg.pose.pose.orientation.x = ox; - msg.pose.pose.orientation.y = oy; - msg.pose.pose.orientation.z = oz; - msg.pose.pose.orientation.w = ow; - } else { - msg.pose.pose.position.x = odom.pose.pose.position.x; - msg.pose.pose.position.y = odom.pose.pose.position.y; - msg.pose.pose.position.z = odom.pose.pose.position.z; - msg.pose.pose.orientation.x = odom.pose.pose.orientation.x; - msg.pose.pose.orientation.y = odom.pose.pose.orientation.y; - msg.pose.pose.orientation.z = odom.pose.pose.orientation.z; - msg.pose.pose.orientation.w = odom.pose.pose.orientation.w; - } + // Pose in the SLAM/sensor frame (no mount offset applied). + msg.pose.pose.position.x = odom.pose.pose.position.x; + msg.pose.pose.position.y = odom.pose.pose.position.y; + msg.pose.pose.position.z = odom.pose.pose.position.z; + msg.pose.pose.orientation.x = odom.pose.pose.orientation.x; + msg.pose.pose.orientation.y = odom.pose.pose.orientation.y; + msg.pose.pose.orientation.z = odom.pose.pose.orientation.z; + msg.pose.pose.orientation.w = odom.pose.pose.orientation.w; for (int i = 0; i < 36; ++i) { msg.pose.covariance[i] = odom.pose.covariance[i]; @@ -455,30 +395,8 @@ int main(int argc, char** argv) { ports.host_imu_data = mod.arg_int("host_imu_data_port", port_defaults.host_imu_data); ports.host_log_data = mod.arg_int("host_log_data_port", port_defaults.host_log_data); - // Initial pose offset [x, y, z, qx, qy, qz, qw] - { - std::string init_str = mod.arg("init_pose", ""); - if (!init_str.empty()) { - double vals[7] = {0, 0, 0, 0, 0, 0, 1}; - int n = 0; - size_t pos = 0; - while (pos < init_str.size() && n < 7) { - size_t comma = init_str.find(',', pos); - if (comma == std::string::npos) comma = init_str.size(); - vals[n++] = std::stod(init_str.substr(pos, comma - pos)); - pos = comma + 1; - } - g_init_x = vals[0]; g_init_y = vals[1]; g_init_z = vals[2]; - g_init_qx = vals[3]; g_init_qy = vals[4]; g_init_qz = vals[5]; g_init_qw = vals[6]; - } - } - if (debug) { printf("[fastlio2] Starting FAST-LIO2 + Livox Mid-360 native module\n"); - if (has_init_pose()) { - printf("[fastlio2] init_pose: xyz=(%.3f, %.3f, %.3f) quat=(%.4f, %.4f, %.4f, %.4f)\n", - g_init_x, g_init_y, g_init_z, g_init_qx, g_init_qy, g_init_qz, g_init_qw); - } printf("[fastlio2] lidar topic: %s\n", g_lidar_topic.empty() ? "(disabled)" : g_lidar_topic.c_str()); printf("[fastlio2] odometry topic: %s\n", diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index 65be5dee6f..58c105b39b 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -15,7 +15,7 @@ """Python NativeModule wrapper for the Point-LIO + Livox Mid-360 binary. Binds Livox SDK2 directly into Point-LIO for real-time LiDAR SLAM. -Outputs registered (world-frame) point clouds and odometry with covariance. +Outputs sensor-frame (mid360_link) point clouds and odometry with covariance. Usage:: @@ -55,7 +55,6 @@ SDK_POINT_DATA_PORT, SDK_PUSH_MSG_PORT, ) -from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -79,10 +78,6 @@ class PointLioConfig(NativeModuleConfig): lidar_ip: str = "192.168.1.155" frequency: float = 10.0 - # Sensor mount pose — position + orientation of the sensor relative to ground. - # Converted to init_pose CLI arg [x, y, z, qx, qy, qz, qw] in model_post_init. - mount: Pose = Pose() - # frame_id is the header frame for BOTH the point cloud and the odometry # message (the Mid-360 sensor frame). The TF published by the module is a # separate odom_parent_frame_id -> odom_frame_id transform. @@ -128,27 +123,15 @@ class PointLioConfig(NativeModuleConfig): # Resolved in __post_init__, passed as --config_path to the binary config_path: str | None = None - # init_pose is computed from mount; config is resolved to config_path - init_pose: list[float] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] - cli_exclude: frozenset[str] = frozenset({"config", "mount", "odom_parent_frame_id"}) + cli_exclude: frozenset[str] = frozenset({"config", "odom_parent_frame_id"}) def model_post_init(self, __context: object) -> None: - """Resolve config_path and compute init_pose from mount.""" + """Resolve the FAST-LIO YAML config to an absolute config_path.""" super().model_post_init(__context) cfg = self.config if not cfg.is_absolute(): cfg = _CONFIG_DIR / cfg self.config_path = str(cfg.resolve()) - m = self.mount - self.init_pose = [ - m.x, - m.y, - m.z, - m.orientation.x, - m.orientation.y, - m.orientation.z, - m.orientation.w, - ] class PointLio(NativeModule, perception.Lidar, perception.Odometry): From e741ad809c6362274fa18c022c9c0185a96cf41c Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 21:17:02 -0700 Subject: [PATCH 069/185] virtual_mid360: over-the-wire pcap replay for fastlio (live SDK path) Port the Rust virtual_mid360 native module (fake Mid-360 on a virtual NIC that replays a pcap with a synthesized SDK2 handshake) and make pcap_to_db a live-only recorder driven by it: FastLio2 runs in live SDK mode, fed by virtual_mid360 over a veth via tools/replay_via_virtual_mid360.sh, and the two fastlio streams are recorded + time-aligned into a memory2 db. Replaces the in-process pcap reader (removed next). The harness uses a configurable $SUDO for the netns setup. --- .../lidar/fastlio2/tools/pcap_to_db.py | 126 +-- .../tools/replay_via_virtual_mid360.sh | 90 ++ .../lidar/livox/virtual_mid360/Cargo.lock | 900 ++++++++++++++++++ .../lidar/livox/virtual_mid360/Cargo.toml | 18 + .../lidar/livox/virtual_mid360/blueprints.py | 40 + .../lidar/livox/virtual_mid360/flake.lock | 78 ++ .../lidar/livox/virtual_mid360/flake.nix | 50 + .../lidar/livox/virtual_mid360/module.py | 68 ++ .../lidar/livox/virtual_mid360/src/main.rs | 511 ++++++++++ dimos/robot/all_blueprints.py | 2 + 10 files changed, 1795 insertions(+), 88 deletions(-) create mode 100755 dimos/hardware/sensors/lidar/fastlio2/tools/replay_via_virtual_mid360.sh create mode 100644 dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.lock create mode 100644 dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml create mode 100644 dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py create mode 100644 dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.lock create mode 100644 dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.nix create mode 100644 dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py create mode 100644 dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index 16d75b952b..4fd5bca4ac 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -12,57 +12,35 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Run FAST-LIO over a .pcap and write its outputs into a .db. +"""Record live FAST-LIO output into a .db while a virtual sensor replays a pcap. -Given a Livox Mid-360 pcap capture, this streams the pcap through the FastLio2 -native module and writes two streams into a memory2 SQLite database: +FastLio2 runs in **live SDK mode** and this tool records its two output streams +into a memory2 SQLite database for ``--duration`` seconds: * ``fastlio_odometry`` -- the IESKF pose at the native odom rate (~30 Hz). * ``fastlio_lidar`` -- the registered (deskewed, odom-frame) point cloud at the native pointcloud rate (~10 Hz). -The ``--db`` is optional. With no existing db the tool builds one **from -scratch** (omit ``--db`` and it defaults to ``.db`` next to the pcap). -With an existing db the two streams are appended and time-aligned onto the db's -clock, so FAST-LIO output can be compared against whatever it already holds. +There is no in-process pcap reader: the packets come from the live Livox SDK +path, fed by ``virtual_mid360`` — a fake Mid-360 on a virtual NIC that replays a +recorded pcap with a synthesized SDK2 handshake. FastLio2 connects to it exactly +as it would to real hardware and never knows the sensor is synthetic. The netns +setup + sensor are orchestrated by ``tools/replay_via_virtual_mid360.sh``, which +drives this tool as its consumer; that wrapper is the normal entry point. -This mirrors the Point-LIO ``pcap_to_db.py`` tool, with one deliberate -difference: FAST-LIO is *not* bit-deterministic (OpenMP reduction order), so the -replay runs ``deterministic_clock=False`` -- the feeder paces packets at -wall-clock realtime, exactly as the live SDK delivers them, and publish -timestamps come from the pcap's capture clock. A 20-minute recording therefore -takes ~20 minutes of wall time to replay. +The db is appended in place: the two fastlio streams are time-aligned onto the +db's existing clock (so they line up with whatever else it holds), and an +existing ``fastlio_odometry`` / ``fastlio_lidar`` pair aborts the run unless +``--force`` is given. ``--time-offset`` overrides the auto-derived clock shift. -If either stream already exists in the db the run aborts, unless ``--force`` is -given, in which case the existing ``fastlio_odometry`` and ``fastlio_lidar`` -streams are dropped before the new ones are written. +Usage (normally via the wrapper):: -Timing conversion ------------------ -With ``deterministic_clock=False`` FAST-LIO publishes with the pcap packet -clock, which for a real recording is the original capture's *unix wall time* -- -the same clock the db's other streams already use. So the common case needs no -shift. The offset is auto-derived from the two clocks: + bash tools/replay_via_virtual_mid360.sh [config.yaml] -* db + replay on the same clock family (both wall, or both sensor): offset 0. -* cross-clock (e.g. a deterministic sensor-clock replay into a wall-clock db): - start-align by shifting the replay's first ts onto the db's earliest ts. -* db has no existing timestamped rows: offset 0. +Direct (only useful inside the consumer netns, fed by an external sensor):: -Pass ``--time-offset`` to override the auto choice. - -Usage (from the dimos5 venv):: - - source .venv/bin/activate - - # Build a fresh db from scratch (no existing db needed); defaults to - # .db next to the pcap. python -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db \ - --pcap /path/to/capture.pcap - - # Or append into an existing recording db for comparison. - python -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db \ - --pcap /path/to/capture.pcap --db /path/to/memory.db + --db /path/to/memory.db --duration 200 """ from __future__ import annotations @@ -78,10 +56,8 @@ # Below this an absolute timestamp is sensor-boot seconds, not unix wall time. _SENSOR_CLOCK_MAX = 1e8 -# Poll the db on this cadence while the replay drains the pcap. +# Poll cadence while recording the live stream. _POLL_SEC = 1.0 -# Stop after the odom stream has been stagnant this long (pcap fully drained). -_STAGNANT_SEC = 6.0 def _db_ref_start_ts(db_path: Path) -> float: @@ -131,17 +107,16 @@ def _table_stats(db_path: Path, table: str) -> tuple[int, float, float]: def _run(args: argparse.Namespace) -> int: - pcap_path = Path(args.pcap).expanduser().resolve() - if not pcap_path.exists(): - print(f"[pcap_to_db] missing pcap: {pcap_path}", file=sys.stderr) + # FastLio2 runs in live SDK mode, fed by an external sensor — virtual_mid360 + # replaying a pcap over a veth (see tools/replay_via_virtual_mid360.sh). We + # record whatever the SDK receives into the db for --duration seconds. + if not args.db: + print("[pcap_to_db] --db is required", file=sys.stderr) return 2 - if args.max_sensor_sec < 0: - print("[pcap_to_db] --max-sensor-sec must be >= 0", file=sys.stderr) + if args.duration <= 0: + print("[pcap_to_db] --duration must be > 0", file=sys.stderr) return 2 - # --db is optional: with no existing db, build one from scratch. When - # omitted the output defaults to .db next to the pcap, so a fresh - # db can be generated with just --pcap. - db_path = Path(args.db).expanduser().resolve() if args.db else pcap_path.with_suffix(".db") + db_path = Path(args.db).expanduser().resolve() db_existed = db_path.exists() db_path.parent.mkdir(parents=True, exist_ok=True) @@ -178,7 +153,7 @@ def _run(args: argparse.Namespace) -> int: else: offset_desc = f"auto: db wall-clock (R0={ref_start_ts:.2f})" print( - f"[pcap_to_db] pcap={pcap_path.name} db={db_path.name} " + f"[pcap_to_db] src=virtual_mid360 (live SDK) db={db_path.name} " f"({'append' if db_existed else 'new'}) " f"odom_freq={args.odom_freq}Hz offset={offset_desc}", flush=True, @@ -187,8 +162,6 @@ def _run(args: argparse.Namespace) -> int: fastlio_kwargs: dict[str, object] = dict( frame_id="world", odom_freq=args.odom_freq, - replay_pcap=pcap_path, - deterministic_clock=False, debug=False, ) # Omit config to fall back to the module default (config/mid360.yaml). @@ -211,31 +184,10 @@ def _run(args: argparse.Namespace) -> int: coord = ModuleCoordinator.build(blueprint) t0 = time.time() - last_max = 0.0 - first_max: float | None = None - stagnant_since: float | None = None try: - while True: + while time.time() - t0 < args.duration: time.sleep(_POLL_SEC) - cnt, min_ts, max_ts = _table_stats(db_path, "fastlio_odometry") - if cnt == 0: - continue - if first_max is None: - first_max = min_ts - if args.max_sensor_sec > 0 and (max_ts - first_max) >= args.max_sensor_sec: - print( - f"[pcap_to_db] reached --max-sensor-sec={args.max_sensor_sec:.1f}s", - flush=True, - ) - break - if max_ts == last_max: - if stagnant_since is None: - stagnant_since = time.time() - elif time.time() - stagnant_since > _STAGNANT_SEC: - break - else: - last_max = max_ts - stagnant_since = None + print(f"[pcap_to_db] reached --duration={args.duration:.1f}s", flush=True) finally: coord.stop() @@ -253,13 +205,17 @@ def _run(args: argparse.Namespace) -> int: def main(argv: list[str]) -> int: parser = argparse.ArgumentParser(description=__doc__) - parser.add_argument("--pcap", required=True, help="Livox Mid-360 pcap capture") parser.add_argument( "--db", - default=None, - help="target memory2 SQLite db. If it exists, fastlio streams are appended/aligned " - "onto its clock; if it doesn't, a fresh db is built from scratch. " - "Omit to default to .db next to the pcap.", + required=True, + help="target memory2 SQLite db. fastlio streams are appended + time-aligned " + "onto its clock (or it's created fresh if absent).", + ) + parser.add_argument( + "--duration", + type=float, + required=True, + help="record for this many seconds of wall time, then stop", ) parser.add_argument( "--odom-freq", @@ -273,12 +229,6 @@ def main(argv: list[str]) -> int: default=None, help="FAST-LIO yaml (relative to config/ or absolute); omit for the module default", ) - parser.add_argument( - "--max-sensor-sec", - type=float, - default=0.0, - help="stop after this many seconds of sensor time (0 = whole pcap)", - ) parser.add_argument( "--time-offset", type=float, diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/replay_via_virtual_mid360.sh b/dimos/hardware/sensors/lidar/fastlio2/tools/replay_via_virtual_mid360.sh new file mode 100755 index 0000000000..056f96d6c8 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/replay_via_virtual_mid360.sh @@ -0,0 +1,90 @@ +#!/usr/bin/env bash +# Replay a Livox Mid-360 pcap through FAST-LIO over the wire, recording odometry +# + lidar into a memory2 db. +# +# virtual_mid360 stands up a fake Mid-360 on a virtual NIC and replays the pcap +# with a synthesized SDK2 handshake; FastLio2 connects to it as if to real +# hardware (live SDK mode) and never knows the sensor is synthetic. This is the +# only replay path — the fastlio binary has no in-process pcap reader. Use it to +# reproduce divergence / non-divergence exactly as the robot would see it. +# +# Two network namespaces joined by a veth: the lidar ns runs virtual_mid360, the +# drv ns runs `pcap_to_db` (FastLio2 live + FastLio2Recorder). Needs root for the +# netns/veth setup — set $SUDO to your privilege-escalation command (default +# `sudo`; it must run `ip`/`pkill` without a password prompt). +# +# The netns + veth NAMES are distinct from pointlio's harness (drv/lidar + +# veth-drv/veth-lidar) so the two can run concurrently. Override via env +# (DRV_NS/LIDAR_NS/VETH_DRV/VETH_LIDAR). IPs live inside each netns, so the +# .1.x addresses don't conflict with pointlio's even though they're the same. +# +# Usage: +# source /bin/activate # provide a python with dimos installed +# replay_via_virtual_mid360.sh [duration_sec] [fastlio_config.yaml] +# +set -u +PCAP="${1:?usage: replay_via_virtual_mid360.sh [duration] [config.yaml]}" +DB="${2:?missing }" +DUR="${3:-200}" +CONFIG="${4:-}" + +SUDO="${SUDO:-sudo}" +HOST_IP=192.168.1.5 +LIDAR_IP=192.168.1.155 +DRV_NS="${DRV_NS:-fl_drv}" +LIDAR_NS="${LIDAR_NS:-fl_lidar}" +VETH_DRV="${VETH_DRV:-veth-fl-drv}" +VETH_LIDAR="${VETH_LIDAR:-veth-fl-lidar}" +REPO="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../../../../.." && pwd)" +VM="$REPO/dimos/hardware/sensors/lidar/livox/virtual_mid360/result/bin/virtual_mid360" +PYTHON="${PYTHON:-$(command -v python)}" +VM_LOG="${VM_LOG:-/tmp/vmid360_vm.log}" +FL_LOG="${FL_LOG:-/tmp/vmid360_fastlio.log}" + +[ -x "$VM" ] || { echo "missing virtual_mid360 binary at $VM — build it: (cd $(dirname "$VM")/.. && nix build .#default)"; exit 2; } +[ -f "$PCAP" ] || { echo "missing pcap: $PCAP"; exit 2; } +[ -n "$PYTHON" ] || { echo "no python on PATH — activate the dimos venv first"; exit 2; } + +cleanup() { + # Match the binary path, NOT a bare "virtual_mid360" — this script's own name + # contains that string, so a loose pattern would SIGKILL the wrapper itself. + $SUDO pkill -9 -f "result/bin/virtual_mid360" 2>/dev/null + $SUDO ip netns del "$DRV_NS" 2>/dev/null + $SUDO ip netns del "$LIDAR_NS" 2>/dev/null + $SUDO ip link del "$VETH_DRV" 2>/dev/null +} +cleanup +$SUDO ip netns add "$DRV_NS"; $SUDO ip netns add "$LIDAR_NS" +$SUDO ip link add "$VETH_DRV" type veth peer name "$VETH_LIDAR" +$SUDO ip link set "$VETH_DRV" netns "$DRV_NS"; $SUDO ip link set "$VETH_LIDAR" netns "$LIDAR_NS" +$SUDO ip netns exec "$DRV_NS" ip addr add "$HOST_IP/24" dev "$VETH_DRV" +$SUDO ip netns exec "$LIDAR_NS" ip addr add "$LIDAR_IP/24" dev "$VETH_LIDAR" +for NS in "$DRV_NS" "$LIDAR_NS"; do + $SUDO ip netns exec "$NS" ip link set lo up + $SUDO ip netns exec "$NS" ip link set lo multicast on + $SUDO ip netns exec "$NS" ip route add 224.0.0.0/4 dev lo +done +$SUDO ip netns exec "$DRV_NS" ip link set "$VETH_DRV" up; $SUDO ip netns exec "$LIDAR_NS" ip link set "$VETH_LIDAR" up +$SUDO ip netns exec "$DRV_NS" ip link set "$VETH_DRV" multicast on; $SUDO ip netns exec "$LIDAR_NS" ip link set "$VETH_LIDAR" multicast on +$SUDO ip netns exec "$LIDAR_NS" ip route add 255.255.255.255/32 dev "$VETH_LIDAR" +# Mid-360 multicasts point/IMU to 224.1.1.5 — egress the virtual NIC. +$SUDO ip netns exec "$LIDAR_NS" ip route add 224.1.1.5/32 dev "$VETH_LIDAR" + +# Consumer: FastLio2 (live SDK) + FastLio2Recorder, recording into the db. +CFG_ARG=(); [ -n "$CONFIG" ] && CFG_ARG=(--config "$CONFIG") +$SUDO ip netns exec "$DRV_NS" env "PYTHONPATH=$REPO" "$PYTHON" \ + -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db \ + --db "$DB" --duration "$DUR" --force "${CFG_ARG[@]}" > "$FL_LOG" 2>&1 & +CONSUMER=$! +sleep 5 # let the coordinator boot + open the SDK sockets + +# Fake lidar: replay the pcap over the wire (delay lets the consumer settle). +echo "{\"topics\":{},\"config\":{\"pcap\":\"$PCAP\",\"rate\":1.0,\"delay\":2.0,\"lidar_netns\":\"$LIDAR_NS\"}}" \ + | $SUDO ip netns exec "$LIDAR_NS" "$VM" > "$VM_LOG" 2>&1 & + +wait "$CONSUMER" +RC=$? +echo "=== handshake marker (vm log) ==="; grep -i "arming data stream\|0x0100" "$VM_LOG" | tail -1 +cleanup +echo "DONE rc=$RC db=$DB" +exit "$RC" diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.lock b/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.lock new file mode 100644 index 0000000000..f9b68b4d57 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.lock @@ -0,0 +1,900 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 4 + +[[package]] +name = "aho-corasick" +version = "1.1.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ddd31a130427c27518df266943a5308ed92d4b226cc639f5a8f1002816174301" +dependencies = [ + "memchr", +] + +[[package]] +name = "byteorder" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" + +[[package]] +name = "bytes" +version = "1.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1e748733b7cbc798e1434b6ac524f0c1ff2ab456fe201501e6497c8417a4fc33" + +[[package]] +name = "cfg-if" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9330f8b2ff13f34540b44e946ef35111825727b38d33286ef986142615121801" + +[[package]] +name = "darling" +version = "0.20.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fc7f46116c46ff9ab3eb1597a45688b6715c6e628b5c133e288e709a29bcb4ee" +dependencies = [ + "darling_core", + "darling_macro", +] + +[[package]] +name = "darling_core" +version = "0.20.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0d00b9596d185e565c2207a0b01f8bd1a135483d02d9b7b0a54b11da8d53412e" +dependencies = [ + "fnv", + "ident_case", + "proc-macro2", + "quote", + "strsim", + "syn", +] + +[[package]] +name = "darling_macro" +version = "0.20.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fc34b93ccb385b40dc71c6fceac4b2ad23662c7eeb248cf10d529b7e055b6ead" +dependencies = [ + "darling_core", + "quote", + "syn", +] + +[[package]] +name = "dimos-lcm" +version = "0.1.0" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#e7c9428b7201cdfeadecd181c77c9e2d60a14503" +dependencies = [ + "byteorder", + "socket2 0.5.10", + "tokio", +] + +[[package]] +name = "dimos-module" +version = "0.1.0" +dependencies = [ + "dimos-lcm", + "dimos-module-macros", + "serde", + "serde_json", + "tokio", + "tracing", + "tracing-subscriber", + "validator", +] + +[[package]] +name = "dimos-module-macros" +version = "0.1.0" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "displaydoc" +version = "0.2.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1ac70aa55017e108007fbaf5aa0f54b021c98f92ff8af59d42eda9da96e3dd4f" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "errno" +version = "0.3.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "39cab71617ae0d63f51a36d69f866391735b51691dbda63cf6f96d042b63efeb" +dependencies = [ + "libc", + "windows-sys 0.61.2", +] + +[[package]] +name = "fnv" +version = "1.0.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" + +[[package]] +name = "form_urlencoded" +version = "1.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cb4cb245038516f5f85277875cdaa4f7d2c9a0fa0468de06ed190163b1581fcf" +dependencies = [ + "percent-encoding", +] + +[[package]] +name = "icu_collections" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2984d1cd16c883d7935b9e07e44071dca8d917fd52ecc02c04d5fa0b5a3f191c" +dependencies = [ + "displaydoc", + "potential_utf", + "utf8_iter", + "yoke", + "zerofrom", + "zerovec", +] + +[[package]] +name = "icu_locale_core" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "92219b62b3e2b4d88ac5119f8904c10f8f61bf7e95b640d25ba3075e6cac2c29" +dependencies = [ + "displaydoc", + "litemap", + "tinystr", + "writeable", + "zerovec", +] + +[[package]] +name = "icu_normalizer" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c56e5ee99d6e3d33bd91c5d85458b6005a22140021cc324cea84dd0e72cff3b4" +dependencies = [ + "icu_collections", + "icu_normalizer_data", + "icu_properties", + "icu_provider", + "smallvec", + "zerovec", +] + +[[package]] +name = "icu_normalizer_data" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "da3be0ae77ea334f4da67c12f149704f19f81d1adf7c51cf482943e84a2bad38" + +[[package]] +name = "icu_properties" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bee3b67d0ea5c2cca5003417989af8996f8604e34fb9ddf96208a033901e70de" +dependencies = [ + "icu_collections", + "icu_locale_core", + "icu_properties_data", + "icu_provider", + "zerotrie", + "zerovec", +] + +[[package]] +name = "icu_properties_data" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e2bbb201e0c04f7b4b3e14382af113e17ba4f63e2c9d2ee626b720cbce54a14" + +[[package]] +name = "icu_provider" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "139c4cf31c8b5f33d7e199446eff9c1e02decfc2f0eec2c8d71f65befa45b421" +dependencies = [ + "displaydoc", + "icu_locale_core", + "writeable", + "yoke", + "zerofrom", + "zerotrie", + "zerovec", +] + +[[package]] +name = "ident_case" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39" + +[[package]] +name = "idna" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3b0875f23caa03898994f6ddc501886a45c7d3d62d04d2d90788d47be1b1e4de" +dependencies = [ + "idna_adapter", + "smallvec", + "utf8_iter", +] + +[[package]] +name = "idna_adapter" +version = "1.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cb68373c0d6620ef8105e855e7745e18b0d00d3bdb07fb532e434244cdb9a714" +dependencies = [ + "icu_normalizer", + "icu_properties", +] + +[[package]] +name = "itoa" +version = "1.0.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8f42a60cbdf9a97f5d2305f08a87dc4e09308d1276d28c869c684d7777685682" + +[[package]] +name = "lazy_static" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" + +[[package]] +name = "libc" +version = "0.2.186" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "68ab91017fe16c622486840e4c83c9a37afeff978bd239b5293d61ece587de66" + +[[package]] +name = "litemap" +version = "0.8.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "92daf443525c4cce67b150400bc2316076100ce0b3686209eb8cf3c31612e6f0" + +[[package]] +name = "log" +version = "0.4.32" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "953f07c43838f8e6f9758cab68bf5bed85465e7587ebe0b823f1bcd81978ad3a" + +[[package]] +name = "matchers" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d1525a2a28c7f4fa0fc98bb91ae755d1e2d1505079e05539e35bc876b5d65ae9" +dependencies = [ + "regex-automata", +] + +[[package]] +name = "memchr" +version = "2.8.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "88904434abc2901f197fe8cc55f0445e7ded921dba5911dad2e2b39b48e663c4" + +[[package]] +name = "mio" +version = "1.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "02bd0af71c67b473010cbbc60715ee815645a4dc942899111f494b4b737d6fda" +dependencies = [ + "libc", + "wasi", + "windows-sys 0.61.2", +] + +[[package]] +name = "nu-ansi-term" +version = "0.50.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7957b9740744892f114936ab4a57b3f487491bbeafaf8083688b16841a4240e5" +dependencies = [ + "windows-sys 0.61.2", +] + +[[package]] +name = "once_cell" +version = "1.21.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9f7c3e4beb33f85d45ae3e3a1792185706c8e16d043238c593331cc7cd313b50" + +[[package]] +name = "percent-encoding" +version = "2.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9b4f627cb1b25917193a259e49bdad08f671f8d9708acfd5fe0a8c1455d87220" + +[[package]] +name = "pin-project-lite" +version = "0.2.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a89322df9ebe1c1578d689c92318e070967d1042b512afbe49518723f4e6d5cd" + +[[package]] +name = "potential_utf" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0103b1cef7ec0cf76490e969665504990193874ea05c85ff9bab8b911d0a0564" +dependencies = [ + "zerovec", +] + +[[package]] +name = "proc-macro-error-attr2" +version = "2.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "96de42df36bb9bba5542fe9f1a054b8cc87e172759a1868aa05c1f3acc89dfc5" +dependencies = [ + "proc-macro2", + "quote", +] + +[[package]] +name = "proc-macro-error2" +version = "2.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11ec05c52be0a07b08061f7dd003e7d7092e0472bc731b4af7bb1ef876109802" +dependencies = [ + "proc-macro-error-attr2", + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "proc-macro2" +version = "1.0.106" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8fd00f0bb2e90d81d1044c2b32617f68fcb9fa3bb7640c23e9c748e53fb30934" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "quote" +version = "1.0.45" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "41f2619966050689382d2b44f664f4bc593e129785a36d6ee376ddf37259b924" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "regex" +version = "1.12.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f1292b7759ae1cb9ec195452d1390a074f0cd8541ab7a5a8c31cd6db45d4a6ba" +dependencies = [ + "aho-corasick", + "memchr", + "regex-automata", + "regex-syntax", +] + +[[package]] +name = "regex-automata" +version = "0.4.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6e1dd4122fc1595e8162618945476892eefca7b88c52820e74af6262213cae8f" +dependencies = [ + "aho-corasick", + "memchr", + "regex-syntax", +] + +[[package]] +name = "regex-syntax" +version = "0.8.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d6f6ff9a378485b298a5286656da665ba74413d36db0979633275d2e708145d4" + +[[package]] +name = "serde" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a8e94ea7f378bd32cbbd37198a4a91436180c5bb472411e48b5ec2e2124ae9e" +dependencies = [ + "serde_core", + "serde_derive", +] + +[[package]] +name = "serde_core" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "41d385c7d4ca58e59fc732af25c3983b67ac852c1a25000afe1175de458b67ad" +dependencies = [ + "serde_derive", +] + +[[package]] +name = "serde_derive" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d540f220d3187173da220f885ab66608367b6574e925011a9353e4badda91d79" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "serde_json" +version = "1.0.150" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e8014e44b4736ed0538adeecded0fce2a272f22dc9578a7eb6b2d9993c74cfb9" +dependencies = [ + "itoa", + "memchr", + "serde", + "serde_core", + "zmij", +] + +[[package]] +name = "sharded-slab" +version = "0.1.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f40ca3c46823713e0d4209592e8d6e826aa57e928f09752619fc696c499637f6" +dependencies = [ + "lazy_static", +] + +[[package]] +name = "signal-hook-registry" +version = "1.4.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c4db69cba1110affc0e9f7bcd48bbf87b3f4fc7c61fc9155afd4c469eb3d6c1b" +dependencies = [ + "errno", + "libc", +] + +[[package]] +name = "smallvec" +version = "1.15.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8ed6a63f02c8539c91a8685a86f4099661ba3da017932f6ebbea6de3f0fa7c90" + +[[package]] +name = "socket2" +version = "0.5.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e22376abed350d73dd1cd119b57ffccad95b4e585a7cda43e286245ce23c0678" +dependencies = [ + "libc", + "windows-sys 0.52.0", +] + +[[package]] +name = "socket2" +version = "0.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "52d1cfed4120b4d927bf7c0f86d2087a4a7d6027c906d9f9d525a80573b9be51" +dependencies = [ + "libc", + "windows-sys 0.61.2", +] + +[[package]] +name = "stable_deref_trait" +version = "1.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6ce2be8dc25455e1f91df71bfa12ad37d7af1092ae736f3a6cd0e37bc7810596" + +[[package]] +name = "strsim" +version = "0.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" + +[[package]] +name = "syn" +version = "2.0.117" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e665b8803e7b1d2a727f4023456bbbbe74da67099c585258af0ad9c5013b9b99" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "synstructure" +version = "0.13.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "728a70f3dbaf5bab7f0c4b1ac8d7ae5ea60a4b5549c8a5914361c99147a709d2" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "thread_local" +version = "1.1.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f60246a4944f24f6e018aa17cdeffb7818b76356965d03b07d6a9886e8962185" +dependencies = [ + "cfg-if", +] + +[[package]] +name = "tinystr" +version = "0.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c8323304221c2a851516f22236c5722a72eaa19749016521d6dff0824447d96d" +dependencies = [ + "displaydoc", + "zerovec", +] + +[[package]] +name = "tokio" +version = "1.52.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8fc7f01b389ac15039e4dc9531aa973a135d7a4135281b12d7c1bc79fd57fffe" +dependencies = [ + "bytes", + "libc", + "mio", + "pin-project-lite", + "signal-hook-registry", + "socket2 0.6.4", + "tokio-macros", + "windows-sys 0.61.2", +] + +[[package]] +name = "tokio-macros" +version = "2.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "385a6cb71ab9ab790c5fe8d67f1645e6c450a7ce006a33de03daa956cf70a496" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "tracing" +version = "0.1.44" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "63e71662fa4b2a2c3a26f570f037eb95bb1f85397f3cd8076caed2f026a6d100" +dependencies = [ + "pin-project-lite", + "tracing-attributes", + "tracing-core", +] + +[[package]] +name = "tracing-attributes" +version = "0.1.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7490cfa5ec963746568740651ac6781f701c9c5ea257c58e057f3ba8cf69e8da" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "tracing-core" +version = "0.1.36" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "db97caf9d906fbde555dd62fa95ddba9eecfd14cb388e4f491a66d74cd5fb79a" +dependencies = [ + "once_cell", + "valuable", +] + +[[package]] +name = "tracing-log" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ee855f1f400bd0e5c02d150ae5de3840039a3f54b025156404e34c23c03f47c3" +dependencies = [ + "log", + "once_cell", + "tracing-core", +] + +[[package]] +name = "tracing-serde" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "704b1aeb7be0d0a84fc9828cae51dab5970fee5088f83d1dd7ee6f6246fc6ff1" +dependencies = [ + "serde", + "tracing-core", +] + +[[package]] +name = "tracing-subscriber" +version = "0.3.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cb7f578e5945fb242538965c2d0b04418d38ec25c79d160cd279bf0731c8d319" +dependencies = [ + "matchers", + "nu-ansi-term", + "once_cell", + "regex-automata", + "serde", + "serde_json", + "sharded-slab", + "smallvec", + "thread_local", + "tracing", + "tracing-core", + "tracing-log", + "tracing-serde", +] + +[[package]] +name = "unicode-ident" +version = "1.0.24" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e6e4313cd5fcd3dad5cafa179702e2b244f760991f45397d14d4ebf38247da75" + +[[package]] +name = "url" +version = "2.5.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ff67a8a4397373c3ef660812acab3268222035010ab8680ec4215f38ba3d0eed" +dependencies = [ + "form_urlencoded", + "idna", + "percent-encoding", + "serde", +] + +[[package]] +name = "utf8_iter" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b6c140620e7ffbb22c2dee59cafe6084a59b5ffc27a8859a5f0d494b5d52b6be" + +[[package]] +name = "validator" +version = "0.20.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "43fb22e1a008ece370ce08a3e9e4447a910e92621bb49b85d6e48a45397e7cfa" +dependencies = [ + "idna", + "once_cell", + "regex", + "serde", + "serde_derive", + "serde_json", + "url", + "validator_derive", +] + +[[package]] +name = "validator_derive" +version = "0.20.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b7df16e474ef958526d1205f6dda359fdfab79d9aa6d54bafcb92dcd07673dca" +dependencies = [ + "darling", + "once_cell", + "proc-macro-error2", + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "valuable" +version = "0.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ba73ea9cf16a25df0c8caa16c51acb937d5712a8429db78a3ee29d5dcacd3a65" + +[[package]] +name = "virtual-mid360" +version = "0.1.0" +dependencies = [ + "dimos-module", + "serde", + "tokio", + "tracing", + "validator", +] + +[[package]] +name = "wasi" +version = "0.11.1+wasi-snapshot-preview1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ccf3ec651a847eb01de73ccad15eb7d99f80485de043efb2f370cd654f4ea44b" + +[[package]] +name = "windows-link" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f0805222e57f7521d6a62e36fa9163bc891acd422f971defe97d64e70d0a4fe5" + +[[package]] +name = "windows-sys" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "282be5f36a8ce781fad8c8ae18fa3f9beff57ec1b52cb3de0789201425d9a33d" +dependencies = [ + "windows-targets", +] + +[[package]] +name = "windows-sys" +version = "0.61.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ae137229bcbd6cdf0f7b80a31df61766145077ddf49416a728b02cb3921ff3fc" +dependencies = [ + "windows-link", +] + +[[package]] +name = "windows-targets" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973" +dependencies = [ + "windows_aarch64_gnullvm", + "windows_aarch64_msvc", + "windows_i686_gnu", + "windows_i686_gnullvm", + "windows_i686_msvc", + "windows_x86_64_gnu", + "windows_x86_64_gnullvm", + "windows_x86_64_msvc", +] + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469" + +[[package]] +name = "windows_i686_gnu" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b" + +[[package]] +name = "windows_i686_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66" + +[[package]] +name = "windows_i686_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" + +[[package]] +name = "writeable" +version = "0.6.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1ffae5123b2d3fc086436f8834ae3ab053a283cfac8fe0a0b8eaae044768a4c4" + +[[package]] +name = "yoke" +version = "0.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "709fe23a0424b6a435d82152b1bd3fdfb0833487d5fa90d05d42762a9891fef5" +dependencies = [ + "stable_deref_trait", + "yoke-derive", + "zerofrom", +] + +[[package]] +name = "yoke-derive" +version = "0.8.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "de844c262c8848816172cef550288e7dc6c7b7814b4ee56b3e1553f275f1858e" +dependencies = [ + "proc-macro2", + "quote", + "syn", + "synstructure", +] + +[[package]] +name = "zerofrom" +version = "0.1.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ec05a11813ea801ff6d75110ad09cd0824ddba17dfe17128ea0d5f68e6c5272" +dependencies = [ + "zerofrom-derive", +] + +[[package]] +name = "zerofrom-derive" +version = "0.1.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11532158c46691caf0f2593ea8358fed6bbf68a0315e80aae9bd41fbade684a1" +dependencies = [ + "proc-macro2", + "quote", + "syn", + "synstructure", +] + +[[package]] +name = "zerotrie" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0f9152d31db0792fa83f70fb2f83148effb5c1f5b8c7686c3459e361d9bc20bf" +dependencies = [ + "displaydoc", + "yoke", + "zerofrom", +] + +[[package]] +name = "zerovec" +version = "0.11.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "90f911cbc359ab6af17377d242225f4d75119aec87ea711a880987b18cd7b239" +dependencies = [ + "yoke", + "zerofrom", + "zerovec-derive", +] + +[[package]] +name = "zerovec-derive" +version = "0.11.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "625dc425cab0dca6dc3c3319506e6593dcb08a9f387ea3b284dbd52a92c40555" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "zmij" +version = "1.0.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b8848ee67ecc8aedbaf3e4122217aff892639231befc6a1b58d29fff4c2cabaa" diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml b/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml new file mode 100644 index 0000000000..c01c38cdae --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml @@ -0,0 +1,18 @@ +[package] +name = "virtual-mid360" +version = "0.1.0" +edition = "2021" + +# Fake Livox Mid-360: replays a recorded pcap over a virtual NIC and synthesizes +# the Livox SDK2 control handshake so an unmodified live-mode pointlio ingests it +# as if from a real sensor. Inverse of pointlio's in-process --replay_pcap. +[[bin]] +name = "virtual_mid360" +path = "src/main.rs" + +[dependencies] +dimos-module = { path = "../../../../../../native/rust/dimos-module" } +tokio = { version = "1", features = ["rt-multi-thread", "macros", "time", "net", "sync"] } +serde = { version = "1", features = ["derive"] } +tracing = "0.1" +validator = { version = "0.20", features = ["derive"] } diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py new file mode 100644 index 0000000000..51ba8b5139 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py @@ -0,0 +1,40 @@ +# 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. + +"""Blueprint: FastLio2 fed by a VirtualMid360 replaying a recorded pcap. + +VirtualMid360 stands up a fake Mid-360 on a virtual NIC and replays the pcap +over the Livox wire protocol; FastLio2 connects to it exactly as it would to +real hardware (no replay_pcap — it runs in live SDK mode and never knows the +sensor is synthetic). Use this to re-run a recorded session through the live +SLAM path, e.g. to reproduce (or rule out) the velocity-spike divergence. + +The two talk over UDP on lidar_ip/host_ip, so they need a network where those +IPs are reachable: the e2e harness runs VirtualMid360 in a `lidar` netns and +FastLio2 in a `drv` netns joined by a veth carrying lidar_ip. See +tools/replay_via_virtual_mid360.sh for the full netns setup + a db recording. +""" + +from dimos.core.coordination.blueprints import autoconnect +from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 +from dimos.hardware.sensors.lidar.livox.virtual_mid360.module import VirtualMid360 +from dimos.visualization.vis_module import vis_module + +# Set pcap to a recorded Mid-360 capture before running, e.g.: +# dimos run virtual-mid360-fastlio --VirtualMid360.pcap /path/to/capture.pcap +demo_virtual_mid360_fastlio = autoconnect( + VirtualMid360.blueprint(pcap=""), + FastLio2.blueprint(), + vis_module("rerun"), +).global_config(n_workers=3, robot_model="virtual_mid360_fastlio") diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.lock b/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.lock new file mode 100644 index 0000000000..a548660557 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.lock @@ -0,0 +1,78 @@ +{ + "nodes": { + "dimos-repo": { + "flake": false, + "locked": { + "lastModified": 1779865691, + "narHash": "sha256-2CVWcov7DiC1qX/B/zFKDJiSYsnbrZ3FNT/viprFWTQ=", + "ref": "refs/heads/jeff/feat/g1_raycast", + "rev": "51666bcd298c1d08bdee179f176f45c0a7dd417d", + "revCount": 744, + "type": "git", + "url": "file:../../../.." + }, + "original": { + "type": "git", + "url": "file:../../../.." + } + }, + "flake-utils": { + "inputs": { + "systems": "systems" + }, + "locked": { + "lastModified": 1731533236, + "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=", + "owner": "numtide", + "repo": "flake-utils", + "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b", + "type": "github" + }, + "original": { + "owner": "numtide", + "repo": "flake-utils", + "type": "github" + } + }, + "nixpkgs": { + "locked": { + "lastModified": 1779560665, + "narHash": "sha256-tpyBcxPpcQb8ukyNF7DoCwfSY3VPsxHoYwj00Cayv5o=", + "owner": "NixOS", + "repo": "nixpkgs", + "rev": "64c08a7ca051951c8eae34e3e3cb1e202fe36786", + "type": "github" + }, + "original": { + "owner": "NixOS", + "ref": "nixos-unstable", + "repo": "nixpkgs", + "type": "github" + } + }, + "root": { + "inputs": { + "dimos-repo": "dimos-repo", + "flake-utils": "flake-utils", + "nixpkgs": "nixpkgs" + } + }, + "systems": { + "locked": { + "lastModified": 1681028828, + "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", + "owner": "nix-systems", + "repo": "default", + "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", + "type": "github" + }, + "original": { + "owner": "nix-systems", + "repo": "default", + "type": "github" + } + } + }, + "root": "root", + "version": 7 +} diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.nix b/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.nix new file mode 100644 index 0000000000..a74d6bb71b --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.nix @@ -0,0 +1,50 @@ +{ + description = "Fake Livox Mid-360 pcap replayer (virtual NIC) native module for DimOS"; + + inputs = { + nixpkgs.url = "github:NixOS/nixpkgs/nixos-unstable"; + flake-utils.url = "github:numtide/flake-utils"; + # Relative git+file: reaches the repo root for the local dimos-module path + # deps (same approach as dimos/mapping/ray_tracing/rust). + dimos-repo = { url = "git+file:../../../../../.."; flake = false; }; + }; + + outputs = { self, nixpkgs, flake-utils, dimos-repo }: + flake-utils.lib.eachDefaultSystem (system: + let + pkgs = import nixpkgs { inherit system; }; + sub = "dimos/hardware/sensors/lidar/livox/virtual_mid360"; + + src = pkgs.runCommand "virtual-mid360-src" {} '' + mkdir -p $out/${sub} + cp -r ${./src} $out/${sub}/src + cp ${./Cargo.toml} $out/${sub}/Cargo.toml + cp ${./Cargo.lock} $out/${sub}/Cargo.lock + + mkdir -p $out/native/rust + cp -r ${dimos-repo}/native/rust/dimos-module $out/native/rust/dimos-module + cp -r ${dimos-repo}/native/rust/dimos-module-macros $out/native/rust/dimos-module-macros + ''; + in { + packages.default = pkgs.rustPlatform.buildRustPackage { + pname = "virtual-mid360"; + version = "0.1.0"; + + inherit src; + cargoRoot = sub; + buildAndTestSubdir = sub; + + # Vendor straight from Cargo.lock. nix's fetchurl sends a User-Agent, + # so crates.io won't 403 it the way nixpkgs' fetchCargoVendor util does. + # The dimos-lcm git dep needs its fetched tree hash pinned here. + cargoLock = { + lockFile = ./Cargo.lock; + outputHashes = { + "dimos-lcm-0.1.0" = "sha256-4DWFTf7Xqnx6pd2jXA/MVpRmZiFr6HqTSp9Qo9ZjToA="; + }; + }; + + meta.mainProgram = "virtual_mid360"; + }; + }); +} diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py new file mode 100644 index 0000000000..d50cf2cd9a --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py @@ -0,0 +1,68 @@ +# 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. + +"""Python NativeModule wrapper for the virtual_mid360 Rust binary. + +virtual_mid360 replays a recorded Livox Mid-360 pcap onto a virtual network +interface, rewriting packet timestamps to current-time and synthesizing the +Livox SDK2 control protocol so an unmodified consumer (e.g. PointLio) connects +to it as if it were a real sensor. It carries no dimos streams; it speaks the +Livox wire protocol over UDP, so consumers reach it by host_ip/lidar_ip, not by +stream wiring. + +Usage:: + + from dimos.hardware.sensors.lidar.livox.virtual_mid360.module import VirtualMid360 + from dimos.hardware.sensors.lidar.pointlio.module import PointLio + from dimos.core.coordination.blueprints import autoconnect + + autoconnect( + VirtualMid360.blueprint(pcap="/path/to/ruwik2.pcap"), + PointLio.blueprint(), + ) +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from dimos.core.native_module import NativeModule, NativeModuleConfig + + +class VirtualMid360Config(NativeModuleConfig): + cwd: str | None = "." + executable: str = "result/bin/virtual_mid360" + build_command: str | None = "nix build .#default" + + # Recorded Mid-360 pcap (point/IMU/status UDP). Read fully into RAM. + pcap: str = "" + # Replay-speed multiplier; 1.0 = original inter-packet timing. + rate: float = 1.0 + # Seconds to wait after start before streaming begins. + delay: float = 0.0 + # IP the fake lidar sends from (must be on this netns's veth). + lidar_ip: str = "192.168.1.155" + # Host IP the recorded data is delivered to (where the SDK listens). + host_ip: str = "192.168.1.5" + # Network namespace the fake lidar runs inside. + lidar_netns: str = "lidar" + + +class VirtualMid360(NativeModule): + config: VirtualMid360Config + + +# Verify the module constructs (mirrors the pointlio wrapper's check). +if TYPE_CHECKING: + VirtualMid360() diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs new file mode 100644 index 0000000000..1472b8c17b --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs @@ -0,0 +1,511 @@ +// Fake Livox Mid-360 — replays a recorded pcap over a virtual NIC and synthesizes +// the Livox SDK2 control handshake so an unmodified, live-mode pointlio ingests it +// through the real Livox SDK as if from a live sensor. +// +// Inverse of pointlio's in-process `cpp/pcap_replay.hpp` (--replay_pcap), which +// bypasses the network. This exercises the full live stack: SDK discovery + +// control handshake, then point/IMU UDP off a (virtual) wire. +// +// Runs inside the "lidar" network namespace (see setup_commands()); the unmodified +// pointlio runs in the peer "drv" namespace. On any failure the error names the +// exact command to run, then asks the user to re-run the module. + +use dimos_module::{run, LcmTransport, Module}; +use serde::Deserialize; +use std::net::{Ipv4Addr, SocketAddrV4, UdpSocket}; +use std::sync::atomic::{AtomicBool, Ordering}; +use std::sync::Arc; +use std::time::{Duration, Instant, SystemTime, UNIX_EPOCH}; +use validator::Validate; + +// ---- Livox SDK2 control wire format (SdkPacket) ---- +const SOF: u8 = 0xAA; +const WRAPPER: usize = 24; // bytes before data[] +const CMD_PORT: u16 = 56100; +const DISCOVERY_PORT: u16 = 56000; +// data plane: lidar src port -> host dst port +const PORT_POINT: u16 = 56300; +const PORT_IMU: u16 = 56400; +const PORT_STATUS: u16 = 56200; +const DST_POINT: u16 = 56301; +const DST_IMU: u16 = 56401; +const DST_STATUS: u16 = 56201; +// Mid-360 multicasts point/IMU data to this group (the SDK joins it). Add a +// route (224.1.1.5/32 dev ) so it egresses the virtual NIC. +const MCAST_DATA: Ipv4Addr = Ipv4Addr::new(224, 1, 1, 5); +// cmd_id whose ACK means the host finished configuring -> start streaming +const CMD_WORKMODE: u16 = 0x0100; + +#[derive(Debug, Deserialize, Validate)] +#[serde(deny_unknown_fields)] +struct Config { + /// Recorded Mid-360 pcap (data plane: point/IMU/status UDP). Read fully into RAM. + pcap: String, + /// Replay-speed multiplier; 1.0 = original inter-packet timing, >1 = faster. + #[serde(default = "one")] + #[validate(range(min = 0.01, max = 1000.0))] + rate: f64, + /// Seconds to wait after start before streaming begins. + #[serde(default)] + #[validate(range(min = 0.0, max = 3600.0))] + delay: f64, + /// IP the fake lidar sends from (must be assigned to this netns's veth). + #[serde(default = "default_lidar_ip")] + lidar_ip: String, + /// Host IP the recorded data is delivered to (where pointlio's SDK listens). + #[serde(default = "default_host_ip")] + host_ip: String, + /// Network namespace the fake lidar must run inside. + #[serde(default = "default_netns")] + lidar_netns: String, +} + +fn one() -> f64 { + 1.0 +} +fn default_lidar_ip() -> String { + "192.168.1.155".into() +} +fn default_host_ip() -> String { + "192.168.1.5".into() +} +fn default_netns() -> String { + "lidar".into() +} + +#[derive(Module)] +#[module(setup = start)] +struct VirtualMid360 { + #[config] + config: Config, +} + +// ---- CRCs (Livox SDK2: CRC16-CCITT-FALSE over header[0:18], CRC32/IEEE over data[]) ---- +fn crc16_ccitt_false(data: &[u8]) -> u16 { + let mut crc: u16 = 0xFFFF; + for &b in data { + crc ^= (b as u16) << 8; + for _ in 0..8 { + crc = if crc & 0x8000 != 0 { + (crc << 1) ^ 0x1021 + } else { + crc << 1 + }; + } + } + crc +} + +fn crc32_ieee(data: &[u8]) -> u32 { + let mut crc: u32 = 0xFFFF_FFFF; + for &b in data { + crc ^= b as u32; + for _ in 0..8 { + crc = if crc & 1 != 0 { + (crc >> 1) ^ 0xEDB8_8320 + } else { + crc >> 1 + }; + } + } + !crc +} + +/// Build a Livox SDK2 ACK frame from scratch (synthesized, not replayed): +/// header[0:18] (SOF, version=0, length, seq, cmd_id, cmd_type=1 ACK, sender_type=1) +/// + crc16_h@18 + data[] + crc32_d. `data` is the per-cmd ACK payload. +fn build_ack(cmd_id: u16, seq: u32, data: &[u8]) -> Vec { + let length = (WRAPPER + data.len()) as u16; + let mut f = vec![0u8; WRAPPER + data.len()]; + f[0] = SOF; + f[1] = 0; // version + f[2..4].copy_from_slice(&length.to_le_bytes()); + f[4..8].copy_from_slice(&seq.to_le_bytes()); + f[8..10].copy_from_slice(&cmd_id.to_le_bytes()); + f[10] = 1; // cmd_type = ACK + f[11] = 1; // sender_type = lidar + // f[12..18] reserved (0) + let crc16 = crc16_ccitt_false(&f[0..18]); + f[18..20].copy_from_slice(&crc16.to_le_bytes()); + // f[20..24] = crc32 of data[] + f[24..].copy_from_slice(data); + let crc32 = crc32_ieee(data); + f[20..24].copy_from_slice(&crc32.to_le_bytes()); + f +} + +// ---- classic pcap (LE, magic d4c3b2a1) parser -> data-plane UDP packets ---- +struct Pkt { + ts: f64, + src_port: u16, + payload: Vec, +} + +fn parse_pcap(path: &str) -> std::io::Result> { + let buf = std::fs::read(path)?; + if buf.len() < 24 || buf[0..4] != [0xd4, 0xc3, 0xb2, 0xa1] { + return Err(std::io::Error::new( + std::io::ErrorKind::InvalidData, + format!("unsupported pcap (need classic little-endian, magic d4c3b2a1) at {path}"), + )); + } + let mut out = Vec::new(); + let mut off = 24usize; + while off + 16 <= buf.len() { + let ts_sec = u32::from_le_bytes(buf[off..off + 4].try_into().unwrap()); + let ts_usec = u32::from_le_bytes(buf[off + 4..off + 8].try_into().unwrap()); + let incl = u32::from_le_bytes(buf[off + 8..off + 12].try_into().unwrap()) as usize; + off += 16; + if off + incl > buf.len() { + break; + } + let frame = &buf[off..off + incl]; + off += incl; + // Ethernet(14) -> IPv4 -> UDP + if frame.len() < 14 + 20 + 8 || frame[12] != 0x08 || frame[13] != 0x00 { + continue; + } + let ihl = ((frame[14] & 0x0f) as usize) * 4; + if frame[14 + 9] != 17 { + continue; // not UDP + } + let udp = 14 + ihl; + if frame.len() < udp + 8 { + continue; + } + let src_port = u16::from_be_bytes([frame[udp], frame[udp + 1]]); + let udp_len = u16::from_be_bytes([frame[udp + 4], frame[udp + 5]]) as usize; + let payload_start = udp + 8; + let payload_end = (udp + udp_len).min(frame.len()); + if payload_end <= payload_start { + continue; + } + out.push(Pkt { + ts: ts_sec as f64 + ts_usec as f64 / 1e6, + src_port, + payload: frame[payload_start..payload_end].to_vec(), + }); + } + Ok(out) +} + +/// Verify we're in the lidar netns with lidar_ip bindable; else return a helpful +/// error naming the exact `sudo ip netns ...` commands and to re-run. +fn ensure_interface(cfg: &Config) -> Result { + let lidar_ip: Ipv4Addr = cfg + .lidar_ip + .parse() + .map_err(|_| format!("invalid lidar_ip '{}'", cfg.lidar_ip))?; + // Probe: can we bind the lidar control port on lidar_ip? If not, the veth/netns + // isn't set up (or we're in the wrong namespace). + let probe = UdpSocket::bind(SocketAddrV4::new(lidar_ip, CMD_PORT)); + if probe.is_err() { + let ns = &cfg.lidar_netns; + let lip = &cfg.lidar_ip; + let hip = &cfg.host_ip; + return Err(format!( + "cannot bind {lip}:{CMD_PORT} — the virtual network interface isn't set up \ + (or this process isn't in the '{ns}' netns).\n\ + Run this once (creates the lidar/drv veth pair), then re-run the module:\n\ + \n sudo ip netns add drv\n sudo ip netns add {ns}\n \ + sudo ip link add veth-drv type veth peer name veth-lidar\n \ + sudo ip link set veth-drv netns drv\n \ + sudo ip link set veth-lidar netns {ns}\n \ + sudo ip netns exec drv ip addr add {hip}/24 dev veth-drv\n \ + sudo ip netns exec {ns} ip addr add {lip}/24 dev veth-lidar\n \ + sudo ip netns exec drv ip link set veth-drv up\n \ + sudo ip netns exec {ns} ip link set veth-lidar up\n \ + sudo ip netns exec drv ip link set lo up\n \ + sudo ip netns exec {ns} ip link set lo up\n \ + sudo ip netns exec drv ip link set veth-drv multicast on\n \ + sudo ip netns exec {ns} ip link set veth-lidar multicast on\n \ + sudo ip netns exec {ns} ip route add 255.255.255.255/32 dev veth-lidar\n \ + sudo ip netns exec {ns} ip route add 224.1.1.5/32 dev veth-lidar # point/IMU multicast\n \ + sudo ip netns exec drv ip route add 224.0.0.0/4 dev lo # LCM (dimos transport)\n \ + sudo ip netns exec {ns} ip route add 224.0.0.0/4 dev lo # LCM (dimos transport)\n\ + \nThen launch this module inside the lidar netns:\n \ + sudo ip netns exec {ns} " + )); + } + Ok(lidar_ip) +} + +impl VirtualMid360 { + async fn start(&mut self) { + let cfg = &self.config; + let lidar_ip = match ensure_interface(cfg) { + Ok(ip) => ip, + Err(msg) => { + // Actionable error: print the fix command, then exit non-zero so the + // coordinator surfaces it and the user can re-run after setup. + tracing::error!("{msg}"); + eprintln!("\n[virtual_mid360] {msg}\n"); + std::process::exit(2); + } + }; + let host_ip: Ipv4Addr = cfg.host_ip.parse().expect("host_ip validated bindable"); + + let packets = match parse_pcap(&cfg.pcap) { + Ok(p) if !p.is_empty() => Arc::new(p), + Ok(_) => { + eprintln!( + "[virtual_mid360] pcap '{}' has no Livox UDP data packets. \ + Check the path / that it's a Mid-360 capture, then re-run.", + cfg.pcap + ); + std::process::exit(2); + } + Err(e) => { + eprintln!( + "[virtual_mid360] failed to read pcap '{}': {e}. Fix the path, then re-run.", + cfg.pcap + ); + std::process::exit(2); + } + }; + + let stop = Arc::new(AtomicBool::new(false)); + let armed = Arc::new(AtomicBool::new(false)); + let rate = cfg.rate; + let delay = cfg.delay; + + // Role 1: discovery responder (:56000 broadcast) — synthesize the 0x0000 ACK. + spawn_discovery(lidar_ip, stop.clone()); + // Role 2: control responder (:56100) — synthesize per-cmd ACKs; arm streaming + // when the host issues the work-mode/config command (0x0100). + spawn_control(lidar_ip, armed.clone(), stop.clone()); + // Role 3: data streamer — point/IMU/status, paced at `rate`, timestamps rewritten + // to now, armed by the handshake (with `delay` as a startup floor / fallback). + spawn_stream(lidar_ip, host_ip, packets, rate, delay, armed, stop); + tracing::info!(lidar = %lidar_ip, host = %host_ip, rate, delay, "virtual_mid360 started"); + } +} + +fn spawn_discovery(lidar_ip: Ipv4Addr, stop: Arc) { + std::thread::spawn(move || { + let sock = match UdpSocket::bind(SocketAddrV4::new(Ipv4Addr::UNSPECIFIED, DISCOVERY_PORT)) { + Ok(s) => s, + Err(e) => { + tracing::error!("discovery bind :{DISCOVERY_PORT} failed: {e}"); + return; + } + }; + let _ = sock.set_broadcast(true); + sock.set_read_timeout(Some(Duration::from_millis(500))).ok(); + let bcast = SocketAddrV4::new(Ipv4Addr::BROADCAST, DISCOVERY_PORT); + let mut buf = [0u8; 2048]; + while !stop.load(Ordering::Relaxed) { + let n = match sock.recv_from(&mut buf) { + Ok((n, _)) => n, + Err(_) => continue, + }; + if n < WRAPPER || buf[0] != SOF { + continue; + } + let cmd_id = u16::from_le_bytes([buf[8], buf[9]]); + let cmd_type = buf[10]; + if cmd_id != 0x0000 || cmd_type != 0 { + continue; + } + let seq = u32::from_le_bytes([buf[4], buf[5], buf[6], buf[7]]); + // TODO(payload): discovery ACK data describes the device (dev_type, serial, + // lidar_ip, cmd port). Enumerate the exact layout from livox-sdk2 source. + let ack = build_ack(0x0000, seq, &discovery_ack_payload(lidar_ip)); + let _ = sock.send_to(&ack, bcast); + } + }); +} + +fn spawn_control(lidar_ip: Ipv4Addr, armed: Arc, stop: Arc) { + std::thread::spawn(move || { + let sock = match UdpSocket::bind(SocketAddrV4::new(lidar_ip, CMD_PORT)) { + Ok(s) => s, + Err(e) => { + tracing::error!("control bind {lidar_ip}:{CMD_PORT} failed: {e}"); + return; + } + }; + sock.set_read_timeout(Some(Duration::from_millis(500))).ok(); + let mut buf = [0u8; 2048]; + while !stop.load(Ordering::Relaxed) { + let (n, from) = match sock.recv_from(&mut buf) { + Ok(x) => x, + Err(_) => continue, + }; + if n < WRAPPER || buf[0] != SOF { + continue; + } + let seq = u32::from_le_bytes([buf[4], buf[5], buf[6], buf[7]]); + let cmd_id = u16::from_le_bytes([buf[8], buf[9]]); + // TODO(payload): per-cmd_id ACK data. Most replies = ret_code(u8)=0 (success); + // queries echo the requested fields. Enumerate cmd_ids + payloads from + // livox-sdk2 source (comm/command_impl) or one captured real handshake. + let ack = build_ack(cmd_id, seq, &control_ack_payload(cmd_id)); + let _ = sock.send_to(&ack, from); + tracing::info!( + cmd_id = format!("0x{cmd_id:04x}"), + seq, + "control REQ -> ACK" + ); + if cmd_id == CMD_WORKMODE { + armed.store(true, Ordering::Relaxed); + tracing::info!("work-mode cmd 0x0100 acked -> arming data stream"); + } + } + }); +} + +fn spawn_stream( + lidar_ip: Ipv4Addr, + host_ip: Ipv4Addr, + packets: Arc>, + rate: f64, + delay: f64, + armed: Arc, + stop: Arc, +) { + std::thread::spawn(move || { + let mk = |sport: u16| -> std::io::Result { + UdpSocket::bind(SocketAddrV4::new(lidar_ip, sport)) + }; + let (point, imu, status) = match (mk(PORT_POINT), mk(PORT_IMU), mk(PORT_STATUS)) { + (Ok(a), Ok(b), Ok(c)) => (a, b, c), + _ => { + tracing::error!("failed to bind data-plane source ports on {lidar_ip}"); + return; + } + }; + // Wait for handshake to arm streaming, with `delay` as a startup floor + fallback. + let waited = Instant::now(); + while !armed.load(Ordering::Relaxed) && !stop.load(Ordering::Relaxed) { + if waited.elapsed().as_secs_f64() >= delay.max(0.0) && delay > 0.0 { + tracing::warn!("no handshake within delay={delay}s — arming stream anyway"); + break; + } + std::thread::sleep(Duration::from_millis(50)); + } + std::thread::sleep(Duration::from_secs_f64(delay.max(0.0))); + tracing::info!("streaming {} packets at {rate}x", packets.len()); + + // Shift every packet's Livox sensor timestamp by a constant so the first + // emitted packet reads ≈ now and the original inter-packet spacing (used for + // intra-scan deskew) is preserved — the stream looks current/live. + let now_ns = SystemTime::now() + .duration_since(UNIX_EPOCH) + .unwrap() + .as_nanos() as u64; + let first_orig = packets + .iter() + .find(|p| matches!(p.src_port, PORT_POINT | PORT_IMU)) + .map(|p| read_ts_ns(&p.payload)) + .unwrap_or(0); + let ts_shift = now_ns.wrapping_sub(first_orig); + + let t_wall0 = Instant::now(); + let mut t_cap0: Option = None; + for p in packets.iter() { + if stop.load(Ordering::Relaxed) { + break; + } + // The Mid-360 MULTICASTS point/IMU to MCAST_DATA:port (the SDK joins that + // group — confirmed via `ss -ulnp` showing 224.1.1.5:56301/56401); status + // is unicast to the host. Sending point/IMU unicast is silently dropped. + let (sock, dst_ip, dst) = match p.src_port { + PORT_POINT => (&point, MCAST_DATA, DST_POINT), + PORT_IMU => (&imu, MCAST_DATA, DST_IMU), + PORT_STATUS => (&status, host_ip, DST_STATUS), + _ => continue, + }; + let t0 = *t_cap0.get_or_insert(p.ts); + let target = (p.ts - t0) / rate; + let elapsed = t_wall0.elapsed().as_secs_f64(); + if target > elapsed { + std::thread::sleep(Duration::from_secs_f64(target - elapsed)); + } + let mut out = p.payload.clone(); + if matches!(p.src_port, PORT_POINT | PORT_IMU) { + rewrite_ts(&mut out, ts_shift); + } + let _ = sock.send_to(&out, SocketAddrV4::new(dst_ip, dst)); + } + tracing::info!("data stream finished"); + }); +} + +// ---- payload synthesizers (layouts from Livox-SDK2 sdk_core/comm/define.h) ---- +// Mid-360 device type (livox_lidar_def.h: kLivoxLidarTypeMid360 = 9). +const DEV_TYPE_MID360: u8 = 9; + +/// Detection/search (0x0000) ACK body == `DetectionData`: +/// ret_code:u8, dev_type:u8, sn[16], lidar_ip[4], cmd_port:u16 LE. +/// The SDK's VerifyNetSegment requires lidar_ip on the host's /24 (192.168.1.x). +fn discovery_ack_payload(lidar_ip: Ipv4Addr) -> Vec { + let mut d = Vec::with_capacity(24); + d.push(0); // ret_code = success + d.push(DEV_TYPE_MID360); + // sn[16] MUST be null-terminated within 16 bytes — the SDK treats it as a + // C-string (strcpy), so a full-16 SN with no NUL overruns its buffer. + let mut sn = [0u8; 16]; + sn[..10].copy_from_slice(b"FAKEMID360"); // sn[10..]=0 -> NUL-terminated + d.extend_from_slice(&sn); + d.extend_from_slice(&lidar_ip.octets()); + d.extend_from_slice(&CMD_PORT.to_le_bytes()); + d +} + +// kKeyFwType (livox_lidar_def.h ParamKeyName = 0x8010); fw_type != 0 => app +// firmware (not loader/upgrade mode), so the SDK proceeds to normal operation. +const KEY_FW_TYPE: u16 = 0x8010; +const FW_TYPE_APP: u8 = 1; + +/// Control-plane ACK bodies. The SDK casts the SdkPacket data[] directly to the +/// per-cmd response struct, which are #pragma pack(1) (packed, no padding). +fn control_ack_payload(cmd_id: u16) -> Vec { + match cmd_id { + // GetInternalInfo (0x0101): LivoxLidarDiagInternalInfoResponse (packed) — + // ret_code:u8 @0, param_num:u16 @1, data @3 (= LivoxLidarKeyValueParam: + // key:u16 @0, length:u16 @2, value @4). QueryFwType expects one param + // keyed kKeyFwType (0x8010) with a 1-byte fw_type value (non-zero = app). + 0x0101 => { + let mut d = vec![0u8; 8]; + // d[0] ret_code = 0 + d[1..3].copy_from_slice(&1u16.to_le_bytes()); // param_num = 1 + d[3..5].copy_from_slice(&KEY_FW_TYPE.to_le_bytes()); + d[5..7].copy_from_slice(&1u16.to_le_bytes()); // value length = 1 + d[7] = FW_TYPE_APP; + d + } + // Others: LivoxLidarAsyncControlResponse (packed) { ret_code:u8 @0, + // error_key:u16 @1 } = 3 bytes. ret_code=0 (success), error_key=0. + _ => vec![0u8; 3], + } +} +// LivoxLidarEthernetPacket.timestamp[8] sits at payload offset 28 (packed: +// version@0,len@1,time_interval@3,dot_num@5,udp_cnt@7,frame_cnt@9,data_type@10, +// time_type@11,rsvd@12,crc32@24,timestamp@28). The SDK casts the UDP payload +// directly to LivoxLidarEthernetPacket*, so offset 28 is in the payload. +const TS_OFFSET: usize = 28; + +fn read_ts_ns(payload: &[u8]) -> u64 { + if payload.len() >= TS_OFFSET + 8 { + u64::from_le_bytes(payload[TS_OFFSET..TS_OFFSET + 8].try_into().unwrap()) + } else { + 0 + } +} + +fn rewrite_ts(payload: &mut [u8], shift: u64) { + if payload.len() >= TS_OFFSET + 8 { + let orig = u64::from_le_bytes(payload[TS_OFFSET..TS_OFFSET + 8].try_into().unwrap()); + let new = orig.wrapping_add(shift); + payload[TS_OFFSET..TS_OFFSET + 8].copy_from_slice(&new.to_le_bytes()); + } +} + +#[tokio::main] +async fn main() { + let transport = LcmTransport::new() + .await + .expect("Failed to create transport"); + run::(transport).await; +} diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index eb1793f984..9fd2d4b94e 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -54,6 +54,7 @@ "demo-object-scene-registration": "dimos.perception.demo_object_scene_registration:demo_object_scene_registration", "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", + "demo-virtual-mid360-fastlio": "dimos.hardware.sensors.lidar.livox.virtual_mid360.blueprints:demo_virtual_mid360_fastlio", "desk-marker-tf": "dimos.perception.fiducial.blueprints.desk_marker_tf:desk_marker_tf", "drone-agentic": "dimos.robot.drone.blueprints.agentic.drone_agentic:drone_agentic", "drone-basic": "dimos.robot.drone.blueprints.basic.drone_basic:drone_basic", @@ -235,6 +236,7 @@ "unitree-skill-container": "dimos.robot.unitree.unitree_skill_container.UnitreeSkillContainer", "unity-bridge-module": "dimos.simulation.unity.module.UnityBridgeModule", "video-arm-teleop-module": "dimos.teleop.quest.quest_extensions.VideoArmTeleopModule", + "virtual-mid360": "dimos.hardware.sensors.lidar.livox.virtual_mid360.module.VirtualMid360", "vlm-agent": "dimos.agents.vlm_agent.VLMAgent", "vlm-stream-tester": "dimos.agents.vlm_stream_tester.VlmStreamTester", "voxel-grid-mapper": "dimos.mapping.voxels.VoxelGridMapper", From 1c421289ad8662f353176f75558a33878b33419a Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 21:17:16 -0700 Subject: [PATCH 070/185] fastlio2: remove the in-process pcap replay subsystem virtual_mid360 is now the only replay path, so strip the in-process reader from the binary: delete cpp/pcap_replay.hpp and remove the feeder thread, virtual clock, deterministic-clock mode, and first-packet-marker from main.cpp; drop replay_pcap/deterministic_clock/replay_skip_until_ns/first_packet_marker from module.py and always validate the network. FastLio2 is now a pure live-SDK binary. --- .../sensors/lidar/fastlio2/cpp/main.cpp | 248 ++---------------- .../lidar/fastlio2/cpp/pcap_replay.hpp | 222 ---------------- .../hardware/sensors/lidar/fastlio2/module.py | 20 +- 3 files changed, 26 insertions(+), 464 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/pcap_replay.hpp diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index 86e8bb139e..c1ec26831b 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -33,7 +33,6 @@ #include "cloud_filter.hpp" #include "dimos_native_module.hpp" -#include "pcap_replay.hpp" #include "timing.hpp" // dimos LCM message headers @@ -61,76 +60,15 @@ static std::atomic g_running{true}; static lcm::LCM* g_lcm = nullptr; static FastLio* g_fastlio = nullptr; -// Virtual clock: in replay mode, tracks the pcap timestamp of the packet -// currently being fed so publish_*() reports the original capture time -// instead of replay wall time. Live mode leaves it at 0 and publish_*() -// falls back to system_clock::now(). -static std::atomic g_replay_mode{false}; -static std::atomic g_virtual_clock_ns{0}; - -// Deterministic clock mode. When set, both live and replay drive -// g_virtual_clock_ns from the packet's sensor-clock pkt->timestamp (which -// is identical bit-for-bit between SDK delivery and pcap), and use it as -// the source for scan-boundary rate limits and publish timestamps. This -// removes wall-clock jitter from scan boundaries → live and replay produce -// the same algorithm state. Trade-off: published header.stamp values -// become sensor-boot-relative seconds instead of unix wall time, so this -// is off by default and only flipped on by the record/replay demos. -static std::atomic g_deterministic_clock{false}; - -// First packet's sensor ts (deterministic mode only). Used to seed the -// main loop's rate-limit bookmarks at exactly the first delivered packet, -// independent of when the main loop's first iteration happens to run. -static std::atomic g_first_packet_clock_ns{0}; - -// First-packet marker. Used by record/replay tooling to align the SDK's -// warmup-induced packet drop with replay. The C++ binary writes the wall -// clock of the first on_point_cloud / on_imu_data callback (live mode -// only) to this file; demo_replay reads it back and passes the value as -// --replay_skip_until_ns so pcap_replay drops the same SDK-eaten prefix. -static std::string g_first_packet_marker_path; -static std::atomic g_first_packet_marker_written{false}; - -// The packet's sensor-clock timestamp (pkt->timestamp) is identical bit-for-bit -// between the live SDK delivery path and the recorded pcap, so writing it from -// the first SDK callback gives replay an exact boundary to skip on. Wall clock -// would only let us match within delivery latency (sub-ms). -static void mark_first_packet(uint64_t pkt_timestamp_ns) { - if (g_first_packet_marker_path.empty()) { - return; - } - bool expected = false; - if (!g_first_packet_marker_written.compare_exchange_strong(expected, true)) { - return; - } - FILE* f = std::fopen(g_first_packet_marker_path.c_str(), "w"); - if (f) { - std::fprintf(f, "%lu\n", static_cast(pkt_timestamp_ns)); - std::fclose(f); - } -} - static double get_publish_ts() { - if (g_deterministic_clock.load() || g_replay_mode.load()) { - return static_cast(g_virtual_clock_ns.load()) / 1e9; - } return std::chrono::duration( std::chrono::system_clock::now().time_since_epoch()).count(); } -// Virtualized clock for the main loop's frame/publish rate limiters. In -// replay mode this returns a time_point derived from g_virtual_clock_ns so -// scan boundaries are determined by packet arrival, not by wall-clock thread -// scheduling jitter. Returns nullopt if replay hasn't yet seen a packet -// (caller should skip rate-limit checks in that case). +// Clock for the main loop's frame/publish rate limiters. Returns an optional +// for backward-compatibility with callers that check has_value(); the live +// path always populates it. static std::optional virtual_now() { - if (g_deterministic_clock.load() || g_replay_mode.load()) { - uint64_t ns = g_virtual_clock_ns.load(); - if (ns == 0) { - return std::nullopt; - } - return std::chrono::steady_clock::time_point(std::chrono::nanoseconds(ns)); - } return std::chrono::steady_clock::now(); } @@ -343,25 +281,8 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ uint64_t ts_ns = get_timestamp_ns(data); uint16_t dot_num = data->dot_num; - if (!g_replay_mode.load()) { - mark_first_packet(ts_ns); - } - std::lock_guard lock(g_pc_mutex); - // Update the deterministic-mode virtual clock INSIDE the accumulator - // mutex so the main loop can never observe a clock advance without - // also seeing the matching points (race that drifts scan composition). - // Monotonic update: SDK threads can deliver packets slightly out of - // sensor-ts order, and we must not let a later store(lower_ts) undo - // a previous store(higher_ts). - if (g_deterministic_clock.load()) { - uint64_t expected = 0; - g_first_packet_clock_ns.compare_exchange_strong(expected, ts_ns); - uint64_t cur = g_virtual_clock_ns.load(); - while (cur < ts_ns && !g_virtual_clock_ns.compare_exchange_weak(cur, ts_ns)) {} - } - if (!g_frame_has_timestamp) { g_frame_start_ns = ts_ns; g_frame_has_timestamp = true; @@ -401,9 +322,6 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, if (!g_running.load() || data == nullptr || !g_fastlio) return; uint64_t pkt_ts_ns = get_timestamp_ns(data); - if (!g_replay_mode.load()) { - mark_first_packet(pkt_ts_ns); - } double ts = static_cast(pkt_ts_ns) / 1e9; auto* imu_pts = reinterpret_cast(data->data); @@ -436,18 +354,6 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, g_fastlio->feed_imu(imu_msg); } - - // Advance the deterministic-mode virtual clock AFTER feed_imu has - // queued the sample, holding g_pc_mutex so this is fully serialized - // with on_point_cloud / the main-loop scan swap. Monotonic CAS so - // out-of-order SDK delivery can't roll the clock backward. - if (g_deterministic_clock.load()) { - std::lock_guard lock(g_pc_mutex); - uint64_t expected = 0; - g_first_packet_clock_ns.compare_exchange_strong(expected, pkt_ts_ns); - uint64_t cur = g_virtual_clock_ns.load(); - while (cur < pkt_ts_ns && !g_virtual_clock_ns.compare_exchange_weak(cur, pkt_ts_ns)) {} - } } static void on_info_change(const uint32_t handle, const LivoxLidarInfo* info, @@ -543,34 +449,6 @@ int main(int argc, char** argv) { ports.host_imu_data = mod.arg_int("host_imu_data_port", port_defaults.host_imu_data); ports.host_log_data = mod.arg_int("host_log_data_port", port_defaults.host_log_data); - // Replay mode (offline). When --replay_pcap is given the SDK is not - // initialized; a feeder thread reads the pcap and calls the existing - // on_point_cloud / on_imu_data callbacks directly. publish_*() uses - // the pcap timestamps as the clock so outputs match the original run. - std::string replay_pcap = mod.arg("replay_pcap", ""); - g_replay_mode.store(!replay_pcap.empty()); - - // Drop pcap packets with pcap_ts < this value. Used in replay to mimic - // the SDK warmup discard that the live run experienced — so the - // algorithm starts from the same first packet in both modes. - uint64_t replay_skip_until_ns = 0; - { - std::string s = mod.arg("replay_skip_until_ns", "0"); - if (!s.empty()) { - replay_skip_until_ns = std::stoull(s); - } - } - - // Live mode: write the wall_clock_ns of the first SDK callback to this - // file. Pair with replay's --replay_skip_until_ns to align packet sets. - g_first_packet_marker_path = mod.arg("first_packet_marker", ""); - - // Drive virtual_now() and get_publish_ts() off the packet's sensor - // clock in both live and replay. Eliminates wall-clock jitter from - // scan boundaries and makes outputs bit-comparable across modes. - // Changes header.stamp from unix wall time to sensor-boot seconds. - g_deterministic_clock.store(mod.arg_bool("deterministic_clock", false)); - // Initial pose offset [x, y, z, qx, qy, qz, qw] { std::string init_str = mod.arg("init_pose", ""); @@ -626,10 +504,8 @@ int main(int argc, char** argv) { g_fastlio = &fast_lio; if (debug) printf("[fastlio2] FAST-LIO initialized.\n"); - // Main-loop state. The body lives in `run_main_iter` below so it can be - // invoked from either the wall-clock-paced main thread (live) or the - // pcap-paced feeder thread (replay), with no race on accumulator - // contents in the replay case. + // Main-loop state. The body lives in `run_main_iter` below, driven by the + // wall-clock-paced main thread. auto frame_interval = std::chrono::microseconds( static_cast(1e6 / g_frequency)); std::optional last_emit; @@ -659,20 +535,9 @@ int main(int argc, char** argv) { auto run_main_iter = [&](std::chrono::steady_clock::time_point now) { timing::Scope iter_scope(t_iter); // Lazy-seed all rate-limit bookmarks on the first iteration so they - // line up with the chosen clock (wall in live, pcap in replay) and - // don't fire immediately based on an arbitrary "since program start" - // delta. In deterministic mode we seed from the FIRST packet's - // sensor ts (not the current ts) so both live and replay anchor - // their first scan boundary at the same packet — required for - // bit-for-bit live↔replay parity. + // line up with the wall clock and don't fire immediately based on an + // arbitrary "since program start" delta. auto seed = now; - if (g_deterministic_clock.load()) { - uint64_t first = g_first_packet_clock_ns.load(); - if (first != 0) { - seed = std::chrono::steady_clock::time_point( - std::chrono::nanoseconds(first)); - } - } if (!last_emit.has_value()) { last_emit = seed; } @@ -685,23 +550,15 @@ int main(int argc, char** argv) { // At frame rate: drain accumulated raw points into a CustomMsg // and feed to FAST-LIO. Hold g_pc_mutex across the rate-limit - // CHECK as well as the swap, so in deterministic mode the - // virtual clock + accumulator are observed atomically with no - // other thread able to slip a packet in between the decision - // and the swap. + // CHECK as well as the swap so the accumulator is observed + // atomically with no other thread able to slip a packet in + // between the decision and the swap. std::vector points; uint64_t frame_start = 0; { timing::Scope s(t_emit_check); std::lock_guard lock(g_pc_mutex); auto check_now = now; - if (g_deterministic_clock.load()) { - uint64_t ns = g_virtual_clock_ns.load(); - if (ns != 0) { - check_now = std::chrono::steady_clock::time_point( - std::chrono::nanoseconds(ns)); - } - } if (check_now - *last_emit >= frame_interval) { if (!g_accumulated_points.empty()) { points.swap(g_accumulated_points); @@ -768,76 +625,26 @@ int main(int argc, char** argv) { timing::maybe_flush(std::chrono::steady_clock::now()); }; - // Source of point/IMU packets: - // live mode -> Livox SDK opens UDP sockets + dispatches via callbacks - // from its own threads. Main thread drives run_main_iter - // at main_freq, consuming whatever the SDK queued. - // replay mode -> the feeder thread reads the pcap and pushes packets - // through the same on_point/on_imu callbacks (paced at - // realtime via sleep_until). The MAIN thread — same - // one that runs in live mode — owns run_main_iter and - // drains the accumulator. Two threads in both modes, - // matching architectures, so the only difference is - // the source of packets (SDK vs pcap). - std::thread replay_thread; - if (g_replay_mode.load()) { - if (debug) printf("[fastlio2] REPLAY mode, pcap=%s\n", replay_pcap.c_str()); - replay_thread = std::thread([&]() { - pcap_replay::Replayer rep; - rep.path = replay_pcap; - rep.host_point_port = static_cast(ports.host_point_data); - rep.host_imu_port = static_cast(ports.host_imu_data); - rep.on_point = [](LivoxLidarEthernetPacket* p) { - on_point_cloud(0, 0, p, nullptr); - }; - rep.on_imu = [](LivoxLidarEthernetPacket* p) { - on_imu_data(0, 0, p, nullptr); - }; - rep.on_clock = [](uint64_t pcap_ts_ns) { - // In deterministic mode the callbacks already pushed the - // sensor pkt->timestamp into g_virtual_clock_ns — don't - // overwrite with the pcap (wall) ts. - if (g_deterministic_clock.load()) { - return; - } - g_virtual_clock_ns.store(pcap_ts_ns); - }; - // No rep.on_iter — the main thread drives run_main_iter in - // replay mode now, same as in live. This decouples packet - // ingestion from per-iter filter_cloud cost and lets replay - // run at the same wall throughput as live. - rep.running = &g_running; - // Pace the replay feeder at live wall-clock rate. sleep_until - // throttles the feeder so packets land in the accumulator at - // the same wall cadence as the SDK delivers in live mode. - rep.realtime = true; - rep.skip_until_ns = replay_skip_until_ns; - rep.run(); - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - g_running.store(false); - }); - } else { - if (!livox_common::init_livox_sdk(host_ip, lidar_ip, ports, debug)) { - return 1; - } - SetLivoxLidarPointCloudCallBack(on_point_cloud, nullptr); - SetLivoxLidarImuDataCallback(on_imu_data, nullptr); - SetLivoxLidarInfoChangeCallback(on_info_change, nullptr); - if (!LivoxLidarSdkStart()) { - fprintf(stderr, "Error: LivoxLidarSdkStart failed\n"); - LivoxLidarSdkUninit(); - return 1; - } - if (debug) printf("[fastlio2] SDK started, waiting for device...\n"); + // Source of point/IMU packets: the Livox SDK opens UDP sockets and + // dispatches via callbacks from its own threads. The main thread drives + // run_main_iter at main_freq, consuming whatever the SDK queued. + if (!livox_common::init_livox_sdk(host_ip, lidar_ip, ports, debug)) { + return 1; } + SetLivoxLidarPointCloudCallBack(on_point_cloud, nullptr); + SetLivoxLidarImuDataCallback(on_imu_data, nullptr); + SetLivoxLidarInfoChangeCallback(on_info_change, nullptr); + if (!LivoxLidarSdkStart()) { + fprintf(stderr, "Error: LivoxLidarSdkStart failed\n"); + LivoxLidarSdkUninit(); + return 1; + } + if (debug) printf("[fastlio2] SDK started, waiting for device...\n"); while (g_running.load()) { auto loop_start = std::chrono::high_resolution_clock::now(); auto now_opt = virtual_now(); if (!now_opt.has_value()) { - // No clock yet — in replay this means the feeder hasn't read - // its first packet; in live it shouldn't happen because - // virtual_now falls back to steady_clock::now(). std::this_thread::sleep_for(std::chrono::milliseconds(1)); continue; } @@ -858,12 +665,7 @@ int main(int argc, char** argv) { // Cleanup if (debug) printf("[fastlio2] Shutting down...\n"); g_fastlio = nullptr; - if (replay_thread.joinable()) { - replay_thread.join(); - } - if (!g_replay_mode.load()) { - LivoxLidarSdkUninit(); - } + LivoxLidarSdkUninit(); g_lcm = nullptr; if (debug) printf("[fastlio2] Done.\n"); diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/pcap_replay.hpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/pcap_replay.hpp deleted file mode 100644 index cfbc0e58c1..0000000000 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/pcap_replay.hpp +++ /dev/null @@ -1,222 +0,0 @@ -// Copyright 2026 Dimensional Inc. -// SPDX-License-Identifier: Apache-2.0 -// -// Read a pcap of recorded Mid-360 UDP traffic and feed each point/imu -// payload to the existing SDK callbacks. Used by `--replay_pcap` to bypass -// the Livox SDK for deterministic offline regression testing. - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "livox_lidar_def.h" - -namespace pcap_replay { - -constexpr uint32_t PCAP_MAGIC_LE_US = 0xa1b2c3d4u; -constexpr uint32_t PCAP_MAGIC_LE_NS = 0xa1b23c4du; -constexpr uint32_t LINKTYPE_ETHERNET = 1u; -constexpr uint16_t ETHERTYPE_IPV4 = 0x0800u; -constexpr uint8_t IPPROTO_UDP = 17u; -constexpr size_t ETH_HDR_LEN = 14; -constexpr size_t IP_MIN_HDR_LEN = 20; -constexpr size_t UDP_HDR_LEN = 8; -constexpr size_t LIVOX_ETH_HDR_LEN = 36; - -using PacketCb = std::function; -using ClockCb = std::function; -using IterCb = std::function; - -struct Replayer { - std::string path; - uint16_t host_point_port = 0; - uint16_t host_imu_port = 0; - PacketCb on_point; - PacketCb on_imu; - ClockCb on_clock; - // Called synchronously after every packet, once the payload has been - // appended and the virtual clock advanced. The replay path runs the - // main-loop body here so feeding + processing happen on a single - // thread — eliminates the feeder-vs-main-loop race on accumulator - // contents. - IterCb on_iter; - std::atomic* running = nullptr; - bool realtime = true; - // Drop Livox packets whose sensor timestamp (pkt->timestamp) is - // strictly less than this. Used to mimic the SDK warmup window from a - // paired live run so the algorithm starts from the same first packet - // in both modes. Comparing on sensor ts (which is identical bit-for-bit - // between live SDK delivery and pcap replay) is exact; comparing on - // wall pcap_ts would be off by SDK delivery latency. - uint64_t skip_until_ns = 0; - - bool run() { - std::ifstream f(path, std::ios::binary); - if (!f) { - fprintf(stderr, "[replay] cannot open %s\n", path.c_str()); - return false; - } - - uint8_t global_hdr[24]; - f.read(reinterpret_cast(global_hdr), 24); - if (!f) { - fprintf(stderr, "[replay] short read on pcap global header\n"); - return false; - } - uint32_t magic; - std::memcpy(&magic, global_hdr, 4); - const bool nanos = (magic == PCAP_MAGIC_LE_NS); - if (magic != PCAP_MAGIC_LE_US && magic != PCAP_MAGIC_LE_NS) { - fprintf(stderr, "[replay] unsupported pcap magic 0x%08x\n", magic); - return false; - } - uint32_t linktype; - std::memcpy(&linktype, global_hdr + 20, 4); - if (linktype != LINKTYPE_ETHERNET) { - fprintf(stderr, "[replay] unsupported linktype %u (need ETHERNET=1)\n", linktype); - return false; - } - - printf("[replay] reading %s (port=%u imu=%u realtime=%d)\n", - path.c_str(), host_point_port, host_imu_port, realtime ? 1 : 0); - - uint64_t first_pcap_ts_ns = 0; - std::chrono::steady_clock::time_point start_wall; - bool seeded = false; - - size_t pkts = 0, pts = 0, imu = 0, other = 0; - uint8_t rec_hdr[16]; - std::vector buf; - - while (running == nullptr || running->load()) { - f.read(reinterpret_cast(rec_hdr), 16); - if (!f) { - break; - } - - uint32_t ts_sec, ts_sub, incl_len, orig_len; - std::memcpy(&ts_sec, rec_hdr + 0, 4); - std::memcpy(&ts_sub, rec_hdr + 4, 4); - std::memcpy(&incl_len, rec_hdr + 8, 4); - std::memcpy(&orig_len, rec_hdr + 12, 4); - (void)orig_len; - - const uint64_t pcap_ts_ns = - static_cast(ts_sec) * 1'000'000'000ULL + - (nanos ? static_cast(ts_sub) : static_cast(ts_sub) * 1000ULL); - - buf.resize(incl_len); - f.read(reinterpret_cast(buf.data()), incl_len); - if (!f) { - break; - } - pkts++; - - if (buf.size() < ETH_HDR_LEN) { - continue; - } - uint16_t ethertype = (static_cast(buf[12]) << 8) | buf[13]; - if (ethertype != ETHERTYPE_IPV4) { - continue; - } - size_t ip_off = ETH_HDR_LEN; - if (buf.size() < ip_off + IP_MIN_HDR_LEN) { - continue; - } - uint8_t vihl = buf[ip_off]; - if ((vihl >> 4) != 4) { - continue; - } - int ihl = (vihl & 0x0f) * 4; - if (ihl < static_cast(IP_MIN_HDR_LEN)) { - continue; - } - if (buf[ip_off + 9] != IPPROTO_UDP) { - continue; - } - size_t udp_off = ip_off + ihl; - if (buf.size() < udp_off + UDP_HDR_LEN) { - continue; - } - uint16_t dst_port = (static_cast(buf[udp_off + 2]) << 8) | buf[udp_off + 3]; - uint16_t udp_len = (static_cast(buf[udp_off + 4]) << 8) | buf[udp_off + 5]; - size_t payload_off = udp_off + UDP_HDR_LEN; - size_t payload_end = std::min(buf.size(), static_cast(udp_off + udp_len)); - if (payload_end <= payload_off) { - continue; - } - size_t payload_len = payload_end - payload_off; - if (payload_len < LIVOX_ETH_HDR_LEN) { - continue; - } - - auto* livox_pkt = - reinterpret_cast(buf.data() + payload_off); - - // Sensor-clock skip: drop packets the live SDK wouldn't have - // seen (those before its first delivered callback) so the - // algorithm processes the same input set in both modes. - if (skip_until_ns > 0) { - uint64_t pkt_ts; - std::memcpy(&pkt_ts, livox_pkt->timestamp, sizeof(uint64_t)); - if (pkt_ts < skip_until_ns) { - continue; - } - } - - if (realtime) { - if (!seeded) { - first_pcap_ts_ns = pcap_ts_ns; - start_wall = std::chrono::steady_clock::now(); - seeded = true; - } else { - auto target = start_wall + std::chrono::nanoseconds(pcap_ts_ns - first_pcap_ts_ns); - auto now = std::chrono::steady_clock::now(); - if (target > now) { - std::this_thread::sleep_until(target); - } - } - } - - if (dst_port == host_point_port) { - if (on_point) { - on_point(livox_pkt); - } - pts++; - } else if (dst_port == host_imu_port) { - if (on_imu) { - on_imu(livox_pkt); - } - imu++; - } else { - other++; - } - // Advance the virtual clock AFTER the payload has been added to - // accumulators. Reverse order would let the main-loop thread see - // the clock advance and emit a scan that's missing this packet. - if (on_clock) { - on_clock(pcap_ts_ns); - } - - // Run one main-loop iteration synchronously so feeding and - // processing are strictly serialized in replay mode. - if (on_iter) { - on_iter(); - } - } - - printf("[replay] done: %zu pcap records (point=%zu imu=%zu other=%zu)\n", - pkts, pts, imu, other); - return true; - } -}; - -} // namespace pcap_replay diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index f9a4b02e33..9a547c5952 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -125,23 +125,6 @@ class FastLio2Config(NativeModuleConfig): # Resolved in __post_init__, passed as --config_path to the binary config_path: str | None = None - # Offline replay. When set, the C++ binary skips SDK init and feeds - # packets from this pcap into the same callbacks the SDK would, with - # publish timestamps driven by the pcap clock. - replay_pcap: Path | None = None - # Replay-only: drop pcap records with pcap_ts < this. Used to mimic the - # SDK warmup window from the paired live run. - replay_skip_until_ns: int | None = None - # Live-only: file path where the C++ binary writes the wall_ns of the - # first SDK callback, so a later replay can align its first packet. - first_packet_marker: Path | None = None - # Drive scan boundaries + publish ts off the sensor packet timestamp in - # both live and replay so they produce bit-for-bit identical outputs. - # Side effect: published header.stamp is sensor-boot seconds, not unix - # wall time. Off by default; only the deterministic record/replay path - # flips it on (real-time replay leaves it False). - deterministic_clock: bool = False - # init_pose is computed from mount; config is resolved to config_path init_pose: list[float] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] cli_exclude: frozenset[str] = frozenset({"config", "mount"}) @@ -173,8 +156,7 @@ class FastLio2(NativeModule, perception.Lidar, perception.Odometry): @rpc def start(self) -> None: - if self.config.replay_pcap is None: - self._validate_network() + self._validate_network() super().start() self.register_disposable( Disposable(self.odometry.transport.subscribe(self._on_odom_for_tf, self.odometry)) From 7bf679e4fffb8ca7205d961a469cb211a497e8ce Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 21:21:08 -0700 Subject: [PATCH 071/185] feat(virtual_mid360): make point/IMU multicast group configurable MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 224.1.1.5 was hardcoded. It's the Livox Mid-360 default multicast_ip (what the SDK joins, and what pointlio uses), but a consumer can configure a different multicast_ip — so the sender must match it. Make it a 'mcast_data' config field (default 224.1.1.5), validated as an IPv4 address, threaded into the data streamer + the netns-setup error recipe. Exposed on the Python VirtualMid360 wrapper too. --- .../lidar/livox/virtual_mid360/module.py | 4 ++ .../lidar/livox/virtual_mid360/src/main.rs | 39 +++++++++++++++---- 2 files changed, 35 insertions(+), 8 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py index d50cf2cd9a..980aad3a39 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py @@ -57,6 +57,10 @@ class VirtualMid360Config(NativeModuleConfig): host_ip: str = "192.168.1.5" # Network namespace the fake lidar runs inside. lidar_netns: str = "lidar" + # Multicast group the point/IMU streams are sent to. 224.1.1.5 is the Livox + # default that the SDK joins; override only to match a consumer configured + # with a different multicast_ip. + mcast_data: str = "224.1.1.5" class VirtualMid360(NativeModule): diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs index 1472b8c17b..03109267bd 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs @@ -30,9 +30,6 @@ const PORT_STATUS: u16 = 56200; const DST_POINT: u16 = 56301; const DST_IMU: u16 = 56401; const DST_STATUS: u16 = 56201; -// Mid-360 multicasts point/IMU data to this group (the SDK joins it). Add a -// route (224.1.1.5/32 dev ) so it egresses the virtual NIC. -const MCAST_DATA: Ipv4Addr = Ipv4Addr::new(224, 1, 1, 5); // cmd_id whose ACK means the host finished configuring -> start streaming const CMD_WORKMODE: u16 = 0x0100; @@ -58,6 +55,13 @@ struct Config { /// Network namespace the fake lidar must run inside. #[serde(default = "default_netns")] lidar_netns: String, + /// Multicast group the point/IMU streams are sent to. A real Mid-360 + /// multicasts these and the Livox SDK joins whatever `multicast_ip` is in + /// its host config; 224.1.1.5 is the Livox default (what pointlio uses), so + /// it's the default here. Override only to match a consumer configured with + /// a different `multicast_ip`. (Needs a `/32 dev ` route.) + #[serde(default = "default_mcast_data")] + mcast_data: String, } fn one() -> f64 { @@ -72,6 +76,9 @@ fn default_host_ip() -> String { fn default_netns() -> String { "lidar".into() } +fn default_mcast_data() -> String { + "224.1.1.5".into() +} #[derive(Module)] #[module(setup = start)] @@ -203,6 +210,7 @@ fn ensure_interface(cfg: &Config) -> Result { let ns = &cfg.lidar_netns; let lip = &cfg.lidar_ip; let hip = &cfg.host_ip; + let mcast = &cfg.mcast_data; return Err(format!( "cannot bind {lip}:{CMD_PORT} — the virtual network interface isn't set up \ (or this process isn't in the '{ns}' netns).\n\ @@ -220,7 +228,7 @@ fn ensure_interface(cfg: &Config) -> Result { sudo ip netns exec drv ip link set veth-drv multicast on\n \ sudo ip netns exec {ns} ip link set veth-lidar multicast on\n \ sudo ip netns exec {ns} ip route add 255.255.255.255/32 dev veth-lidar\n \ - sudo ip netns exec {ns} ip route add 224.1.1.5/32 dev veth-lidar # point/IMU multicast\n \ + sudo ip netns exec {ns} ip route add {mcast}/32 dev veth-lidar # point/IMU multicast\n \ sudo ip netns exec drv ip route add 224.0.0.0/4 dev lo # LCM (dimos transport)\n \ sudo ip netns exec {ns} ip route add 224.0.0.0/4 dev lo # LCM (dimos transport)\n\ \nThen launch this module inside the lidar netns:\n \ @@ -244,6 +252,17 @@ impl VirtualMid360 { } }; let host_ip: Ipv4Addr = cfg.host_ip.parse().expect("host_ip validated bindable"); + let mcast_data: Ipv4Addr = match cfg.mcast_data.parse() { + Ok(ip) => ip, + Err(_) => { + eprintln!( + "[virtual_mid360] invalid mcast_data '{}' — expected an IPv4 multicast \ + address matching the consumer's Livox multicast_ip (default 224.1.1.5).", + cfg.mcast_data + ); + std::process::exit(2); + } + }; let packets = match parse_pcap(&cfg.pcap) { Ok(p) if !p.is_empty() => Arc::new(p), @@ -276,7 +295,9 @@ impl VirtualMid360 { spawn_control(lidar_ip, armed.clone(), stop.clone()); // Role 3: data streamer — point/IMU/status, paced at `rate`, timestamps rewritten // to now, armed by the handshake (with `delay` as a startup floor / fallback). - spawn_stream(lidar_ip, host_ip, packets, rate, delay, armed, stop); + spawn_stream( + lidar_ip, host_ip, mcast_data, packets, rate, delay, armed, stop, + ); tracing::info!(lidar = %lidar_ip, host = %host_ip, rate, delay, "virtual_mid360 started"); } } @@ -355,9 +376,11 @@ fn spawn_control(lidar_ip: Ipv4Addr, armed: Arc, stop: Arc>, rate: f64, delay: f64, @@ -407,12 +430,12 @@ fn spawn_stream( if stop.load(Ordering::Relaxed) { break; } - // The Mid-360 MULTICASTS point/IMU to MCAST_DATA:port (the SDK joins that + // The Mid-360 MULTICASTS point/IMU to mcast_data:port (the SDK joins that // group — confirmed via `ss -ulnp` showing 224.1.1.5:56301/56401); status // is unicast to the host. Sending point/IMU unicast is silently dropped. let (sock, dst_ip, dst) = match p.src_port { - PORT_POINT => (&point, MCAST_DATA, DST_POINT), - PORT_IMU => (&imu, MCAST_DATA, DST_IMU), + PORT_POINT => (&point, mcast_data, DST_POINT), + PORT_IMU => (&imu, mcast_data, DST_IMU), PORT_STATUS => (&status, host_ip, DST_STATUS), _ => continue, }; From 1118f2383cf07494805f50e12d19944f15caf583 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 21:30:54 -0700 Subject: [PATCH 072/185] pcap_to_db: orchestrate virtual_mid360 behind the --pcap interface Restore the simple CLI on top of the over-the-wire replay: 'pcap_to_db --pcap X' builds .db, '--pcap X --db Y' appends. The tool now sets up the netns/veth itself (via $SUDO), runs virtual_mid360 streaming the pcap in one ns and a FastLio2 live-SDK recorder in the other, records + time-aligns the two fastlio streams, stops when the pcap drains, and hands the (root-written) db back to the caller via chown. Folds in the old replay_via_virtual_mid360.sh wrapper, which is removed. --- .../lidar/fastlio2/tools/pcap_to_db.py | 306 +++++++++++++----- .../tools/replay_via_virtual_mid360.sh | 90 ------ 2 files changed, 230 insertions(+), 166 deletions(-) delete mode 100755 dimos/hardware/sensors/lidar/fastlio2/tools/replay_via_virtual_mid360.sh diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index 4fd5bca4ac..eaa60036e1 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -12,52 +12,63 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Record live FAST-LIO output into a .db while a virtual sensor replays a pcap. +"""Replay a Livox Mid-360 pcap through FAST-LIO and record its output to a .db. -FastLio2 runs in **live SDK mode** and this tool records its two output streams -into a memory2 SQLite database for ``--duration`` seconds: +The pcap is replayed over the wire by ``virtual_mid360`` — a fake Mid-360 on a +virtual NIC that synthesizes the Livox SDK2 handshake — and FastLio2 connects to +it in live SDK mode, exactly as it would to real hardware (there is no in-process +pcap reader). This tool orchestrates the whole thing behind a simple CLI: -* ``fastlio_odometry`` -- the IESKF pose at the native odom rate (~30 Hz). -* ``fastlio_lidar`` -- the registered (deskewed, odom-frame) point cloud at the - native pointcloud rate (~10 Hz). +* ``pcap_to_db --pcap capture.pcap`` -> writes ``capture.db`` from scratch +* ``pcap_to_db --pcap capture.pcap --db mem2.db`` -> appends into an existing db -There is no in-process pcap reader: the packets come from the live Livox SDK -path, fed by ``virtual_mid360`` — a fake Mid-360 on a virtual NIC that replays a -recorded pcap with a synthesized SDK2 handshake. FastLio2 connects to it exactly -as it would to real hardware and never knows the sensor is synthetic. The netns -setup + sensor are orchestrated by ``tools/replay_via_virtual_mid360.sh``, which -drives this tool as its consumer; that wrapper is the normal entry point. +It records two streams — ``fastlio_odometry`` and ``fastlio_lidar`` — and +time-aligns them onto the db's clock (``--time-offset`` overrides the auto +choice; ``--force`` overwrites pre-existing fastlio streams). Replay runs at +real time and stops automatically when the pcap is drained. -The db is appended in place: the two fastlio streams are time-aligned onto the -db's existing clock (so they line up with whatever else it holds), and an -existing ``fastlio_odometry`` / ``fastlio_lidar`` pair aborts the run unless -``--force`` is given. ``--time-offset`` overrides the auto-derived clock shift. +It stands up two network namespaces joined by a veth (the fake lidar in one, the +FastLio2 consumer in the other), which needs root: set ``$SUDO`` to a +privilege-escalation command that runs ``ip``/``pkill``/``chown`` without a +password prompt (default ``sudo``). Netns/veth names default to +``fl_drv``/``fl_lidar`` + ``veth-fl-*`` (distinct from pointlio's harness so the +two can run at once); override via ``$DRV_NS``/``$LIDAR_NS``/``$VETH_*``. -Usage (normally via the wrapper):: +Build the virtual_mid360 binary once:: - bash tools/replay_via_virtual_mid360.sh [config.yaml] - -Direct (only useful inside the consumer netns, fed by an external sensor):: - - python -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db \ - --db /path/to/memory.db --duration 200 + (cd dimos/hardware/sensors/lidar/livox/virtual_mid360 && nix build .#default) """ from __future__ import annotations import argparse -import math +import json +import os from pathlib import Path import sqlite3 +import subprocess import sys import time from dimos.hardware.sensors.lidar.fastlio2.recorder import FastLio2Recorder -# Below this an absolute timestamp is sensor-boot seconds, not unix wall time. -_SENSOR_CLOCK_MAX = 1e8 -# Poll cadence while recording the live stream. +# Poll cadence while recording. _POLL_SEC = 1.0 +# Stop once the odom stream has been stagnant this long (pcap fully replayed). +_STAGNANT_SEC = 6.0 + +# Privilege-escalation command + network namespace / veth names. The lidar ns +# runs virtual_mid360; the drv ns runs the FastLio2 consumer. Defaults are +# distinct from pointlio's harness so both can run concurrently. +_SUDO = os.environ.get("SUDO", "sudo") +_DRV_NS = os.environ.get("DRV_NS", "fl_drv") +_LIDAR_NS = os.environ.get("LIDAR_NS", "fl_lidar") +_VETH_DRV = os.environ.get("VETH_DRV", "veth-fl-drv") +_VETH_LIDAR = os.environ.get("VETH_LIDAR", "veth-fl-lidar") +_HOST_IP = "192.168.1.5" +_LIDAR_IP = "192.168.1.155" +_REPO = Path(__file__).resolve().parents[6] +_VM_BIN = _REPO / "dimos/hardware/sensors/lidar/livox/virtual_mid360/result/bin/virtual_mid360" def _db_ref_start_ts(db_path: Path) -> float: @@ -75,8 +86,8 @@ def _db_ref_start_ts(db_path: Path) -> float: if table.startswith("_") or table.startswith("sqlite_"): continue try: - # vec0/rtree virtual tables (sqlite-vec etc.) raise "no such - # module" here when the extension isn't loaded -- skip them. + # vec0/rtree virtual tables raise "no such module" when the + # extension isn't loaded -- skip them. cols = [c[1] for c in con.execute(f"PRAGMA table_info('{table}')").fetchall()] if "ts" not in cols: continue @@ -106,18 +117,152 @@ def _table_stats(db_path: Path, table: str) -> tuple[int, float, float]: con.close() -def _run(args: argparse.Namespace) -> int: - # FastLio2 runs in live SDK mode, fed by an external sensor — virtual_mid360 - # replaying a pcap over a veth (see tools/replay_via_virtual_mid360.sh). We - # record whatever the SDK receives into the db for --duration seconds. - if not args.db: - print("[pcap_to_db] --db is required", file=sys.stderr) +# --------------------------------------------------------------------------- +# Orchestrator: set up the netns + fake sensor, drive the consumer, tear down. +# --------------------------------------------------------------------------- + + +def _sudo(*args: str, check: bool = True) -> subprocess.CompletedProcess[bytes]: + return subprocess.run([_SUDO, *args], check=check) + + +def _teardown_netns() -> None: + _sudo("pkill", "-9", "-f", "result/bin/virtual_mid360", check=False) + _sudo("ip", "netns", "del", _DRV_NS, check=False) + _sudo("ip", "netns", "del", _LIDAR_NS, check=False) + _sudo("ip", "link", "del", _VETH_DRV, check=False) + + +def _setup_netns() -> None: + _teardown_netns() + _sudo("ip", "netns", "add", _DRV_NS) + _sudo("ip", "netns", "add", _LIDAR_NS) + _sudo("ip", "link", "add", _VETH_DRV, "type", "veth", "peer", "name", _VETH_LIDAR) + _sudo("ip", "link", "set", _VETH_DRV, "netns", _DRV_NS) + _sudo("ip", "link", "set", _VETH_LIDAR, "netns", _LIDAR_NS) + _sudo("ip", "netns", "exec", _DRV_NS, "ip", "addr", "add", f"{_HOST_IP}/24", "dev", _VETH_DRV) + _sudo( + "ip", "netns", "exec", _LIDAR_NS, "ip", "addr", "add", f"{_LIDAR_IP}/24", "dev", _VETH_LIDAR + ) + for ns in (_DRV_NS, _LIDAR_NS): + _sudo("ip", "netns", "exec", ns, "ip", "link", "set", "lo", "up") + _sudo("ip", "netns", "exec", ns, "ip", "link", "set", "lo", "multicast", "on") + _sudo("ip", "netns", "exec", ns, "ip", "route", "add", "224.0.0.0/4", "dev", "lo") + _sudo("ip", "netns", "exec", _DRV_NS, "ip", "link", "set", _VETH_DRV, "up") + _sudo("ip", "netns", "exec", _LIDAR_NS, "ip", "link", "set", _VETH_LIDAR, "up") + _sudo("ip", "netns", "exec", _DRV_NS, "ip", "link", "set", _VETH_DRV, "multicast", "on") + _sudo("ip", "netns", "exec", _LIDAR_NS, "ip", "link", "set", _VETH_LIDAR, "multicast", "on") + _sudo( + "ip", + "netns", + "exec", + _LIDAR_NS, + "ip", + "route", + "add", + "255.255.255.255/32", + "dev", + _VETH_LIDAR, + ) + # Mid-360 multicasts point/IMU to 224.1.1.5 — egress the virtual NIC. + _sudo( + "ip", "netns", "exec", _LIDAR_NS, "ip", "route", "add", "224.1.1.5/32", "dev", _VETH_LIDAR + ) + + +def _orchestrate(args: argparse.Namespace) -> int: + pcap = Path(args.pcap).expanduser().resolve() + if not pcap.exists(): + print(f"[pcap_to_db] missing pcap: {pcap}", file=sys.stderr) return 2 - if args.duration <= 0: - print("[pcap_to_db] --duration must be > 0", file=sys.stderr) + if not _VM_BIN.exists(): + print( + f"[pcap_to_db] missing virtual_mid360 binary at {_VM_BIN}\n" + f" build it: (cd {_VM_BIN.parents[1]} && nix build .#default)", + file=sys.stderr, + ) return 2 + db = Path(args.db).expanduser().resolve() if args.db else pcap.with_suffix(".db") + db.parent.mkdir(parents=True, exist_ok=True) + print( + f"[pcap_to_db] {pcap.name} -> {db.name} " + f"({'append' if db.exists() else 'new'}) via virtual_mid360 (live SDK)", + flush=True, + ) + + consumer: subprocess.Popen[bytes] | None = None + _setup_netns() + try: + # FastLio2 consumer in the drv netns (re-exec self as the recorder). + cmd = [ + _SUDO, + "ip", + "netns", + "exec", + _DRV_NS, + "env", + f"PYTHONPATH={_REPO}", + sys.executable, + "-m", + "dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db", + "--_consume", + "--db", + str(db), + "--duration", + str(args.duration), + "--odom-freq", + str(args.odom_freq), + ] + if args.config: + cmd += ["--config", args.config] + if args.force: + cmd += ["--force"] + if args.time_offset is not None: + cmd += ["--time-offset", str(args.time_offset)] + consumer = subprocess.Popen(cmd) + time.sleep(5) # let the coordinator boot + open the SDK sockets + + # Fake lidar in the lidar netns, replaying the pcap over the wire. + vm_cfg = json.dumps( + { + "topics": {}, + "config": {"pcap": str(pcap), "rate": 1.0, "delay": 2.0, "lidar_netns": _LIDAR_NS}, + } + ) + vm = subprocess.Popen( + [_SUDO, "ip", "netns", "exec", _LIDAR_NS, str(_VM_BIN)], stdin=subprocess.PIPE + ) + assert vm.stdin is not None + vm.stdin.write((vm_cfg + "\n").encode()) + vm.stdin.close() + + consumer.wait() + rc = consumer.returncode + finally: + if consumer is not None and consumer.poll() is None: + consumer.terminate() + try: + consumer.wait(timeout=10) + except subprocess.TimeoutExpired: + consumer.kill() + _teardown_netns() + + # The consumer ran as root inside the netns, so the db is root-owned — + # hand it back to the invoking user. + for suffix in ("", "-wal", "-shm"): + p = Path(str(db) + suffix) + if p.exists(): + _sudo("chown", f"{os.getuid()}:{os.getgid()}", str(p), check=False) + return rc + + +# --------------------------------------------------------------------------- +# Consumer: FastLio2 live SDK + recorder. Runs inside the drv netns. +# --------------------------------------------------------------------------- + + +def _consume(args: argparse.Namespace) -> int: db_path = Path(args.db).expanduser().resolve() - db_existed = db_path.exists() db_path.parent.mkdir(parents=True, exist_ok=True) from dimos.core.coordination.blueprints import autoconnect @@ -144,27 +289,10 @@ def _run(args: argparse.Namespace) -> int: ref_start_ts = _db_ref_start_ts(db_path) time_offset = float("nan") if args.time_offset is None else args.time_offset - if not math.isnan(time_offset): - offset_desc = f"explicit {time_offset:+.3f}s" - elif ref_start_ts < 0.0: - offset_desc = "auto: db empty -> 0" - elif ref_start_ts < _SENSOR_CLOCK_MAX: - offset_desc = f"auto: db sensor-clock (R0={ref_start_ts:.2f})" - else: - offset_desc = f"auto: db wall-clock (R0={ref_start_ts:.2f})" - print( - f"[pcap_to_db] src=virtual_mid360 (live SDK) db={db_path.name} " - f"({'append' if db_existed else 'new'}) " - f"odom_freq={args.odom_freq}Hz offset={offset_desc}", - flush=True, - ) fastlio_kwargs: dict[str, object] = dict( - frame_id="world", - odom_freq=args.odom_freq, - debug=False, + frame_id="world", odom_freq=args.odom_freq, debug=False ) - # Omit config to fall back to the module default (config/mid360.yaml). if args.config: fastlio_kwargs["config"] = Path(args.config) fastlio = FastLio2.blueprint(**fastlio_kwargs).remappings( @@ -176,27 +304,40 @@ def _run(args: argparse.Namespace) -> int: blueprint = autoconnect( fastlio, FastLio2Recorder.blueprint( - db_path=str(db_path), - ref_start_ts=ref_start_ts, - time_offset=time_offset, + db_path=str(db_path), ref_start_ts=ref_start_ts, time_offset=time_offset ), ).global_config(n_workers=4, robot_model="mid360_fastlio2_pcap_to_db") coord = ModuleCoordinator.build(blueprint) t0 = time.time() + last_max = 0.0 + stagnant_since: float | None = None try: while time.time() - t0 < args.duration: time.sleep(_POLL_SEC) - print(f"[pcap_to_db] reached --duration={args.duration:.1f}s", flush=True) + _cnt, _min_ts, max_ts = _table_stats(db_path, "fastlio_odometry") + if max_ts == 0.0: + continue # no data yet — sensor still warming up + if max_ts == last_max: + # Stream stopped advancing → the pcap has been fully replayed. + if stagnant_since is None: + stagnant_since = time.time() + elif time.time() - stagnant_since > _STAGNANT_SEC: + print("[pcap_to_db] replay drained", flush=True) + break + else: + last_max = max_ts + stagnant_since = None + else: + print(f"[pcap_to_db] reached --duration cap {args.duration:.0f}s", flush=True) finally: coord.stop() o_cnt, o_min, o_max = _table_stats(db_path, "fastlio_odometry") l_cnt = _table_stats(db_path, "fastlio_lidar")[0] - span = o_max - o_min print( f"[pcap_to_db] done odom={o_cnt} lidar={l_cnt} " - f"ts=[{o_min:.3f}, {o_max:.3f}] span={span:.1f}s " + f"ts=[{o_min:.3f}, {o_max:.3f}] span={o_max - o_min:.1f}s " f"wall={time.time() - t0:.1f}s", flush=True, ) @@ -205,17 +346,18 @@ def _run(args: argparse.Namespace) -> int: def main(argv: list[str]) -> int: parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--pcap", default=None, help="Livox Mid-360 pcap capture to replay") parser.add_argument( "--db", - required=True, - help="target memory2 SQLite db. fastlio streams are appended + time-aligned " - "onto its clock (or it's created fresh if absent).", + default=None, + help="target memory2 SQLite db; defaults to .db. Existing fastlio " + "streams are time-aligned onto its clock (use --force to overwrite them).", ) parser.add_argument( - "--duration", - type=float, - required=True, - help="record for this many seconds of wall time, then stop", + "--config", + type=str, + default=None, + help="FAST-LIO yaml (relative to config/ or absolute); omit for the module default", ) parser.add_argument( "--odom-freq", @@ -223,24 +365,36 @@ def main(argv: list[str]) -> int: default=30.0, help="FAST-LIO odometry publish rate in Hz (default 30)", ) - parser.add_argument( - "--config", - type=str, - default=None, - help="FAST-LIO yaml (relative to config/ or absolute); omit for the module default", - ) parser.add_argument( "--time-offset", type=float, default=None, help="seconds added to every output ts; omit to auto-derive from the db clock", ) + parser.add_argument( + "--duration", + type=float, + default=3600.0, + help="safety cap in seconds; replay normally stops when the pcap is drained", + ) parser.add_argument( "--force", action="store_true", help="overwrite existing fastlio_odometry/fastlio_lidar streams in the db", ) - return _run(parser.parse_args(argv)) + # Internal: the in-netns recorder half, spawned by the orchestrator. + parser.add_argument("--_consume", action="store_true", help=argparse.SUPPRESS) + args = parser.parse_args(argv) + + if args._consume: + if not args.db: + print("[pcap_to_db] --_consume requires --db", file=sys.stderr) + return 2 + return _consume(args) + if not args.pcap: + print("[pcap_to_db] --pcap is required", file=sys.stderr) + return 2 + return _orchestrate(args) if __name__ == "__main__": diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/replay_via_virtual_mid360.sh b/dimos/hardware/sensors/lidar/fastlio2/tools/replay_via_virtual_mid360.sh deleted file mode 100755 index 056f96d6c8..0000000000 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/replay_via_virtual_mid360.sh +++ /dev/null @@ -1,90 +0,0 @@ -#!/usr/bin/env bash -# Replay a Livox Mid-360 pcap through FAST-LIO over the wire, recording odometry -# + lidar into a memory2 db. -# -# virtual_mid360 stands up a fake Mid-360 on a virtual NIC and replays the pcap -# with a synthesized SDK2 handshake; FastLio2 connects to it as if to real -# hardware (live SDK mode) and never knows the sensor is synthetic. This is the -# only replay path — the fastlio binary has no in-process pcap reader. Use it to -# reproduce divergence / non-divergence exactly as the robot would see it. -# -# Two network namespaces joined by a veth: the lidar ns runs virtual_mid360, the -# drv ns runs `pcap_to_db` (FastLio2 live + FastLio2Recorder). Needs root for the -# netns/veth setup — set $SUDO to your privilege-escalation command (default -# `sudo`; it must run `ip`/`pkill` without a password prompt). -# -# The netns + veth NAMES are distinct from pointlio's harness (drv/lidar + -# veth-drv/veth-lidar) so the two can run concurrently. Override via env -# (DRV_NS/LIDAR_NS/VETH_DRV/VETH_LIDAR). IPs live inside each netns, so the -# .1.x addresses don't conflict with pointlio's even though they're the same. -# -# Usage: -# source /bin/activate # provide a python with dimos installed -# replay_via_virtual_mid360.sh [duration_sec] [fastlio_config.yaml] -# -set -u -PCAP="${1:?usage: replay_via_virtual_mid360.sh [duration] [config.yaml]}" -DB="${2:?missing }" -DUR="${3:-200}" -CONFIG="${4:-}" - -SUDO="${SUDO:-sudo}" -HOST_IP=192.168.1.5 -LIDAR_IP=192.168.1.155 -DRV_NS="${DRV_NS:-fl_drv}" -LIDAR_NS="${LIDAR_NS:-fl_lidar}" -VETH_DRV="${VETH_DRV:-veth-fl-drv}" -VETH_LIDAR="${VETH_LIDAR:-veth-fl-lidar}" -REPO="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../../../../.." && pwd)" -VM="$REPO/dimos/hardware/sensors/lidar/livox/virtual_mid360/result/bin/virtual_mid360" -PYTHON="${PYTHON:-$(command -v python)}" -VM_LOG="${VM_LOG:-/tmp/vmid360_vm.log}" -FL_LOG="${FL_LOG:-/tmp/vmid360_fastlio.log}" - -[ -x "$VM" ] || { echo "missing virtual_mid360 binary at $VM — build it: (cd $(dirname "$VM")/.. && nix build .#default)"; exit 2; } -[ -f "$PCAP" ] || { echo "missing pcap: $PCAP"; exit 2; } -[ -n "$PYTHON" ] || { echo "no python on PATH — activate the dimos venv first"; exit 2; } - -cleanup() { - # Match the binary path, NOT a bare "virtual_mid360" — this script's own name - # contains that string, so a loose pattern would SIGKILL the wrapper itself. - $SUDO pkill -9 -f "result/bin/virtual_mid360" 2>/dev/null - $SUDO ip netns del "$DRV_NS" 2>/dev/null - $SUDO ip netns del "$LIDAR_NS" 2>/dev/null - $SUDO ip link del "$VETH_DRV" 2>/dev/null -} -cleanup -$SUDO ip netns add "$DRV_NS"; $SUDO ip netns add "$LIDAR_NS" -$SUDO ip link add "$VETH_DRV" type veth peer name "$VETH_LIDAR" -$SUDO ip link set "$VETH_DRV" netns "$DRV_NS"; $SUDO ip link set "$VETH_LIDAR" netns "$LIDAR_NS" -$SUDO ip netns exec "$DRV_NS" ip addr add "$HOST_IP/24" dev "$VETH_DRV" -$SUDO ip netns exec "$LIDAR_NS" ip addr add "$LIDAR_IP/24" dev "$VETH_LIDAR" -for NS in "$DRV_NS" "$LIDAR_NS"; do - $SUDO ip netns exec "$NS" ip link set lo up - $SUDO ip netns exec "$NS" ip link set lo multicast on - $SUDO ip netns exec "$NS" ip route add 224.0.0.0/4 dev lo -done -$SUDO ip netns exec "$DRV_NS" ip link set "$VETH_DRV" up; $SUDO ip netns exec "$LIDAR_NS" ip link set "$VETH_LIDAR" up -$SUDO ip netns exec "$DRV_NS" ip link set "$VETH_DRV" multicast on; $SUDO ip netns exec "$LIDAR_NS" ip link set "$VETH_LIDAR" multicast on -$SUDO ip netns exec "$LIDAR_NS" ip route add 255.255.255.255/32 dev "$VETH_LIDAR" -# Mid-360 multicasts point/IMU to 224.1.1.5 — egress the virtual NIC. -$SUDO ip netns exec "$LIDAR_NS" ip route add 224.1.1.5/32 dev "$VETH_LIDAR" - -# Consumer: FastLio2 (live SDK) + FastLio2Recorder, recording into the db. -CFG_ARG=(); [ -n "$CONFIG" ] && CFG_ARG=(--config "$CONFIG") -$SUDO ip netns exec "$DRV_NS" env "PYTHONPATH=$REPO" "$PYTHON" \ - -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db \ - --db "$DB" --duration "$DUR" --force "${CFG_ARG[@]}" > "$FL_LOG" 2>&1 & -CONSUMER=$! -sleep 5 # let the coordinator boot + open the SDK sockets - -# Fake lidar: replay the pcap over the wire (delay lets the consumer settle). -echo "{\"topics\":{},\"config\":{\"pcap\":\"$PCAP\",\"rate\":1.0,\"delay\":2.0,\"lidar_netns\":\"$LIDAR_NS\"}}" \ - | $SUDO ip netns exec "$LIDAR_NS" "$VM" > "$VM_LOG" 2>&1 & - -wait "$CONSUMER" -RC=$? -echo "=== handshake marker (vm log) ==="; grep -i "arming data stream\|0x0100" "$VM_LOG" | tail -1 -cleanup -echo "DONE rc=$RC db=$DB" -exit "$RC" From b26630114fcdd922830a0b987a368e68929b934f Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 21:36:47 -0700 Subject: [PATCH 073/185] chore(pointlio): trim verbose comments; fix two stale ones Cut redundant section-banner/restatement comments and condense over-long blocks across main.rs/main.cpp/pcap_to_db.py (keeping the protocol/EKF/why notes). Also fixes two stale comments left by earlier removals: the main.rs header no longer references the deleted in-process pcap_replay, and the odometry comment no longer mentions the removed mount offset. Comments only. --- .../lidar/livox/virtual_mid360/src/main.rs | 29 +++++++------------ .../sensors/lidar/pointlio/cpp/main.cpp | 22 ++------------ .../lidar/pointlio/tools/pcap_to_db.py | 9 ++---- 3 files changed, 15 insertions(+), 45 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs index 03109267bd..3d77897a4d 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs @@ -1,14 +1,8 @@ // Fake Livox Mid-360 — replays a recorded pcap over a virtual NIC and synthesizes // the Livox SDK2 control handshake so an unmodified, live-mode pointlio ingests it -// through the real Livox SDK as if from a live sensor. -// -// Inverse of pointlio's in-process `cpp/pcap_replay.hpp` (--replay_pcap), which -// bypasses the network. This exercises the full live stack: SDK discovery + -// control handshake, then point/IMU UDP off a (virtual) wire. -// -// Runs inside the "lidar" network namespace (see setup_commands()); the unmodified -// pointlio runs in the peer "drv" namespace. On any failure the error names the -// exact command to run, then asks the user to re-run the module. +// through the real Livox SDK as if from a live sensor. Runs in the "lidar" netns +// (peer "drv" runs pointlio); on a setup failure the error prints the exact +// netns/veth commands to run. use dimos_module::{run, LcmTransport, Module}; use serde::Deserialize; @@ -203,8 +197,8 @@ fn ensure_interface(cfg: &Config) -> Result { .lidar_ip .parse() .map_err(|_| format!("invalid lidar_ip '{}'", cfg.lidar_ip))?; - // Probe: can we bind the lidar control port on lidar_ip? If not, the veth/netns - // isn't set up (or we're in the wrong namespace). + // If we can't bind the control port on lidar_ip, the veth/netns isn't set up + // (or we're in the wrong namespace). let probe = UdpSocket::bind(SocketAddrV4::new(lidar_ip, CMD_PORT)); if probe.is_err() { let ns = &cfg.lidar_netns; @@ -244,8 +238,7 @@ impl VirtualMid360 { let lidar_ip = match ensure_interface(cfg) { Ok(ip) => ip, Err(msg) => { - // Actionable error: print the fix command, then exit non-zero so the - // coordinator surfaces it and the user can re-run after setup. + // Exit non-zero so the coordinator surfaces the fix command. tracing::error!("{msg}"); eprintln!("\n[virtual_mid360] {msg}\n"); std::process::exit(2); @@ -288,13 +281,13 @@ impl VirtualMid360 { let rate = cfg.rate; let delay = cfg.delay; - // Role 1: discovery responder (:56000 broadcast) — synthesize the 0x0000 ACK. + // discovery responder (:56000 broadcast) — synthesize the 0x0000 ACK spawn_discovery(lidar_ip, stop.clone()); - // Role 2: control responder (:56100) — synthesize per-cmd ACKs; arm streaming - // when the host issues the work-mode/config command (0x0100). + // control responder (:56100) — synthesize per-cmd ACKs; arm streaming + // when the host issues the work-mode/config command (0x0100) spawn_control(lidar_ip, armed.clone(), stop.clone()); - // Role 3: data streamer — point/IMU/status, paced at `rate`, timestamps rewritten - // to now, armed by the handshake (with `delay` as a startup floor / fallback). + // data streamer — point/IMU/status, paced at `rate`, timestamps rewritten + // to now, armed by the handshake (`delay` as a startup floor / fallback) spawn_stream( lidar_ip, host_ip, mcast_data, packets, rate, delay, armed, stop, ); diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 2b7676b467..a56a7ceae7 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -90,10 +90,7 @@ static uint64_t get_timestamp_ns(const LivoxLidarEthernetPacket* pkt) { using dimos::time_from_seconds; using dimos::make_header; -// --------------------------------------------------------------------------- -// Publish lidar point cloud in the sensor body frame (g_frame_id / mid360_link) -// --------------------------------------------------------------------------- -// +// Publish the lidar point cloud in the sensor body frame (g_frame_id). // `cloud` is FAST-LIO's undistorted scan in the sensor's own frame // (get_body_cloud), so points are published as-is with no world registration. static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, @@ -145,10 +142,6 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, g_lcm->publish(chan, &pc); } -// --------------------------------------------------------------------------- -// Publish odometry -// --------------------------------------------------------------------------- - static void publish_odometry(const custom_messages::Odometry& odom, double timestamp) { if (!g_lcm) return; @@ -156,7 +149,7 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times msg.header = make_header(g_frame_id, timestamp); msg.child_frame_id = g_child_frame_id; - // Pose in the SLAM/sensor frame (no mount offset applied). + // Pose in the SLAM/sensor frame. msg.pose.pose.position.x = odom.pose.pose.position.x; msg.pose.pose.position.y = odom.pose.pose.position.y; msg.pose.pose.position.z = odom.pose.pose.position.z; @@ -329,18 +322,10 @@ static void on_info_change(const uint32_t handle, const LivoxLidarInfo* info, EnableLivoxLidarImuData(handle, nullptr, nullptr); } -// --------------------------------------------------------------------------- -// Signal handling -// --------------------------------------------------------------------------- - static void signal_handler(int /*sig*/) { g_running.store(false); } -// --------------------------------------------------------------------------- -// Main -// --------------------------------------------------------------------------- - int main(int argc, char** argv) { dimos::NativeModule mod(argc, argv); @@ -410,11 +395,9 @@ int main(int argc, char** argv) { filter_cfg.voxel_size, filter_cfg.sor_mean_k, filter_cfg.sor_stddev); } - // Signal handlers signal(SIGTERM, signal_handler); signal(SIGINT, signal_handler); - // Init LCM lcm::LCM lcm; if (!lcm.good()) { fprintf(stderr, "Error: LCM init failed\n"); @@ -571,7 +554,6 @@ int main(int argc, char** argv) { } } - // Cleanup if (debug) printf("[fastlio2] Shutting down...\n"); g_fastlio = nullptr; LivoxLidarSdkUninit(); diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py index 286f592110..44cd7d7f97 100644 --- a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py @@ -199,13 +199,10 @@ def _table_stats(db_path: Path, table: str) -> tuple[int, float, float]: con.close() -# --------------------------------------------------------------------------- -# Network-namespace orchestration (outer process; needs CAP_NET_ADMIN via sudo) -# --------------------------------------------------------------------------- +# Network-namespace orchestration (outer process; needs CAP_NET_ADMIN via sudo). def _sudo() -> list[str]: - """Privilege-escalation prefix for the netns/veth commands.""" return ["sudo"] @@ -418,9 +415,7 @@ def _run_outer(args: argparse.Namespace) -> int: return 0 -# --------------------------------------------------------------------------- -# Inner process: live Point-LIO + recorder, already inside the drv netns -# --------------------------------------------------------------------------- +# Inner process: live Point-LIO + recorder, already inside the drv netns. def _run_inner(args: argparse.Namespace) -> int: From d7ffc6ff302a1a5471a406a6a1533ade97c5b287 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 21:38:54 -0700 Subject: [PATCH 074/185] feat(virtual_mid360): read demo blueprint pcap from VIRTUAL_MID360_PCAP env var The demo blueprint's pcap now comes from the VIRTUAL_MID360_PCAP env var (default empty), so it can be pointed at a capture without editing the file and without an import-time LFS pull. --- .../lidar/livox/virtual_mid360/blueprints.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py index 93f7f2ac14..da4d494ab0 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py @@ -25,18 +25,23 @@ PointLio in a `drv` netns joined by a veth carrying lidar_ip). """ +import os + from dimos.core.coordination.blueprints import autoconnect from dimos.hardware.sensors.lidar.livox.virtual_mid360.module import VirtualMid360 from dimos.hardware.sensors.lidar.pointlio.module import PointLio from dimos.visualization.vis_module import vis_module -# Set pcap to a recorded Mid-360 capture before running, e.g. the ruwik2_part3 -# LFS sample: --VirtualMid360.pcap "$(python -c 'from dimos.utils.data import -# get_data; print(get_data("ruwik2_part3/ruwik2_part3.pcap"))')" -# (Resolved at run time, not import time, so registering this blueprint never -# triggers an LFS pull.) +# Point this at a recorded Mid-360 capture via the env var, e.g. the ruwik2_part3 +# LFS sample: +# VIRTUAL_MID360_PCAP="$(python -c 'from dimos.utils.data import get_data; \ +# print(get_data("ruwik2_part3/ruwik2_part3.pcap"))')" dimos run ... +# Read here (not get_data at import) so registering the blueprint never triggers +# an LFS pull. +_PCAP = os.environ.get("VIRTUAL_MID360_PCAP", "") + demo_virtual_mid360_pointlio = autoconnect( - VirtualMid360.blueprint(pcap=""), + VirtualMid360.blueprint(pcap=_PCAP), PointLio.blueprint(), vis_module("rerun"), ).global_config(n_workers=3, robot_model="virtual_mid360_pointlio") From 0f48de52b6cabd51aad44639351330ac0095f644 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 21:44:55 -0700 Subject: [PATCH 075/185] refactor(virtual_mid360): require lidar_ip/host_ip/lidar_netns (not Livox defaults) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit These three were defaulted to 192.168.1.155/192.168.1.5/'lidar' — but those are network/machine/deployment-specific, not Livox-universal, so a silent default could be silently wrong. Make them required (no default) in both the rust Config and the Python wrapper. mcast_data keeps its default (224.1.1.5 IS the Livox Mid-360 default multicast_ip). The demo blueprint now supplies the netns-harness values explicitly; pcap_to_db already passes all three. --- .../lidar/livox/virtual_mid360/blueprints.py | 9 ++++++++- .../lidar/livox/virtual_mid360/module.py | 19 +++++++++++-------- .../lidar/livox/virtual_mid360/src/main.rs | 19 ++++++------------- 3 files changed, 25 insertions(+), 22 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py index da4d494ab0..f93cf7036f 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py @@ -40,8 +40,15 @@ # an LFS pull. _PCAP = os.environ.get("VIRTUAL_MID360_PCAP", "") +# lidar_ip/host_ip/lidar_netns are deployment-specific (required, no defaults); +# these are the values the e2e netns harness assigns (drv/lidar veth on .1.x). demo_virtual_mid360_pointlio = autoconnect( - VirtualMid360.blueprint(pcap=_PCAP), + VirtualMid360.blueprint( + pcap=_PCAP, + lidar_ip="192.168.1.155", + host_ip="192.168.1.5", + lidar_netns="lidar", + ), PointLio.blueprint(), vis_module("rerun"), ).global_config(n_workers=3, robot_model="virtual_mid360_pointlio") diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py index 980aad3a39..5ccbda0374 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py @@ -51,15 +51,18 @@ class VirtualMid360Config(NativeModuleConfig): rate: float = 1.0 # Seconds to wait after start before streaming begins. delay: float = 0.0 - # IP the fake lidar sends from (must be on this netns's veth). - lidar_ip: str = "192.168.1.155" - # Host IP the recorded data is delivered to (where the SDK listens). - host_ip: str = "192.168.1.5" - # Network namespace the fake lidar runs inside. - lidar_netns: str = "lidar" + # IP the fake lidar sends from (must be on this netns's veth). Network- + # specific, so required (no default). + lidar_ip: str + # Host IP the recorded data is delivered to (where the SDK listens). Machine- + # specific, so required (no default). + host_ip: str + # Network namespace the fake lidar runs inside. Deployment-specific, so + # required (no default). + lidar_netns: str # Multicast group the point/IMU streams are sent to. 224.1.1.5 is the Livox - # default that the SDK joins; override only to match a consumer configured - # with a different multicast_ip. + # default the SDK joins (a genuine Livox default), so it stays defaulted; + # override only to match a consumer with a different multicast_ip. mcast_data: str = "224.1.1.5" diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs index 3d77897a4d..0e7ed80505 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs @@ -41,13 +41,13 @@ struct Config { #[validate(range(min = 0.0, max = 3600.0))] delay: f64, /// IP the fake lidar sends from (must be assigned to this netns's veth). - #[serde(default = "default_lidar_ip")] + /// Network-specific — required, no default. lidar_ip: String, /// Host IP the recorded data is delivered to (where pointlio's SDK listens). - #[serde(default = "default_host_ip")] + /// Machine-specific — required, no default. host_ip: String, - /// Network namespace the fake lidar must run inside. - #[serde(default = "default_netns")] + /// Network namespace the fake lidar runs inside (named in the setup-help + /// error). Deployment-specific — required, no default. lidar_netns: String, /// Multicast group the point/IMU streams are sent to. A real Mid-360 /// multicasts these and the Livox SDK joins whatever `multicast_ip` is in @@ -61,15 +61,8 @@ struct Config { fn one() -> f64 { 1.0 } -fn default_lidar_ip() -> String { - "192.168.1.155".into() -} -fn default_host_ip() -> String { - "192.168.1.5".into() -} -fn default_netns() -> String { - "lidar".into() -} +// 224.1.1.5 is the Livox Mid-360 default multicast_ip (a genuine Livox default, +// unlike the lidar/host IP + netns, which are deployment-specific and required). fn default_mcast_data() -> String { "224.1.1.5".into() } From 5f27cfd05184d15a1e6dc28a1551a3e7e0c70a2b Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 21:54:10 -0700 Subject: [PATCH 076/185] feat(virtual_mid360): tcpdump pcap recorder for raw Livox Mid-360 UDP MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Mid360PcapRecorder — the capture counterpart to VirtualMid360's replay. A dimos Module that runs tcpdump to record only the Mid-360's UDP (point/IMU/ status, 'src host and udp') into a pcap, so a live session can be replayed bit-for-bit later. Modeled on go2_record_clean's FastLio2Recorder pcap path (parent-death reaping, clean SIGINT flush, empty-pcap watchdog + diagnostics) but pcap only — no stream/db recording. lidar_ip required; iface from DIMOS_PCAP_IFACE. Needs CAP_NET_RAW (setcap on tcpdump). --- .../lidar/livox/virtual_mid360/recorder.py | 313 ++++++++++++++++++ dimos/robot/all_blueprints.py | 1 + 2 files changed, 314 insertions(+) create mode 100644 dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py new file mode 100644 index 0000000000..b2cabca03e --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py @@ -0,0 +1,313 @@ +# 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. + +"""tcpdump-based recorder for raw Livox Mid-360 UDP — the input for VirtualMid360. + +Captures only the Mid-360's UDP traffic (point/IMU/status) off the wire into a +pcap, so it can be replayed later with the VirtualMid360 module. It records +nothing else — no dimos streams, no db, just the raw Livox packets. Needs +CAP_NET_RAW (grant tcpdump once: `sudo setcap cap_net_raw,cap_net_admin=eip +$(which tcpdump)`). +""" + +from __future__ import annotations + +import asyncio +from datetime import datetime +import os +from pathlib import Path +import re +import shlex +import shutil +import signal +import subprocess +import textwrap +import time + +from pydantic import Field + +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +def _stamp() -> str: + now = datetime.now() + return now.strftime("%Y-%m-%d_%I-%M%p").lower() + + +def _default_pcap_path() -> Path: + return Path("recordings") / f"mid360_{_stamp()}.pcap" + + +def _stop_when_parent_dies(cmd: list[str], grace_sec: float) -> list[str]: + """Wrap cmd so it's reaped if this recorder dies — including via SIGKILL, + which it can't intercept — otherwise tcpdump would outlive its session.""" + parent_pid = os.getpid() + quoted = " ".join(shlex.quote(arg) for arg in cmd) + # Foreground waits on tcpdump so a startup failure propagates its exit code. + script = textwrap.dedent(f""" + {quoted} & + child=$! + ( + while kill -0 {parent_pid} 2>/dev/null; do + sleep 0.5 + done + kill -INT "$child" 2>/dev/null + sleep {grace_sec} + kill -KILL "$child" 2>/dev/null + ) & + watcher=$! + wait "$child" + code=$? + kill "$watcher" 2>/dev/null + exit $code + """).strip() + return ["bash", "-c", script] + + +class Mid360PcapRecorderConfig(ModuleConfig): + """Where/how to capture; the pcap defaults to recordings/mid360_.pcap.""" + + pcap_path: Path = Field(default_factory=_default_pcap_path) + # Capture interface. Machine-specific, so it defaults from DIMOS_PCAP_IFACE + # (falling back to enp2s0) rather than hardcoding a host-only value. + iface: str = Field(default_factory=lambda: os.environ.get("DIMOS_PCAP_IFACE", "enp2s0")) + # The Mid-360's IP — network-specific, so required (no default). Only its + # UDP is captured (`src host and udp`). + lidar_ip: str + snaplen: int = 2048 + # Grace period per stop signal (SIGINT -> SIGTERM -> SIGKILL) at teardown. + stop_timeout: float = 5.0 + + +class Mid360PcapRecorder(Module): + """Records the raw Livox Mid-360 UDP stream to a pcap via tcpdump. + + Owns nothing but the tcpdump process: on start() it captures every UDP + packet from the lidar into pcap_path; on stop() it flushes + tears it down. + The result replays bit-for-bit through VirtualMid360. + """ + + config: Mid360PcapRecorderConfig + + # tcpdump fails fast (EPERM, bad iface) within a few ms; pause so poll() catches it. + _TCPDUMP_STARTUP_PROBE_SEC: float = 0.3 + # Declare the capture dead if nothing landed after this long. + _PCAP_WATCHDOG_SEC: float = 5.0 + # A libpcap file with zero packets is just its 24-byte global header. + _PCAP_GLOBAL_HEADER_BYTES: int = 24 + # How long the diagnostic sniff listens for *any* UDP source on the iface. + _PCAP_DIAGNOSTIC_SNIFF_SEC: float = 3.0 + + _pcap_proc: subprocess.Popen[bytes] | None = None + + @rpc + def start(self) -> None: + self._start_pcap() + super().start() + if self._pcap_proc is not None: + self.spawn(self._pcap_watchdog()) + + @rpc + def stop(self) -> None: + try: + super().stop() + finally: + self._stop_pcap() + + def _filter(self) -> str: + return f"src host {self.config.lidar_ip} and udp" + + def _start_pcap(self) -> None: + cfg = self.config + path = Path(cfg.pcap_path).expanduser() + path.parent.mkdir(parents=True, exist_ok=True) + + tcpdump = shutil.which("tcpdump") or "tcpdump" + cmd = [ + tcpdump, + "-i", + cfg.iface, + "-w", + str(path), + "-s", + str(cfg.snaplen), + "-U", # packet-buffered: flush each packet so a kill loses nothing + "-n", + self._filter(), + ] + # Own session so _stop_pcap can signal the wrapper + tcpdump without + # touching the recorder, and Ctrl-C doesn't race shutdown. + proc = subprocess.Popen( + _stop_when_parent_dies(cmd, cfg.stop_timeout), + stdout=subprocess.DEVNULL, + stderr=subprocess.PIPE, + start_new_session=True, + ) + time.sleep(self._TCPDUMP_STARTUP_PROBE_SEC) + if proc.poll() is not None: + stderr = proc.stderr.read().decode(errors="replace") if proc.stderr else "" + self._pcap_proc = None + logger.error( + f"Mid360PcapRecorder: tcpdump exited rc={proc.returncode} stderr={stderr.strip()}" + ) + print( + "[mid360_record] tcpdump cannot capture. Grant capture capability once with:\n" + f" sudo setcap cap_net_raw,cap_net_admin=eip {tcpdump}\n" + " then restart. (tcpdump stderr above.)", + flush=True, + ) + return + + logger.info( + f"Mid360PcapRecorder capturing path={path} iface={cfg.iface} " + f"filter={self._filter()!r}" + ) + self._pcap_proc = proc + + async def _pcap_watchdog(self) -> None: + """If tcpdump captured nothing after a few seconds, report why — almost + always a wrong lidar_ip or interface.""" + await asyncio.sleep(self._PCAP_WATCHDOG_SEC) + proc = self._pcap_proc + if proc is None: + return + path = Path(self.config.pcap_path).expanduser() + try: + size = path.stat().st_size + except OSError: + size = -1 + if size > self._PCAP_GLOBAL_HEADER_BYTES: + logger.info( + f"Mid360PcapRecorder healthy — {size} bytes captured in " + f"{self._PCAP_WATCHDOG_SEC:.0f}s path={path}" + ) + return + report = await asyncio.to_thread(self._build_empty_pcap_report, size, proc) + logger.error(report) + print(report, flush=True) + + def _build_empty_pcap_report(self, size: int, proc: subprocess.Popen[bytes]) -> str: + cfg = self.config + proc_alive = proc.poll() is None + stderr_text = "" + if not proc_alive and proc.stderr is not None: + try: + stderr_text = proc.stderr.read().decode(errors="replace").strip() + except (OSError, ValueError): + stderr_text = "" + + observed = self._observed_udp_sources() + if observed: + listing = "\n".join( + f" {source} ({count} pkts)" + for source, count in sorted(observed.items(), key=lambda kv: kv[1], reverse=True) + ) + diagnosis = ( + f" UDP traffic IS flowing on {cfg.iface}, but from other source(s):\n" + f"{listing}\n" + f" None matched 'src host {cfg.lidar_ip}'. The lidar_ip is almost\n" + f" certainly wrong — set it to whichever address above is the lidar." + ) + else: + diagnosis = ( + f" NO UDP traffic at all was seen on {cfg.iface} during a " + f"{self._PCAP_DIAGNOSTIC_SNIFF_SEC:.0f}s probe.\n" + f" Wrong interface, unplugged cable, or the lidar is powered off." + ) + + neigh = self._run_quiet(["ip", "neigh", "show", cfg.lidar_ip]).strip() + return textwrap.dedent(f""" + ============================================================================ + [mid360_record] PCAP WATCHDOG: 0 packets captured after {self._PCAP_WATCHDOG_SEC:.0f}s + ============================================================================ + tcpdump wrote an EMPTY pcap (size={size} bytes; an empty libpcap file is + {self._PCAP_GLOBAL_HEADER_BYTES} bytes of global header). + + Capture config: + interface : {cfg.iface} + lidar_ip : {cfg.lidar_ip} + filter : {self._filter()!r} + pcap_path : {cfg.pcap_path} + tcpdump : alive={proc_alive} pid={proc.pid}{f" stderr={stderr_text!r}" if stderr_text else ""} + + Diagnosis: + {diagnosis} + + arp/neigh for {cfg.lidar_ip}: {neigh or ""} + ============================================================================ + """).strip() + + def _observed_udp_sources(self) -> dict[str, int]: + """Sniff the interface briefly and tally which source IPs send UDP.""" + tcpdump = shutil.which("tcpdump") or "tcpdump" + cmd = [tcpdump, "-i", self.config.iface, "-nn", "-c", "60", "udp"] + try: + result = subprocess.run( + cmd, capture_output=True, text=True, timeout=self._PCAP_DIAGNOSTIC_SNIFF_SEC + ) + output = result.stdout + except subprocess.TimeoutExpired as expired: + stdout = expired.stdout + output = ( + stdout.decode(errors="replace") if isinstance(stdout, bytes) else (stdout or "") + ) + except OSError: + return {} + counts: dict[str, int] = {} + for line in output.splitlines(): + match = re.search(r"\bIP6?\s+(\S+?)\.\d+\s+>", line) + if match: + counts[match.group(1)] = counts.get(match.group(1), 0) + 1 + return counts + + @staticmethod + def _run_quiet(cmd: list[str]) -> str: + try: + return subprocess.run(cmd, capture_output=True, text=True, timeout=2.0).stdout + except (OSError, subprocess.TimeoutExpired): + return "" + + def _stop_pcap(self) -> None: + proc = self._pcap_proc + if proc is None: + return + self._pcap_proc = None + if proc.poll() is not None: + return + # Signal the group so tcpdump gets it directly. SIGINT is its clean-stop + # signal (flushes the pcap); escalate if it hangs. + try: + pgid = os.getpgid(proc.pid) + except ProcessLookupError: + return + timeout = self.config.stop_timeout + for sig in (signal.SIGINT, signal.SIGTERM, signal.SIGKILL): + try: + os.killpg(pgid, sig) + except ProcessLookupError: + break + try: + proc.wait(timeout=timeout) + break + except subprocess.TimeoutExpired: + logger.warning( + f"tcpdump did not exit on {sig.name}; escalating path={self.config.pcap_path}" + ) + else: + proc.wait() + logger.info(f"Mid360PcapRecorder stopped path={self.config.pcap_path}") diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 963f5ecb22..7effefb997 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -188,6 +188,7 @@ "mcp-client": "dimos.agents.mcp.mcp_client.McpClient", "mcp-server": "dimos.agents.mcp.mcp_server.McpServer", "memory-module": "dimos.memory2.module.MemoryModule", + "mid360-pcap-recorder": "dimos.hardware.sensors.lidar.livox.virtual_mid360.recorder.Mid360PcapRecorder", "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", From 3d077c4cf432585983897f5849efc5e32e5f4291 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 22:13:33 -0700 Subject: [PATCH 077/185] fix(virtual_mid360): kill tcpdump via unconfined label when AppArmor blocks it MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit tcpdump's AppArmor profile rejects signals from a confined (e.g. vscode- labeled) sender with EPERM, so the recorder's plain kills silently failed and the capture was orphaned. Both kill paths — the bash death-reaper and _stop_pcap's killpg — now fall back to 'sudo -n aa-exec -p unconfined -- kill' on failure (the same escape the kd command uses); a no-op where AppArmor isn't in the way. Verified the reaper kills the child when the parent dies. --- .../lidar/livox/virtual_mid360/recorder.py | 49 ++++++++++++++++--- 1 file changed, 43 insertions(+), 6 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py index b2cabca03e..c4aabf5320 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py @@ -55,9 +55,21 @@ def _default_pcap_path() -> Path: def _stop_when_parent_dies(cmd: list[str], grace_sec: float) -> list[str]: """Wrap cmd so it's reaped if this recorder dies — including via SIGKILL, - which it can't intercept — otherwise tcpdump would outlive its session.""" + which it can't intercept — otherwise tcpdump would outlive its session. + + The kills fall back to an unconfined AppArmor label: tcpdump's profile + rejects signals from a confined (e.g. vscode-labeled) sender with EPERM, so + a plain kill silently fails there — `sudo -n aa-exec -p unconfined` re-issues + it from a label tcpdump accepts (a no-op where AppArmor isn't in the way).""" parent_pid = os.getpid() quoted = " ".join(shlex.quote(arg) for arg in cmd) + + def _kill(sig: str) -> str: + return ( + f'kill -{sig} "$child" 2>/dev/null' + f' || sudo -n aa-exec -p unconfined -- kill -{sig} "$child" 2>/dev/null' + ) + # Foreground waits on tcpdump so a startup failure propagates its exit code. script = textwrap.dedent(f""" {quoted} & @@ -66,9 +78,9 @@ def _stop_when_parent_dies(cmd: list[str], grace_sec: float) -> list[str]: while kill -0 {parent_pid} 2>/dev/null; do sleep 0.5 done - kill -INT "$child" 2>/dev/null + {_kill("INT")} sleep {grace_sec} - kill -KILL "$child" 2>/dev/null + {_kill("KILL")} ) & watcher=$! wait "$child" @@ -297,9 +309,7 @@ def _stop_pcap(self) -> None: return timeout = self.config.stop_timeout for sig in (signal.SIGINT, signal.SIGTERM, signal.SIGKILL): - try: - os.killpg(pgid, sig) - except ProcessLookupError: + if not self._signal_group(pgid, sig): break try: proc.wait(timeout=timeout) @@ -311,3 +321,30 @@ def _stop_pcap(self) -> None: else: proc.wait() logger.info(f"Mid360PcapRecorder stopped path={self.config.pcap_path}") + + def _signal_group(self, pgid: int, sig: signal.Signals) -> bool: + """Signal the tcpdump process group; False if it's already gone. + + tcpdump's AppArmor profile rejects signals from a confined (e.g. + vscode-labeled) sender with EPERM, so a plain killpg silently fails + there — fall back to re-issuing from an unconfined label, the same + escape the `kd` command uses. No-op where AppArmor isn't in the way.""" + try: + os.killpg(pgid, sig) + return True + except ProcessLookupError: + return False + except PermissionError: + pass + # kill - -- - (negative pid = the whole group) + aa = shutil.which("aa-exec") + if aa is None: + return True + cmd = [aa, "-p", "unconfined", "--", "kill", f"-{int(sig)}", "--", f"-{pgid}"] + if os.geteuid() != 0 and shutil.which("sudo"): + cmd = ["sudo", "-n", *cmd] + try: + subprocess.run(cmd, capture_output=True, timeout=3.0) + except (OSError, subprocess.TimeoutExpired): + pass + return True From d987f87983c25fea36dc5dd181c03ad64e115153 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 22:22:19 -0700 Subject: [PATCH 078/185] mid360 recorder: scream loudly + narrow sudoers hint when tcpdump kill is blocked When AppArmor blocks tcpdump's signal and 'sudo -n aa-exec' cannot escalate, the capture orphans. Detect the surviving tcpdump after stop and print a loud banner with the exact manual kill and a narrow /etc/sudoers.d rule that makes ONLY the unconfined kill passwordless (not all sudo). Bash reaper echoes the same on failure. --- .../lidar/livox/virtual_mid360/recorder.py | 60 ++++++++++++++++++- 1 file changed, 58 insertions(+), 2 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py index c4aabf5320..ef427811e2 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py @@ -25,6 +25,7 @@ import asyncio from datetime import datetime +import getpass import os from pathlib import Path import re @@ -68,6 +69,9 @@ def _kill(sig: str) -> str: return ( f'kill -{sig} "$child" 2>/dev/null' f' || sudo -n aa-exec -p unconfined -- kill -{sig} "$child" 2>/dev/null' + f' || echo "[mid360_record] FAILED to {sig} tcpdump pid $child' + f" (AppArmor blocked it + sudo -n could not escalate) — it is ORPHANED." + f' Kill it: sudo aa-exec -p unconfined -- kill -9 $child" >&2' ) # Foreground waits on tcpdump so a startup failure propagates its exit code. @@ -318,9 +322,13 @@ def _stop_pcap(self) -> None: logger.warning( f"tcpdump did not exit on {sig.name}; escalating path={self.config.pcap_path}" ) + # The bash wrapper can die while a confined tcpdump survives its + # AppArmor-blocked signal (the unconfined fallback couldn't escalate) — + # so check tcpdump directly rather than trusting proc.wait(). + if self._tcpdump_pid() is not None: + self._scream_orphaned() else: - proc.wait() - logger.info(f"Mid360PcapRecorder stopped path={self.config.pcap_path}") + logger.info(f"Mid360PcapRecorder stopped path={self.config.pcap_path}") def _signal_group(self, pgid: int, sig: signal.Signals) -> bool: """Signal the tcpdump process group; False if it's already gone. @@ -348,3 +356,51 @@ def _signal_group(self, pgid: int, sig: signal.Signals) -> bool: except (OSError, subprocess.TimeoutExpired): pass return True + + def _tcpdump_pid(self) -> int | None: + """PID of a tcpdump still writing our pcap, or None — used to detect an + orphan that survived the stop because its signal was AppArmor-blocked.""" + path = str(Path(self.config.pcap_path).expanduser()) + try: + result = subprocess.run( + ["pgrep", "-af", "tcpdump"], capture_output=True, text=True, timeout=2.0 + ) + except (OSError, subprocess.TimeoutExpired): + return None + for line in result.stdout.splitlines(): + if path in line: + try: + return int(line.split(None, 1)[0]) + except (ValueError, IndexError): + continue + return None + + def _scream_orphaned(self) -> None: + """Loudly report a tcpdump that outlived the stop, with the exact fix.""" + pid = self._tcpdump_pid() + aa = shutil.which("aa-exec") or "/usr/sbin/aa-exec" + kill = shutil.which("kill") or "/usr/bin/kill" + user = getpass.getuser() + # Narrow sudoers rule: passwordless for ONLY the unconfined kill. + rule = f"{user} ALL=(root) NOPASSWD: {aa} -p unconfined -- {kill} *" + banner = textwrap.dedent(f""" + ############################################################################ + [mid360_record] !!! tcpdump SURVIVED THE STOP — capture is ORPHANED !!! + ############################################################################ + tcpdump pid={pid} is STILL RUNNING and writing {self.config.pcap_path}. + AppArmor's tcpdump profile rejected the kill from this (confined) process, + and the unconfined fallback could not escalate (sudo -n needs a password, + or aa-exec is missing). It will NOT be reaped on its own. + + Kill it now: + sudo {aa} -p unconfined -- {kill} -9 {pid} + + To let the recorder kill it itself next time — passwordless for ONLY this + unconfined kill, not all sudo — install a narrow sudoers rule: + echo '{rule}' | sudo tee /etc/sudoers.d/dimos-mid360-pcap-kill + sudo chmod 440 /etc/sudoers.d/dimos-mid360-pcap-kill + (Verify the paths match `command -v aa-exec` and `command -v kill`.) + ############################################################################ + """).strip() + logger.error(banner) + print(banner, flush=True) From ab4c19d86226c3013c276cd84fb47fad58fae2c1 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 22:26:03 -0700 Subject: [PATCH 079/185] mid360 recorder: include long-term sudoers fix in the bash reaper's failure echo The Python orphan banner already prints the narrow sudoers rule; the bash parent-death reaper only showed the manual kill. Add the same long-term fix (install the /etc/sudoers.d rule) to its echo so both noisy paths tell people how to permanently unblock the kill. --- .../lidar/livox/virtual_mid360/recorder.py | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py index ef427811e2..d3d7a202b4 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py @@ -64,14 +64,26 @@ def _stop_when_parent_dies(cmd: list[str], grace_sec: float) -> list[str]: it from a label tcpdump accepts (a no-op where AppArmor isn't in the way).""" parent_pid = os.getpid() quoted = " ".join(shlex.quote(arg) for arg in cmd) + # Resolved here so the failure echo can show real paths + the long-term fix. + aa = shutil.which("aa-exec") or "/usr/sbin/aa-exec" + kill = shutil.which("kill") or "/usr/bin/kill" + user = getpass.getuser() + sudoers = "/etc/sudoers.d/dimos-mid360-pcap-kill" + # Narrow rule: passwordless for ONLY the unconfined kill, not all sudo. + rule = f"{user} ALL=(root) NOPASSWD: {aa} -p unconfined -- {kill} *" + long_term_fix = ( + f"Long-term fix (passwordless for ONLY this kill, not all sudo): " + f"echo '{rule}' | sudo tee {sudoers} && sudo chmod 440 {sudoers}" + ) def _kill(sig: str) -> str: return ( f'kill -{sig} "$child" 2>/dev/null' - f' || sudo -n aa-exec -p unconfined -- kill -{sig} "$child" 2>/dev/null' + f' || sudo -n {aa} -p unconfined -- {kill} -{sig} "$child" 2>/dev/null' f' || echo "[mid360_record] FAILED to {sig} tcpdump pid $child' f" (AppArmor blocked it + sudo -n could not escalate) — it is ORPHANED." - f' Kill it: sudo aa-exec -p unconfined -- kill -9 $child" >&2' + f" Kill it now: sudo {aa} -p unconfined -- {kill} -9 $child." + f' {long_term_fix}" >&2' ) # Foreground waits on tcpdump so a startup failure propagates its exit code. From 837680994025aaaa6e7fbcf5c5ad9a9f4d83ef65 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 22:41:09 -0700 Subject: [PATCH 080/185] virtual_mid360: drop verbose default fn + rename single-char vars MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Remove the 'fn one() -> 1.0' rate default (the Python wrapper and pcap_to_db both always supply rate, so the serde default was dead code — the default now lives only in the Python VirtualMid360Config, the user-facing layer). Rename terse loop/bind vars (b->byte, f->frame, d->payload, p->pkt, n->len, s->socket, e->err, mk->bind_port) for readability. clippy -D warnings + fmt clean. --- .../lidar/livox/virtual_mid360/src/main.rs | 128 +++++++++--------- 1 file changed, 64 insertions(+), 64 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs index 0e7ed80505..9ad6255a22 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs @@ -33,7 +33,6 @@ struct Config { /// Recorded Mid-360 pcap (data plane: point/IMU/status UDP). Read fully into RAM. pcap: String, /// Replay-speed multiplier; 1.0 = original inter-packet timing, >1 = faster. - #[serde(default = "one")] #[validate(range(min = 0.01, max = 1000.0))] rate: f64, /// Seconds to wait after start before streaming begins. @@ -58,9 +57,6 @@ struct Config { mcast_data: String, } -fn one() -> f64 { - 1.0 -} // 224.1.1.5 is the Livox Mid-360 default multicast_ip (a genuine Livox default, // unlike the lidar/host IP + netns, which are deployment-specific and required). fn default_mcast_data() -> String { @@ -77,8 +73,8 @@ struct VirtualMid360 { // ---- CRCs (Livox SDK2: CRC16-CCITT-FALSE over header[0:18], CRC32/IEEE over data[]) ---- fn crc16_ccitt_false(data: &[u8]) -> u16 { let mut crc: u16 = 0xFFFF; - for &b in data { - crc ^= (b as u16) << 8; + for &byte in data { + crc ^= (byte as u16) << 8; for _ in 0..8 { crc = if crc & 0x8000 != 0 { (crc << 1) ^ 0x1021 @@ -92,8 +88,8 @@ fn crc16_ccitt_false(data: &[u8]) -> u16 { fn crc32_ieee(data: &[u8]) -> u32 { let mut crc: u32 = 0xFFFF_FFFF; - for &b in data { - crc ^= b as u32; + for &byte in data { + crc ^= byte as u32; for _ in 0..8 { crc = if crc & 1 != 0 { (crc >> 1) ^ 0xEDB8_8320 @@ -110,22 +106,22 @@ fn crc32_ieee(data: &[u8]) -> u32 { /// + crc16_h@18 + data[] + crc32_d. `data` is the per-cmd ACK payload. fn build_ack(cmd_id: u16, seq: u32, data: &[u8]) -> Vec { let length = (WRAPPER + data.len()) as u16; - let mut f = vec![0u8; WRAPPER + data.len()]; - f[0] = SOF; - f[1] = 0; // version - f[2..4].copy_from_slice(&length.to_le_bytes()); - f[4..8].copy_from_slice(&seq.to_le_bytes()); - f[8..10].copy_from_slice(&cmd_id.to_le_bytes()); - f[10] = 1; // cmd_type = ACK - f[11] = 1; // sender_type = lidar - // f[12..18] reserved (0) - let crc16 = crc16_ccitt_false(&f[0..18]); - f[18..20].copy_from_slice(&crc16.to_le_bytes()); - // f[20..24] = crc32 of data[] - f[24..].copy_from_slice(data); + let mut frame = vec![0u8; WRAPPER + data.len()]; + frame[0] = SOF; + frame[1] = 0; // version + frame[2..4].copy_from_slice(&length.to_le_bytes()); + frame[4..8].copy_from_slice(&seq.to_le_bytes()); + frame[8..10].copy_from_slice(&cmd_id.to_le_bytes()); + frame[10] = 1; // cmd_type = ACK + frame[11] = 1; // sender_type = lidar + // frame[12..18] reserved (0) + let crc16 = crc16_ccitt_false(&frame[0..18]); + frame[18..20].copy_from_slice(&crc16.to_le_bytes()); + // frame[20..24] = crc32 of data[] + frame[24..].copy_from_slice(data); let crc32 = crc32_ieee(data); - f[20..24].copy_from_slice(&crc32.to_le_bytes()); - f + frame[20..24].copy_from_slice(&crc32.to_le_bytes()); + frame } // ---- classic pcap (LE, magic d4c3b2a1) parser -> data-plane UDP packets ---- @@ -251,7 +247,7 @@ impl VirtualMid360 { }; let packets = match parse_pcap(&cfg.pcap) { - Ok(p) if !p.is_empty() => Arc::new(p), + Ok(parsed) if !parsed.is_empty() => Arc::new(parsed), Ok(_) => { eprintln!( "[virtual_mid360] pcap '{}' has no Livox UDP data packets. \ @@ -260,9 +256,9 @@ impl VirtualMid360 { ); std::process::exit(2); } - Err(e) => { + Err(err) => { eprintln!( - "[virtual_mid360] failed to read pcap '{}': {e}. Fix the path, then re-run.", + "[virtual_mid360] failed to read pcap '{}': {err}. Fix the path, then re-run.", cfg.pcap ); std::process::exit(2); @@ -291,9 +287,9 @@ impl VirtualMid360 { fn spawn_discovery(lidar_ip: Ipv4Addr, stop: Arc) { std::thread::spawn(move || { let sock = match UdpSocket::bind(SocketAddrV4::new(Ipv4Addr::UNSPECIFIED, DISCOVERY_PORT)) { - Ok(s) => s, - Err(e) => { - tracing::error!("discovery bind :{DISCOVERY_PORT} failed: {e}"); + Ok(socket) => socket, + Err(err) => { + tracing::error!("discovery bind :{DISCOVERY_PORT} failed: {err}"); return; } }; @@ -302,11 +298,11 @@ fn spawn_discovery(lidar_ip: Ipv4Addr, stop: Arc) { let bcast = SocketAddrV4::new(Ipv4Addr::BROADCAST, DISCOVERY_PORT); let mut buf = [0u8; 2048]; while !stop.load(Ordering::Relaxed) { - let n = match sock.recv_from(&mut buf) { - Ok((n, _)) => n, + let len = match sock.recv_from(&mut buf) { + Ok((len, _)) => len, Err(_) => continue, }; - if n < WRAPPER || buf[0] != SOF { + if len < WRAPPER || buf[0] != SOF { continue; } let cmd_id = u16::from_le_bytes([buf[8], buf[9]]); @@ -326,20 +322,20 @@ fn spawn_discovery(lidar_ip: Ipv4Addr, stop: Arc) { fn spawn_control(lidar_ip: Ipv4Addr, armed: Arc, stop: Arc) { std::thread::spawn(move || { let sock = match UdpSocket::bind(SocketAddrV4::new(lidar_ip, CMD_PORT)) { - Ok(s) => s, - Err(e) => { - tracing::error!("control bind {lidar_ip}:{CMD_PORT} failed: {e}"); + Ok(socket) => socket, + Err(err) => { + tracing::error!("control bind {lidar_ip}:{CMD_PORT} failed: {err}"); return; } }; sock.set_read_timeout(Some(Duration::from_millis(500))).ok(); let mut buf = [0u8; 2048]; while !stop.load(Ordering::Relaxed) { - let (n, from) = match sock.recv_from(&mut buf) { - Ok(x) => x, + let (len, from) = match sock.recv_from(&mut buf) { + Ok(received) => received, Err(_) => continue, }; - if n < WRAPPER || buf[0] != SOF { + if len < WRAPPER || buf[0] != SOF { continue; } let seq = u32::from_le_bytes([buf[4], buf[5], buf[6], buf[7]]); @@ -374,11 +370,15 @@ fn spawn_stream( stop: Arc, ) { std::thread::spawn(move || { - let mk = |sport: u16| -> std::io::Result { - UdpSocket::bind(SocketAddrV4::new(lidar_ip, sport)) + let bind_port = |src_port: u16| -> std::io::Result { + UdpSocket::bind(SocketAddrV4::new(lidar_ip, src_port)) }; - let (point, imu, status) = match (mk(PORT_POINT), mk(PORT_IMU), mk(PORT_STATUS)) { - (Ok(a), Ok(b), Ok(c)) => (a, b, c), + let (point, imu, status) = match ( + bind_port(PORT_POINT), + bind_port(PORT_IMU), + bind_port(PORT_STATUS), + ) { + (Ok(point_sock), Ok(imu_sock), Ok(status_sock)) => (point_sock, imu_sock, status_sock), _ => { tracing::error!("failed to bind data-plane source ports on {lidar_ip}"); return; @@ -405,34 +405,34 @@ fn spawn_stream( .as_nanos() as u64; let first_orig = packets .iter() - .find(|p| matches!(p.src_port, PORT_POINT | PORT_IMU)) - .map(|p| read_ts_ns(&p.payload)) + .find(|pkt| matches!(pkt.src_port, PORT_POINT | PORT_IMU)) + .map(|pkt| read_ts_ns(&pkt.payload)) .unwrap_or(0); let ts_shift = now_ns.wrapping_sub(first_orig); let t_wall0 = Instant::now(); let mut t_cap0: Option = None; - for p in packets.iter() { + for pkt in packets.iter() { if stop.load(Ordering::Relaxed) { break; } // The Mid-360 MULTICASTS point/IMU to mcast_data:port (the SDK joins that // group — confirmed via `ss -ulnp` showing 224.1.1.5:56301/56401); status // is unicast to the host. Sending point/IMU unicast is silently dropped. - let (sock, dst_ip, dst) = match p.src_port { + let (sock, dst_ip, dst) = match pkt.src_port { PORT_POINT => (&point, mcast_data, DST_POINT), PORT_IMU => (&imu, mcast_data, DST_IMU), PORT_STATUS => (&status, host_ip, DST_STATUS), _ => continue, }; - let t0 = *t_cap0.get_or_insert(p.ts); - let target = (p.ts - t0) / rate; + let t0 = *t_cap0.get_or_insert(pkt.ts); + let target = (pkt.ts - t0) / rate; let elapsed = t_wall0.elapsed().as_secs_f64(); if target > elapsed { std::thread::sleep(Duration::from_secs_f64(target - elapsed)); } - let mut out = p.payload.clone(); - if matches!(p.src_port, PORT_POINT | PORT_IMU) { + let mut out = pkt.payload.clone(); + if matches!(pkt.src_port, PORT_POINT | PORT_IMU) { rewrite_ts(&mut out, ts_shift); } let _ = sock.send_to(&out, SocketAddrV4::new(dst_ip, dst)); @@ -449,17 +449,17 @@ const DEV_TYPE_MID360: u8 = 9; /// ret_code:u8, dev_type:u8, sn[16], lidar_ip[4], cmd_port:u16 LE. /// The SDK's VerifyNetSegment requires lidar_ip on the host's /24 (192.168.1.x). fn discovery_ack_payload(lidar_ip: Ipv4Addr) -> Vec { - let mut d = Vec::with_capacity(24); - d.push(0); // ret_code = success - d.push(DEV_TYPE_MID360); + let mut payload = Vec::with_capacity(24); + payload.push(0); // ret_code = success + payload.push(DEV_TYPE_MID360); // sn[16] MUST be null-terminated within 16 bytes — the SDK treats it as a // C-string (strcpy), so a full-16 SN with no NUL overruns its buffer. let mut sn = [0u8; 16]; sn[..10].copy_from_slice(b"FAKEMID360"); // sn[10..]=0 -> NUL-terminated - d.extend_from_slice(&sn); - d.extend_from_slice(&lidar_ip.octets()); - d.extend_from_slice(&CMD_PORT.to_le_bytes()); - d + payload.extend_from_slice(&sn); + payload.extend_from_slice(&lidar_ip.octets()); + payload.extend_from_slice(&CMD_PORT.to_le_bytes()); + payload } // kKeyFwType (livox_lidar_def.h ParamKeyName = 0x8010); fw_type != 0 => app @@ -476,13 +476,13 @@ fn control_ack_payload(cmd_id: u16) -> Vec { // key:u16 @0, length:u16 @2, value @4). QueryFwType expects one param // keyed kKeyFwType (0x8010) with a 1-byte fw_type value (non-zero = app). 0x0101 => { - let mut d = vec![0u8; 8]; - // d[0] ret_code = 0 - d[1..3].copy_from_slice(&1u16.to_le_bytes()); // param_num = 1 - d[3..5].copy_from_slice(&KEY_FW_TYPE.to_le_bytes()); - d[5..7].copy_from_slice(&1u16.to_le_bytes()); // value length = 1 - d[7] = FW_TYPE_APP; - d + let mut payload = vec![0u8; 8]; + // payload[0] ret_code = 0 + payload[1..3].copy_from_slice(&1u16.to_le_bytes()); // param_num = 1 + payload[3..5].copy_from_slice(&KEY_FW_TYPE.to_le_bytes()); + payload[5..7].copy_from_slice(&1u16.to_le_bytes()); // value length = 1 + payload[7] = FW_TYPE_APP; + payload } // Others: LivoxLidarAsyncControlResponse (packed) { ret_code:u8 @0, // error_key:u16 @1 } = 3 bytes. ret_code=0 (success), error_key=0. From 344560cd593a8c1300cf65b1280373adf8f58439 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 22:49:27 -0700 Subject: [PATCH 081/185] pointlio/timing: replace single-letter var names with meaningful ones Loop indices (i/j -> point_idx/idx/cov_idx), the imu packet counter (n -> pkt_count), PointField builder (f -> field), timing Scope guards + Section ctor/registry (s/n/r -> scope/section/section_name/sections), pcap_to_db handlers + comprehension + path sidecars (v/c/p -> msg/col/sidecar), and the config lambda + bind error (p/e -> path/err). No behavior change. pre-commit + mypy clean. --- .../sensors/lidar/pointlio/cpp/main.cpp | 108 +++++++++--------- .../sensors/lidar/pointlio/cpp/timing.hpp | 30 ++--- .../hardware/sensors/lidar/pointlio/module.py | 11 +- .../lidar/pointlio/tools/pcap_to_db.py | 28 ++--- 4 files changed, 90 insertions(+), 87 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index a56a7ceae7..c77365a265 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -112,12 +112,12 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, pc.fields.resize(4); auto make_field = [](const std::string& name, int32_t offset) { - sensor_msgs::PointField f; - f.name = name; - f.offset = offset; - f.datatype = sensor_msgs::PointField::FLOAT32; - f.count = 1; - return f; + sensor_msgs::PointField field; + field.name = name; + field.offset = offset; + field.datatype = sensor_msgs::PointField::FLOAT32; + field.count = 1; + return field; }; pc.fields[0] = make_field("x", 0); @@ -131,12 +131,12 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, pc.data_length = pc.row_step; pc.data.resize(pc.data_length); - for (int i = 0; i < num_points; ++i) { - float* dst = reinterpret_cast(pc.data.data() + i * 16); - dst[0] = cloud->points[i].x; - dst[1] = cloud->points[i].y; - dst[2] = cloud->points[i].z; - dst[3] = cloud->points[i].intensity; + for (int point_idx = 0; point_idx < num_points; ++point_idx) { + float* dst = reinterpret_cast(pc.data.data() + point_idx * 16); + dst[0] = cloud->points[point_idx].x; + dst[1] = cloud->points[point_idx].y; + dst[2] = cloud->points[point_idx].z; + dst[3] = cloud->points[point_idx].intensity; } g_lcm->publish(chan, &pc); @@ -158,8 +158,8 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times msg.pose.pose.orientation.z = odom.pose.pose.orientation.z; msg.pose.pose.orientation.w = odom.pose.pose.orientation.w; - for (int i = 0; i < 36; ++i) { - msg.pose.covariance[i] = odom.pose.covariance[i]; + for (int idx = 0; idx < 36; ++idx) { + msg.pose.covariance[idx] = odom.pose.covariance[idx]; } // Twist zeroed — FAST-LIO doesn't output velocity. @@ -201,28 +201,30 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ if (data->data_type == DATA_TYPE_CARTESIAN_HIGH) { auto* pts = reinterpret_cast(data->data); - for (uint16_t i = 0; i < dot_num; ++i) { + for (uint16_t point_idx = 0; point_idx < dot_num; ++point_idx) { custom_messages::CustomPoint cp; - cp.x = static_cast(pts[i].x) / 1000.0; // mm → m - cp.y = static_cast(pts[i].y) / 1000.0; - cp.z = static_cast(pts[i].z) / 1000.0; - cp.reflectivity = pts[i].reflectivity; - cp.tag = pts[i].tag; + cp.x = static_cast(pts[point_idx].x) / 1000.0; // mm → m + cp.y = static_cast(pts[point_idx].y) / 1000.0; + cp.z = static_cast(pts[point_idx].z) / 1000.0; + cp.reflectivity = pts[point_idx].reflectivity; + cp.tag = pts[point_idx].tag; cp.line = 0; // Mid-360: single line - cp.offset_time = static_cast((ts_ns - g_frame_start_ns) + i * point_interval_ns); + cp.offset_time = + static_cast((ts_ns - g_frame_start_ns) + point_idx * point_interval_ns); g_accumulated_points.push_back(cp); } } else if (data->data_type == DATA_TYPE_CARTESIAN_LOW) { auto* pts = reinterpret_cast(data->data); - for (uint16_t i = 0; i < dot_num; ++i) { + for (uint16_t point_idx = 0; point_idx < dot_num; ++point_idx) { custom_messages::CustomPoint cp; - cp.x = static_cast(pts[i].x) / 100.0; // cm → m - cp.y = static_cast(pts[i].y) / 100.0; - cp.z = static_cast(pts[i].z) / 100.0; - cp.reflectivity = pts[i].reflectivity; - cp.tag = pts[i].tag; + cp.x = static_cast(pts[point_idx].x) / 100.0; // cm → m + cp.y = static_cast(pts[point_idx].y) / 100.0; + cp.z = static_cast(pts[point_idx].z) / 100.0; + cp.reflectivity = pts[point_idx].reflectivity; + cp.tag = pts[point_idx].tag; cp.line = 0; - cp.offset_time = static_cast((ts_ns - g_frame_start_ns) + i * point_interval_ns); + cp.offset_time = + static_cast((ts_ns - g_frame_start_ns) + point_idx * point_interval_ns); g_accumulated_points.push_back(cp); } } @@ -244,7 +246,7 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, static auto last_wall = clk::now(); auto now_wall = clk::now(); uint64_t prev = last_pkt_ts_ns.exchange(pkt_ts_ns); - uint64_t n = imu_pkt_count.fetch_add(1) + 1; + uint64_t pkt_count = imu_pkt_count.fetch_add(1) + 1; if (prev != 0 && pkt_ts_ns > prev) { uint64_t sensor_gap_us = (pkt_ts_ns - prev) / 1000; uint64_t wall_gap_us = std::chrono::duration_cast( @@ -256,13 +258,13 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, imu_gap_count.fetch_add(1); fprintf(stderr, "[imu-gap] sensor_gap=%.1fms wall_gap=%.1fms pkt#%llu\n", sensor_gap_us / 1000.0, wall_gap_us / 1000.0, - static_cast(n)); + static_cast(pkt_count)); } } last_wall = now_wall; - if (n % 1000 == 0) { + if (pkt_count % 1000 == 0) { fprintf(stderr, "[imu-stats] pkts=%llu gaps>15ms=%llu max_sensor_gap=%.1fms\n", - static_cast(n), + static_cast(pkt_count), static_cast(imu_gap_count.load()), max_sensor_gap_us.load() / 1000.0); } @@ -272,7 +274,7 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, auto* imu_pts = reinterpret_cast(data->data); uint16_t dot_num = data->dot_num; - for (uint16_t i = 0; i < dot_num; ++i) { + for (uint16_t point_idx = 0; point_idx < dot_num; ++point_idx) { auto imu_msg = boost::make_shared(); imu_msg->header.stamp = custom_messages::Time().fromSec(ts); imu_msg->header.seq = 0; @@ -282,23 +284,23 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, imu_msg->orientation.y = 0.0; imu_msg->orientation.z = 0.0; imu_msg->orientation.w = 1.0; - for (int j = 0; j < 9; ++j) - imu_msg->orientation_covariance[j] = 0.0; + for (int cov_idx = 0; cov_idx < 9; ++cov_idx) + imu_msg->orientation_covariance[cov_idx] = 0.0; - imu_msg->angular_velocity.x = static_cast(imu_pts[i].gyro_x); - imu_msg->angular_velocity.y = static_cast(imu_pts[i].gyro_y); - imu_msg->angular_velocity.z = static_cast(imu_pts[i].gyro_z); - for (int j = 0; j < 9; ++j) - imu_msg->angular_velocity_covariance[j] = 0.0; + imu_msg->angular_velocity.x = static_cast(imu_pts[point_idx].gyro_x); + imu_msg->angular_velocity.y = static_cast(imu_pts[point_idx].gyro_y); + imu_msg->angular_velocity.z = static_cast(imu_pts[point_idx].gyro_z); + for (int cov_idx = 0; cov_idx < 9; ++cov_idx) + imu_msg->angular_velocity_covariance[cov_idx] = 0.0; // Point-LIO expects accel in g (EKF does its own scaling). SDK already // reports g, so feed raw — scaling by GRAVITY_MS2 would double-scale and // trip the satu_acc check at rest. - imu_msg->linear_acceleration.x = static_cast(imu_pts[i].acc_x); - imu_msg->linear_acceleration.y = static_cast(imu_pts[i].acc_y); - imu_msg->linear_acceleration.z = static_cast(imu_pts[i].acc_z); - for (int j = 0; j < 9; ++j) - imu_msg->linear_acceleration_covariance[j] = 0.0; + imu_msg->linear_acceleration.x = static_cast(imu_pts[point_idx].acc_x); + imu_msg->linear_acceleration.y = static_cast(imu_pts[point_idx].acc_y); + imu_msg->linear_acceleration.z = static_cast(imu_pts[point_idx].acc_z); + for (int cov_idx = 0; cov_idx < 9; ++cov_idx) + imu_msg->linear_acceleration_covariance[cov_idx] = 0.0; g_fastlio->feed_imu(imu_msg); } @@ -456,7 +458,7 @@ int main(int argc, char** argv) { std::vector points; uint64_t frame_start = 0; { - timing::Scope s(t_emit_check); + timing::Scope scope(t_emit_check); std::lock_guard lock(g_pc_mutex); if (now - *last_emit >= frame_interval) { if (!g_accumulated_points.empty()) { @@ -475,16 +477,16 @@ int main(int argc, char** argv) { lidar_msg->header.frame_id = "livox_frame"; lidar_msg->timebase = frame_start; lidar_msg->lidar_id = 0; - for (int i = 0; i < 3; i++) lidar_msg->rsvd[i] = 0; + for (int idx = 0; idx < 3; idx++) lidar_msg->rsvd[idx] = 0; lidar_msg->point_num = static_cast(points.size()); lidar_msg->points = std::move(points); - timing::Scope s(t_feed_lidar); + timing::Scope scope(t_feed_lidar); fast_lio.feed_lidar(lidar_msg); } // One FAST-LIO IESKF step (cheap when queues empty). { - timing::Scope s(t_process); + timing::Scope scope(t_process); fast_lio.process(); } @@ -499,15 +501,15 @@ int main(int argc, char** argv) { // so build it only when a publish is due. if (lidar_due) { auto body_cloud = ([&]() { - timing::Scope s(t_get_world_cloud); + timing::Scope scope(t_get_world_cloud); return fast_lio.get_body_cloud(); })(); if (body_cloud && !body_cloud->empty()) { auto filtered = ([&]() { - timing::Scope s(t_filter_cloud); + timing::Scope scope(t_filter_cloud); return filter_cloud(body_cloud, filter_cfg); })(); - timing::Scope s(t_publish_lidar); + timing::Scope scope(t_publish_lidar); publish_lidar(filtered, ts); last_pc_publish = now; } @@ -515,7 +517,7 @@ int main(int argc, char** argv) { // Pose + covariance at odom_freq. if (!g_odometry_topic.empty() && now - *last_odom_publish >= odom_interval) { - timing::Scope s(t_publish_odom); + timing::Scope scope(t_publish_odom); publish_odometry(fast_lio.get_odometry(), ts); last_odom_publish = now; } diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp b/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp index ddd72eac47..ca915484bb 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp @@ -29,7 +29,7 @@ struct Section { std::atomic total_ns{0}; std::atomic max_ns{0}; - explicit Section(const char* n); + explicit Section(const char* section_name); void add(uint64_t ns) { count.fetch_add(1, std::memory_order_relaxed); @@ -42,11 +42,11 @@ struct Section { }; inline std::vector& registry() { - static std::vector r; - return r; + static std::vector sections; + return sections; } -inline Section::Section(const char* n) : name(n) { +inline Section::Section(const char* section_name) : name(section_name) { registry().push_back(this); } @@ -55,7 +55,7 @@ struct Scope { std::chrono::steady_clock::time_point t0; bool active; - explicit Scope(Section& s) : sec(s), active(fastlio_debug) { + explicit Scope(Section& section) : sec(section), active(fastlio_debug) { if (active) { t0 = std::chrono::steady_clock::now(); } @@ -91,22 +91,22 @@ inline void maybe_flush(std::chrono::steady_clock::time_point now) { auto dt_ms = std::chrono::duration(now - last).count(); last = now; - for (Section* s : registry()) { - uint64_t c = s->count.exchange(0, std::memory_order_relaxed); - uint64_t tot = s->total_ns.exchange(0, std::memory_order_relaxed); - uint64_t mx = s->max_ns.exchange(0, std::memory_order_relaxed); - if (c == 0) { - std::fprintf(stderr, "[timing] %-24s n=0\n", s->name); + for (Section* section : registry()) { + uint64_t count = section->count.exchange(0, std::memory_order_relaxed); + uint64_t tot = section->total_ns.exchange(0, std::memory_order_relaxed); + uint64_t mx = section->max_ns.exchange(0, std::memory_order_relaxed); + if (count == 0) { + std::fprintf(stderr, "[timing] %-24s n=0\n", section->name); continue; } - double mean_us = static_cast(tot) / static_cast(c) / 1e3; + double mean_us = static_cast(tot) / static_cast(count) / 1e3; double max_us = static_cast(mx) / 1e3; double total_ms = static_cast(tot) / 1e6; - double rate_hz = static_cast(c) * 1000.0 / dt_ms; + double rate_hz = static_cast(count) * 1000.0 / dt_ms; std::fprintf(stderr, "[timing] %-24s n=%5lu rate=%7.1fHz mean=%8.3fus max=%9.3fus tot=%7.2fms\n", - s->name, - static_cast(c), + section->name, + static_cast(count), rate_hz, mean_us, max_us, total_ms); } } diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index 58c105b39b..be9713aca3 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -103,7 +103,8 @@ class PointLioConfig(NativeModuleConfig): # FAST-LIO YAML config (relative to config/ dir, or absolute path) # C++ binary reads YAML directly via yaml-cpp config: Annotated[ - Path, validate_as(...).transform(lambda p: p if p.is_absolute() else _CONFIG_DIR / p) + Path, + validate_as(...).transform(lambda path: path if path.is_absolute() else _CONFIG_DIR / path), ] = Path("default.yaml") debug: bool = False @@ -223,16 +224,16 @@ def _validate_network(self) -> None: try: with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: sock.bind((host_ip, 0)) - except OSError as e: + except OSError as err: _logger.error( - f"PointLio: Cannot bind UDP socket on host_ip={host_ip!r}: {e}\n" + f"PointLio: Cannot bind UDP socket on host_ip={host_ip!r}: {err}\n" f" Another process may be using the Livox SDK ports.\n" f" → Check: ss -ulnp | grep {host_ip}" ) raise RuntimeError( - f"PointLio: Cannot bind UDP on {host_ip}: {e}. " + f"PointLio: Cannot bind UDP on {host_ip}: {err}. " f"Check if another Livox/PointLio process is running." - ) from e + ) from err _logger.info( "PointLio network check passed", diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py index 44cd7d7f97..18ec4da652 100644 --- a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py @@ -134,24 +134,24 @@ def _aligned_ts(self, raw_ts: float, last_ts: float) -> float: self._offset = self._resolve_offset(raw_ts) return max(raw_ts + self._offset, last_ts + _EPS) - async def handle_pointlio_odometry(self, v: Odometry) -> None: + async def handle_pointlio_odometry(self, msg: Odometry) -> None: # `is not None`, not `or`: a real sensor ts of 0.0 must not fall back to # wall time (would misclassify the stream's clock in _resolve_offset). - raw_ts_raw = getattr(v, "ts", None) + raw_ts_raw = getattr(msg, "ts", None) raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() ts = self._aligned_ts(raw_ts, self._last_odom_ts) self._last_odom_ts = ts - pose = getattr(v, "pose", None) + pose = getattr(msg, "pose", None) pose_inner = getattr(pose, "pose", None) if pose is not None else None - self._os.append(v, ts=ts, pose=pose_inner) + self._os.append(msg, ts=ts, pose=pose_inner) self._odom_count += 1 - async def handle_pointlio_lidar(self, v: PointCloud2) -> None: - raw_ts_raw = getattr(v, "ts", None) + async def handle_pointlio_lidar(self, msg: PointCloud2) -> None: + raw_ts_raw = getattr(msg, "ts", None) raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() ts = self._aligned_ts(raw_ts, self._last_lidar_ts) self._last_lidar_ts = ts - self._ls.append(v, ts=ts) + self._ls.append(msg, ts=ts) self._lidar_count += 1 @@ -170,7 +170,7 @@ def _db_ref_start_ts(db_path: Path) -> float: if table.startswith("_") or table.startswith("sqlite_"): continue try: - cols = [c[1] for c in con.execute(f"PRAGMA table_info('{table}')").fetchall()] + cols = [col[1] for col in con.execute(f"PRAGMA table_info('{table}')").fetchall()] if "ts" not in cols: continue row = con.execute(f"SELECT MIN(ts) FROM '{table}'").fetchone() @@ -321,9 +321,9 @@ def _run_outer(args: argparse.Namespace) -> int: # root inner can write it (SQLite WAL refuses cross-uid writes). Restored below. if db_existed: for suffix in ("", "-wal", "-shm"): - p = Path(f"{db_path}{suffix}") - if p.exists(): - _ns(["chown", "0:0", str(p)], check=False) + sidecar = Path(f"{db_path}{suffix}") + if sidecar.exists(): + _ns(["chown", "0:0", str(sidecar)], check=False) _setup_netns( args.drv_ns, args.lidar_ns, args.veth_drv, args.veth_lidar, args.host_ip, args.lidar_ip @@ -398,9 +398,9 @@ def _run_outer(args: argparse.Namespace) -> int: # files back to the invoking user. uid, gid = os.getuid(), os.getgid() for suffix in ("", "-wal", "-shm"): - p = Path(f"{db_path}{suffix}") - if p.exists(): - _ns(["chown", f"{uid}:{gid}", str(p)], check=False) + sidecar = Path(f"{db_path}{suffix}") + if sidecar.exists(): + _ns(["chown", f"{uid}:{gid}", str(sidecar)], check=False) o_cnt, o_min, o_max = _table_stats(db_path, "pointlio_odometry") l_cnt = _table_stats(db_path, "pointlio_lidar")[0] From bea9ec86340c54bae06da71a841276a28dfd4866 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 22:40:28 -0700 Subject: [PATCH 082/185] virtual_mid360: pull pointlio's latest + fix replay stop logic MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Grab pointlio's virtual_mid360 updates (single-pass streamer, configurable multicast, required lidar_ip/host_ip/lidar_netns). Update the FastLio2 demo blueprint + pcap_to_db's VM config JSON for the now-required fields. Fix the orchestrator stop: watch virtual_mid360 log 'data stream finished', drain the scan backlog, then stop the recorder via a stop-file (stream-stagnation is unreliable — a diverging FastLio2 keeps emitting after the sensor goes quiet). Verified: --pcap reproduces divergence and stops cleanly. --- .../lidar/fastlio2/tools/pcap_to_db.py | 94 +++++++++++++------ .../lidar/livox/virtual_mid360/blueprints.py | 4 +- .../lidar/livox/virtual_mid360/module.py | 19 ++-- .../lidar/livox/virtual_mid360/src/main.rs | 85 +++++++++-------- 4 files changed, 130 insertions(+), 72 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index eaa60036e1..49c9bac1fb 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -48,14 +48,15 @@ import sqlite3 import subprocess import sys +import tempfile import time from dimos.hardware.sensors.lidar.fastlio2.recorder import FastLio2Recorder # Poll cadence while recording. _POLL_SEC = 1.0 -# Stop once the odom stream has been stagnant this long (pcap fully replayed). -_STAGNANT_SEC = 6.0 +# Let FastLio2 drain its scan backlog after the pcap finishes before stopping. +_DRAIN_SEC = 10.0 # Privilege-escalation command + network namespace / veth names. The lidar ns # runs virtual_mid360; the drv ns runs the FastLio2 consumer. Defaults are @@ -191,6 +192,10 @@ def _orchestrate(args: argparse.Namespace) -> int: ) consumer: subprocess.Popen[bytes] | None = None + tmp = Path(tempfile.gettempdir()) + stopfile = tmp / f"pcap_to_db_stop.{os.getpid()}" + vmlog = tmp / f"pcap_to_db_vm.{os.getpid()}.log" + stopfile.unlink(missing_ok=True) _setup_netns() try: # FastLio2 consumer in the drv netns (re-exec self as the recorder). @@ -206,6 +211,8 @@ def _orchestrate(args: argparse.Namespace) -> int: "-m", "dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db", "--_consume", + "--_stopfile", + str(stopfile), "--db", str(db), "--duration", @@ -219,6 +226,14 @@ def _orchestrate(args: argparse.Namespace) -> int: cmd += ["--force"] if args.time_offset is not None: cmd += ["--time-offset", str(args.time_offset)] + # SQLite won't let root write a db owned by another user, and the + # recorder runs as root inside the netns. So if we're appending into an + # existing (user-owned) db, take ownership for the run — the chown back + # to the caller at the end restores it. + for suffix in ("", "-wal", "-shm"): + q = Path(str(db) + suffix) + if q.exists(): + _sudo("chown", "0:0", str(q), check=False) consumer = subprocess.Popen(cmd) time.sleep(5) # let the coordinator boot + open the SDK sockets @@ -226,18 +241,48 @@ def _orchestrate(args: argparse.Namespace) -> int: vm_cfg = json.dumps( { "topics": {}, - "config": {"pcap": str(pcap), "rate": 1.0, "delay": 2.0, "lidar_netns": _LIDAR_NS}, + "config": { + "pcap": str(pcap), + "rate": 1.0, + "delay": 2.0, + "lidar_ip": _LIDAR_IP, + "host_ip": _HOST_IP, + "lidar_netns": _LIDAR_NS, + }, } ) - vm = subprocess.Popen( - [_SUDO, "ip", "netns", "exec", _LIDAR_NS, str(_VM_BIN)], stdin=subprocess.PIPE - ) - assert vm.stdin is not None - vm.stdin.write((vm_cfg + "\n").encode()) - vm.stdin.close() - - consumer.wait() - rc = consumer.returncode + with open(vmlog, "wb") as vmerr: + vm = subprocess.Popen( + [_SUDO, "ip", "netns", "exec", _LIDAR_NS, str(_VM_BIN)], + stdin=subprocess.PIPE, + stderr=vmerr, + ) + assert vm.stdin is not None + vm.stdin.write((vm_cfg + "\n").encode()) + vm.stdin.close() + + # virtual_mid360 streams the pcap exactly once, then logs "data + # stream finished". Wait for that (bounded by --duration), let + # FastLio2 drain its scan backlog, then stop the recorder via the + # stop-file. (Stagnation-watching the db is unreliable: a diverging + # FastLio2 keeps emitting long after the sensor goes quiet.) + deadline = time.time() + args.duration + while time.time() < deadline: + if vm.poll() is not None: + break + try: + if b"data stream finished" in vmlog.read_bytes(): + break + except OSError: + pass + time.sleep(1.0) + time.sleep(_DRAIN_SEC) + stopfile.touch() + try: + consumer.wait(timeout=60) + except subprocess.TimeoutExpired: + consumer.terminate() + rc = consumer.returncode or 0 finally: if consumer is not None and consumer.poll() is None: consumer.terminate() @@ -246,6 +291,8 @@ def _orchestrate(args: argparse.Namespace) -> int: except subprocess.TimeoutExpired: consumer.kill() _teardown_netns() + stopfile.unlink(missing_ok=True) + vmlog.unlink(missing_ok=True) # The consumer ran as root inside the netns, so the db is root-owned — # hand it back to the invoking user. @@ -309,25 +356,17 @@ def _consume(args: argparse.Namespace) -> int: ).global_config(n_workers=4, robot_model="mid360_fastlio2_pcap_to_db") coord = ModuleCoordinator.build(blueprint) + # The orchestrator drives the lifetime: it watches virtual_mid360 finish + # streaming, lets the backlog drain, then touches the stop-file. --duration + # is just a safety cap. + stopfile = Path(args._stopfile) if args._stopfile else None t0 = time.time() - last_max = 0.0 - stagnant_since: float | None = None try: while time.time() - t0 < args.duration: time.sleep(_POLL_SEC) - _cnt, _min_ts, max_ts = _table_stats(db_path, "fastlio_odometry") - if max_ts == 0.0: - continue # no data yet — sensor still warming up - if max_ts == last_max: - # Stream stopped advancing → the pcap has been fully replayed. - if stagnant_since is None: - stagnant_since = time.time() - elif time.time() - stagnant_since > _STAGNANT_SEC: - print("[pcap_to_db] replay drained", flush=True) - break - else: - last_max = max_ts - stagnant_since = None + if stopfile is not None and stopfile.exists(): + print("[pcap_to_db] stop signalled", flush=True) + break else: print(f"[pcap_to_db] reached --duration cap {args.duration:.0f}s", flush=True) finally: @@ -384,6 +423,7 @@ def main(argv: list[str]) -> int: ) # Internal: the in-netns recorder half, spawned by the orchestrator. parser.add_argument("--_consume", action="store_true", help=argparse.SUPPRESS) + parser.add_argument("--_stopfile", default=None, help=argparse.SUPPRESS) args = parser.parse_args(argv) if args._consume: diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py index 51ba8b5139..6a95688003 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py @@ -34,7 +34,9 @@ # Set pcap to a recorded Mid-360 capture before running, e.g.: # dimos run virtual-mid360-fastlio --VirtualMid360.pcap /path/to/capture.pcap demo_virtual_mid360_fastlio = autoconnect( - VirtualMid360.blueprint(pcap=""), + VirtualMid360.blueprint( + pcap="", lidar_ip="192.168.1.155", host_ip="192.168.1.5", lidar_netns="fl_lidar" + ), FastLio2.blueprint(), vis_module("rerun"), ).global_config(n_workers=3, robot_model="virtual_mid360_fastlio") diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py index d50cf2cd9a..5ccbda0374 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py @@ -51,12 +51,19 @@ class VirtualMid360Config(NativeModuleConfig): rate: float = 1.0 # Seconds to wait after start before streaming begins. delay: float = 0.0 - # IP the fake lidar sends from (must be on this netns's veth). - lidar_ip: str = "192.168.1.155" - # Host IP the recorded data is delivered to (where the SDK listens). - host_ip: str = "192.168.1.5" - # Network namespace the fake lidar runs inside. - lidar_netns: str = "lidar" + # IP the fake lidar sends from (must be on this netns's veth). Network- + # specific, so required (no default). + lidar_ip: str + # Host IP the recorded data is delivered to (where the SDK listens). Machine- + # specific, so required (no default). + host_ip: str + # Network namespace the fake lidar runs inside. Deployment-specific, so + # required (no default). + lidar_netns: str + # Multicast group the point/IMU streams are sent to. 224.1.1.5 is the Livox + # default the SDK joins (a genuine Livox default), so it stays defaulted; + # override only to match a consumer with a different multicast_ip. + mcast_data: str = "224.1.1.5" class VirtualMid360(NativeModule): diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs index 1472b8c17b..0e7ed80505 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs @@ -1,14 +1,8 @@ // Fake Livox Mid-360 — replays a recorded pcap over a virtual NIC and synthesizes // the Livox SDK2 control handshake so an unmodified, live-mode pointlio ingests it -// through the real Livox SDK as if from a live sensor. -// -// Inverse of pointlio's in-process `cpp/pcap_replay.hpp` (--replay_pcap), which -// bypasses the network. This exercises the full live stack: SDK discovery + -// control handshake, then point/IMU UDP off a (virtual) wire. -// -// Runs inside the "lidar" network namespace (see setup_commands()); the unmodified -// pointlio runs in the peer "drv" namespace. On any failure the error names the -// exact command to run, then asks the user to re-run the module. +// through the real Livox SDK as if from a live sensor. Runs in the "lidar" netns +// (peer "drv" runs pointlio); on a setup failure the error prints the exact +// netns/veth commands to run. use dimos_module::{run, LcmTransport, Module}; use serde::Deserialize; @@ -30,9 +24,6 @@ const PORT_STATUS: u16 = 56200; const DST_POINT: u16 = 56301; const DST_IMU: u16 = 56401; const DST_STATUS: u16 = 56201; -// Mid-360 multicasts point/IMU data to this group (the SDK joins it). Add a -// route (224.1.1.5/32 dev ) so it egresses the virtual NIC. -const MCAST_DATA: Ipv4Addr = Ipv4Addr::new(224, 1, 1, 5); // cmd_id whose ACK means the host finished configuring -> start streaming const CMD_WORKMODE: u16 = 0x0100; @@ -50,27 +41,30 @@ struct Config { #[validate(range(min = 0.0, max = 3600.0))] delay: f64, /// IP the fake lidar sends from (must be assigned to this netns's veth). - #[serde(default = "default_lidar_ip")] + /// Network-specific — required, no default. lidar_ip: String, /// Host IP the recorded data is delivered to (where pointlio's SDK listens). - #[serde(default = "default_host_ip")] + /// Machine-specific — required, no default. host_ip: String, - /// Network namespace the fake lidar must run inside. - #[serde(default = "default_netns")] + /// Network namespace the fake lidar runs inside (named in the setup-help + /// error). Deployment-specific — required, no default. lidar_netns: String, + /// Multicast group the point/IMU streams are sent to. A real Mid-360 + /// multicasts these and the Livox SDK joins whatever `multicast_ip` is in + /// its host config; 224.1.1.5 is the Livox default (what pointlio uses), so + /// it's the default here. Override only to match a consumer configured with + /// a different `multicast_ip`. (Needs a `/32 dev ` route.) + #[serde(default = "default_mcast_data")] + mcast_data: String, } fn one() -> f64 { 1.0 } -fn default_lidar_ip() -> String { - "192.168.1.155".into() -} -fn default_host_ip() -> String { - "192.168.1.5".into() -} -fn default_netns() -> String { - "lidar".into() +// 224.1.1.5 is the Livox Mid-360 default multicast_ip (a genuine Livox default, +// unlike the lidar/host IP + netns, which are deployment-specific and required). +fn default_mcast_data() -> String { + "224.1.1.5".into() } #[derive(Module)] @@ -196,13 +190,14 @@ fn ensure_interface(cfg: &Config) -> Result { .lidar_ip .parse() .map_err(|_| format!("invalid lidar_ip '{}'", cfg.lidar_ip))?; - // Probe: can we bind the lidar control port on lidar_ip? If not, the veth/netns - // isn't set up (or we're in the wrong namespace). + // If we can't bind the control port on lidar_ip, the veth/netns isn't set up + // (or we're in the wrong namespace). let probe = UdpSocket::bind(SocketAddrV4::new(lidar_ip, CMD_PORT)); if probe.is_err() { let ns = &cfg.lidar_netns; let lip = &cfg.lidar_ip; let hip = &cfg.host_ip; + let mcast = &cfg.mcast_data; return Err(format!( "cannot bind {lip}:{CMD_PORT} — the virtual network interface isn't set up \ (or this process isn't in the '{ns}' netns).\n\ @@ -220,7 +215,7 @@ fn ensure_interface(cfg: &Config) -> Result { sudo ip netns exec drv ip link set veth-drv multicast on\n \ sudo ip netns exec {ns} ip link set veth-lidar multicast on\n \ sudo ip netns exec {ns} ip route add 255.255.255.255/32 dev veth-lidar\n \ - sudo ip netns exec {ns} ip route add 224.1.1.5/32 dev veth-lidar # point/IMU multicast\n \ + sudo ip netns exec {ns} ip route add {mcast}/32 dev veth-lidar # point/IMU multicast\n \ sudo ip netns exec drv ip route add 224.0.0.0/4 dev lo # LCM (dimos transport)\n \ sudo ip netns exec {ns} ip route add 224.0.0.0/4 dev lo # LCM (dimos transport)\n\ \nThen launch this module inside the lidar netns:\n \ @@ -236,14 +231,24 @@ impl VirtualMid360 { let lidar_ip = match ensure_interface(cfg) { Ok(ip) => ip, Err(msg) => { - // Actionable error: print the fix command, then exit non-zero so the - // coordinator surfaces it and the user can re-run after setup. + // Exit non-zero so the coordinator surfaces the fix command. tracing::error!("{msg}"); eprintln!("\n[virtual_mid360] {msg}\n"); std::process::exit(2); } }; let host_ip: Ipv4Addr = cfg.host_ip.parse().expect("host_ip validated bindable"); + let mcast_data: Ipv4Addr = match cfg.mcast_data.parse() { + Ok(ip) => ip, + Err(_) => { + eprintln!( + "[virtual_mid360] invalid mcast_data '{}' — expected an IPv4 multicast \ + address matching the consumer's Livox multicast_ip (default 224.1.1.5).", + cfg.mcast_data + ); + std::process::exit(2); + } + }; let packets = match parse_pcap(&cfg.pcap) { Ok(p) if !p.is_empty() => Arc::new(p), @@ -269,14 +274,16 @@ impl VirtualMid360 { let rate = cfg.rate; let delay = cfg.delay; - // Role 1: discovery responder (:56000 broadcast) — synthesize the 0x0000 ACK. + // discovery responder (:56000 broadcast) — synthesize the 0x0000 ACK spawn_discovery(lidar_ip, stop.clone()); - // Role 2: control responder (:56100) — synthesize per-cmd ACKs; arm streaming - // when the host issues the work-mode/config command (0x0100). + // control responder (:56100) — synthesize per-cmd ACKs; arm streaming + // when the host issues the work-mode/config command (0x0100) spawn_control(lidar_ip, armed.clone(), stop.clone()); - // Role 3: data streamer — point/IMU/status, paced at `rate`, timestamps rewritten - // to now, armed by the handshake (with `delay` as a startup floor / fallback). - spawn_stream(lidar_ip, host_ip, packets, rate, delay, armed, stop); + // data streamer — point/IMU/status, paced at `rate`, timestamps rewritten + // to now, armed by the handshake (`delay` as a startup floor / fallback) + spawn_stream( + lidar_ip, host_ip, mcast_data, packets, rate, delay, armed, stop, + ); tracing::info!(lidar = %lidar_ip, host = %host_ip, rate, delay, "virtual_mid360 started"); } } @@ -355,9 +362,11 @@ fn spawn_control(lidar_ip: Ipv4Addr, armed: Arc, stop: Arc>, rate: f64, delay: f64, @@ -407,12 +416,12 @@ fn spawn_stream( if stop.load(Ordering::Relaxed) { break; } - // The Mid-360 MULTICASTS point/IMU to MCAST_DATA:port (the SDK joins that + // The Mid-360 MULTICASTS point/IMU to mcast_data:port (the SDK joins that // group — confirmed via `ss -ulnp` showing 224.1.1.5:56301/56401); status // is unicast to the host. Sending point/IMU unicast is silently dropped. let (sock, dst_ip, dst) = match p.src_port { - PORT_POINT => (&point, MCAST_DATA, DST_POINT), - PORT_IMU => (&imu, MCAST_DATA, DST_IMU), + PORT_POINT => (&point, mcast_data, DST_POINT), + PORT_IMU => (&imu, mcast_data, DST_IMU), PORT_STATUS => (&status, host_ip, DST_STATUS), _ => continue, }; From 90079f39cb9926d453c5b780b60510aa7556eaf3 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 22:51:36 -0700 Subject: [PATCH 083/185] virtual_mid360: drop stale handshake TODOs The discovery + control ACK payloads they flagged are fully implemented (discovery_ack_payload builds the DetectionData; control_ack_payload handles QueryFwType + the generic success ACK) and proven end-to-end, so the 'enumerate the layout' TODOs are obsolete. Replaced with accurate descriptions. --- .../sensors/lidar/livox/virtual_mid360/src/main.rs | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs index 9ad6255a22..557dedbd6d 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs @@ -311,8 +311,7 @@ fn spawn_discovery(lidar_ip: Ipv4Addr, stop: Arc) { continue; } let seq = u32::from_le_bytes([buf[4], buf[5], buf[6], buf[7]]); - // TODO(payload): discovery ACK data describes the device (dev_type, serial, - // lidar_ip, cmd port). Enumerate the exact layout from livox-sdk2 source. + // ACK describes the device (dev_type, serial, lidar_ip, cmd port). let ack = build_ack(0x0000, seq, &discovery_ack_payload(lidar_ip)); let _ = sock.send_to(&ack, bcast); } @@ -340,9 +339,8 @@ fn spawn_control(lidar_ip: Ipv4Addr, armed: Arc, stop: Arc Date: Sun, 14 Jun 2026 22:55:19 -0700 Subject: [PATCH 084/185] pointlio: fix FAST-LIO mislabels in prose/logs (module is Point-LIO) Comments, the file header, the [fastlio2]->[pointlio] log tag, the usage binary name (fastlio2_native->pointlio_native), and module.py comments/docstring called this module FAST-LIO; it's Point-LIO. Left real code symbols (FastLio class, fast_lio.hpp, fastlio_debug, the fast_lio instance + its timing labels) and the build wiring to the upstream dimos-module-fastlio2 repo untouched. --- .../sensors/lidar/pointlio/cpp/main.cpp | 52 +++++++++---------- .../hardware/sensors/lidar/pointlio/module.py | 6 +-- 2 files changed, 29 insertions(+), 29 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index c77365a265..5d835180d9 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -1,14 +1,14 @@ // Copyright 2026 Dimensional Inc. // SPDX-License-Identifier: Apache-2.0 // -// FAST-LIO2 + Livox Mid-360 native module for dimos NativeModule framework. +// Point-LIO + Livox Mid-360 native module for dimos NativeModule framework. // -// Binds Livox SDK2 directly into FAST-LIO-NON-ROS: SDK callbacks feed -// CustomMsg/Imu to FastLio, which performs EKF-LOAM SLAM. Sensor-frame -// (mid360_link) point clouds and odometry are published on LCM. +// Binds Livox SDK2 directly into the Point-LIO core: SDK callbacks feed +// CustomMsg/Imu to the IESKF estimator, which performs LiDAR-inertial SLAM. +// Sensor-frame (mid360_link) point clouds and odometry are published on LCM. // // Usage: -// ./fastlio2_native \ +// ./pointlio_native \ // --lidar '/lidar#sensor_msgs.PointCloud2' \ // --odometry '/odometry#nav_msgs.Odometry' \ // --config_path /path/to/default.yaml \ @@ -43,7 +43,7 @@ #include "sensor_msgs/PointCloud2.hpp" #include "sensor_msgs/PointField.hpp" -// FAST-LIO (header-only core, compiled sources linked via CMake) +// Point-LIO (header-only core, compiled sources linked via CMake) #include "fast_lio.hpp" #include "fast_lio_debug.hpp" @@ -91,7 +91,7 @@ using dimos::time_from_seconds; using dimos::make_header; // Publish the lidar point cloud in the sensor body frame (g_frame_id). -// `cloud` is FAST-LIO's undistorted scan in the sensor's own frame +// `cloud` is Point-LIO's undistorted scan in the sensor's own frame // (get_body_cloud), so points are published as-is with no world registration. static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, const std::string& topic = "") { @@ -162,7 +162,7 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times msg.pose.covariance[idx] = odom.pose.covariance[idx]; } - // Twist zeroed — FAST-LIO doesn't output velocity. + // Twist zeroed — Point-LIO doesn't output velocity. msg.twist.twist.linear.x = 0; msg.twist.twist.linear.y = 0; msg.twist.twist.linear.z = 0; @@ -316,7 +316,7 @@ static void on_info_change(const uint32_t handle, const LivoxLidarInfo* info, std::memcpy(ip, info->lidar_ip, 16); if (fastlio_debug) { - printf("[fastlio2] Device connected: handle=%u type=%u sn=%s ip=%s\n", + printf("[pointlio] Device connected: handle=%u type=%u sn=%s ip=%s\n", handle, info->dev_type, sn, ip); } @@ -340,14 +340,14 @@ int main(int argc, char** argv) { return 1; } - // FAST-LIO config path + // Point-LIO config path std::string config_path = mod.arg("config_path", ""); if (config_path.empty()) { fprintf(stderr, "Error: --config_path is required\n"); return 1; } - // FAST-LIO internal processing rates + // Point-LIO internal processing rates double msr_freq = mod.arg_float("msr_freq", 50.0f); double main_freq = mod.arg_float("main_freq", 5000.0f); @@ -364,7 +364,7 @@ int main(int argc, char** argv) { filter_cfg.sor_mean_k = mod.arg_int("sor_mean_k", 50); filter_cfg.sor_stddev = mod.arg_float("sor_stddev", 1.0f); - // Propagates to the FAST-LIO core via the `fastlio_debug` global. + // Propagates to the Point-LIO core via the `fastlio_debug` global. bool debug = mod.arg_bool("debug", false); fastlio_debug = debug; @@ -383,17 +383,17 @@ int main(int argc, char** argv) { ports.host_log_data = mod.arg_int("host_log_data_port", port_defaults.host_log_data); if (debug) { - printf("[fastlio2] Starting FAST-LIO2 + Livox Mid-360 native module\n"); - printf("[fastlio2] lidar topic: %s\n", + printf("[pointlio] Starting Point-LIO + Livox Mid-360 native module\n"); + printf("[pointlio] lidar topic: %s\n", g_lidar_topic.empty() ? "(disabled)" : g_lidar_topic.c_str()); - printf("[fastlio2] odometry topic: %s\n", + printf("[pointlio] odometry topic: %s\n", g_odometry_topic.empty() ? "(disabled)" : g_odometry_topic.c_str()); - printf("[fastlio2] config: %s\n", config_path.c_str()); - printf("[fastlio2] host_ip: %s lidar_ip: %s frequency: %.1f Hz\n", + printf("[pointlio] config: %s\n", config_path.c_str()); + printf("[pointlio] host_ip: %s lidar_ip: %s frequency: %.1f Hz\n", host_ip.c_str(), lidar_ip.c_str(), g_frequency); - printf("[fastlio2] pointcloud_freq: %.1f Hz odom_freq: %.1f Hz\n", + printf("[pointlio] pointcloud_freq: %.1f Hz odom_freq: %.1f Hz\n", pointcloud_freq, odom_freq); - printf("[fastlio2] voxel_size: %.3f sor_mean_k: %d sor_stddev: %.1f\n", + printf("[pointlio] voxel_size: %.3f sor_mean_k: %d sor_stddev: %.1f\n", filter_cfg.voxel_size, filter_cfg.sor_mean_k, filter_cfg.sor_stddev); } @@ -407,10 +407,10 @@ int main(int argc, char** argv) { } g_lcm = &lcm; - if (debug) printf("[fastlio2] Initializing FAST-LIO...\n"); + if (debug) printf("[pointlio] Initializing Point-LIO...\n"); FastLio fast_lio(config_path, msr_freq, main_freq); g_fastlio = &fast_lio; - if (debug) printf("[fastlio2] FAST-LIO initialized.\n"); + if (debug) printf("[pointlio] Point-LIO initialized.\n"); // Main-loop state. Body lives in `run_main_iter`, driven by the wall-paced // main thread. @@ -453,7 +453,7 @@ int main(int argc, char** argv) { } // At frame rate: drain accumulated points into a CustomMsg and feed - // FAST-LIO. Hold g_pc_mutex across the rate-limit check AND swap so the + // Point-LIO. Hold g_pc_mutex across the rate-limit check AND swap so the // clock + accumulator are observed atomically (no packet slips between). std::vector points; uint64_t frame_start = 0; @@ -484,7 +484,7 @@ int main(int argc, char** argv) { fast_lio.feed_lidar(lidar_msg); } - // One FAST-LIO IESKF step (cheap when queues empty). + // One Point-LIO IESKF step (cheap when queues empty). { timing::Scope scope(t_process); fast_lio.process(); @@ -539,7 +539,7 @@ int main(int argc, char** argv) { LivoxLidarSdkUninit(); return 1; } - if (debug) printf("[fastlio2] SDK started, waiting for device...\n"); + if (debug) printf("[pointlio] SDK started, waiting for device...\n"); while (g_running.load()) { auto loop_start = std::chrono::high_resolution_clock::now(); @@ -556,11 +556,11 @@ int main(int argc, char** argv) { } } - if (debug) printf("[fastlio2] Shutting down...\n"); + if (debug) printf("[pointlio] Shutting down...\n"); g_fastlio = nullptr; LivoxLidarSdkUninit(); g_lcm = nullptr; - if (debug) printf("[fastlio2] Done.\n"); + if (debug) printf("[pointlio] Done.\n"); return 0; } diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index be9713aca3..459941dc10 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -87,7 +87,7 @@ class PointLioConfig(NativeModuleConfig): odom_parent_frame_id: str = FRAME_ODOM odom_frame_id: str = "base_link" - # FAST-LIO internal processing rates + # Point-LIO internal processing rates msr_freq: float = 50.0 main_freq: float = 5000.0 @@ -100,7 +100,7 @@ class PointLioConfig(NativeModuleConfig): sor_mean_k: int = 50 sor_stddev: float = 1.0 - # FAST-LIO YAML config (relative to config/ dir, or absolute path) + # Point-LIO YAML config (relative to config/ dir, or absolute path) # C++ binary reads YAML directly via yaml-cpp config: Annotated[ Path, @@ -127,7 +127,7 @@ class PointLioConfig(NativeModuleConfig): cli_exclude: frozenset[str] = frozenset({"config", "odom_parent_frame_id"}) def model_post_init(self, __context: object) -> None: - """Resolve the FAST-LIO YAML config to an absolute config_path.""" + """Resolve the Point-LIO YAML config to an absolute config_path.""" super().model_post_init(__context) cfg = self.config if not cfg.is_absolute(): From 2e7c755581489636d02b79ec62a4b4fe6544f01c Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 23:25:37 -0700 Subject: [PATCH 085/185] pointlio cpp: drop timing.hpp, rename FastLio->PointLio, body frame ids, trim comments - Remove timing.hpp + all timing::Section/Scope/maybe_flush; replace with plain debug-gated (fastlio_debug) fprintf on feed/publish. - Rename the SLAM instance fast_lio->point_lio, g_fastlio->g_point_lio, and the type FastLio->PointLio (class renamed upstream in dimos-module-fastlio2@pointlio). - odom_frame_id arg -> body_frame_id (matches the Python config rename). - Drop restate-the-code comments. --- .../sensors/lidar/pointlio/cpp/main.cpp | 71 +++++------ .../sensors/lidar/pointlio/cpp/timing.hpp | 114 ------------------ 2 files changed, 27 insertions(+), 158 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 5d835180d9..99ea887b31 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -33,7 +33,6 @@ #include "cloud_filter.hpp" #include "dimos_native_module.hpp" -#include "timing.hpp" // dimos LCM message headers #include "geometry_msgs/Quaternion.hpp" @@ -58,7 +57,7 @@ using livox_common::DATA_TYPE_CARTESIAN_LOW; static std::atomic g_running{true}; static lcm::LCM* g_lcm = nullptr; -static FastLio* g_fastlio = nullptr; +static PointLio* g_point_lio = nullptr; static double get_publish_ts() { return std::chrono::duration( @@ -107,7 +106,6 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, pc.is_bigendian = 0; pc.is_dense = 1; - // x, y, z, intensity (float32 each) pc.fields_length = 4; pc.fields.resize(4); @@ -232,7 +230,7 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, LivoxLidarEthernetPacket* data, void* /*client_data*/) { - if (!g_running.load() || data == nullptr || !g_fastlio) return; + if (!g_running.load() || data == nullptr || !g_point_lio) return; uint64_t pkt_ts_ns = get_timestamp_ns(data); // Live IMU-drop instrumentation: a dropped datagram shows as a sensor-ts @@ -302,7 +300,7 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, for (int cov_idx = 0; cov_idx < 9; ++cov_idx) imu_msg->linear_acceleration_covariance[cov_idx] = 0.0; - g_fastlio->feed_imu(imu_msg); + g_point_lio->feed_imu(imu_msg); } } @@ -340,7 +338,6 @@ int main(int argc, char** argv) { return 1; } - // Point-LIO config path std::string config_path = mod.arg("config_path", ""); if (config_path.empty()) { fprintf(stderr, "Error: --config_path is required\n"); @@ -356,7 +353,7 @@ int main(int argc, char** argv) { std::string lidar_ip = mod.arg("lidar_ip", "192.168.1.155"); g_frequency = mod.arg_float("frequency", 10.0f); g_frame_id = mod.arg_required("frame_id"); - g_child_frame_id = mod.arg_required("odom_frame_id"); + g_child_frame_id = mod.arg_required("body_frame_id"); float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f); float odom_freq = mod.arg_float("odom_freq", 50.0f); CloudFilterConfig filter_cfg; @@ -408,8 +405,8 @@ int main(int argc, char** argv) { g_lcm = &lcm; if (debug) printf("[pointlio] Initializing Point-LIO...\n"); - FastLio fast_lio(config_path, msr_freq, main_freq); - g_fastlio = &fast_lio; + PointLio point_lio(config_path, msr_freq, main_freq); + g_point_lio = &point_lio; if (debug) printf("[pointlio] Point-LIO initialized.\n"); // Main-loop state. Body lives in `run_main_iter`, driven by the wall-paced @@ -427,19 +424,7 @@ int main(int argc, char** argv) { std::optional last_odom_publish; - // Per-section timing for `run_main_iter`, active only with --debug. - // maybe_flush() below prints a summary every second. - static timing::Section t_iter{"run_main_iter"}; - static timing::Section t_emit_check{"emit.lock+swap"}; - static timing::Section t_feed_lidar{"fast_lio.feed_lidar"}; - static timing::Section t_process{"fast_lio.process"}; - static timing::Section t_get_world_cloud{"fast_lio.get_body_cloud"}; - static timing::Section t_filter_cloud{"filter_cloud"}; - static timing::Section t_publish_lidar{"publish_lidar"}; - static timing::Section t_publish_odom{"publish_odometry"}; - auto run_main_iter = [&](std::chrono::steady_clock::time_point now) { - timing::Scope iter_scope(t_iter); // Lazy-seed rate-limit bookmarks on the first iteration so they align // with the wall clock. if (!last_emit.has_value()) { @@ -458,7 +443,6 @@ int main(int argc, char** argv) { std::vector points; uint64_t frame_start = 0; { - timing::Scope scope(t_emit_check); std::lock_guard lock(g_pc_mutex); if (now - *last_emit >= frame_interval) { if (!g_accumulated_points.empty()) { @@ -470,6 +454,7 @@ int main(int argc, char** argv) { } } if (!points.empty()) { + const size_t num_points = points.size(); auto lidar_msg = boost::make_shared(); lidar_msg->header.seq = 0; lidar_msg->header.stamp = custom_messages::Time().fromSec( @@ -478,19 +463,18 @@ int main(int argc, char** argv) { lidar_msg->timebase = frame_start; lidar_msg->lidar_id = 0; for (int idx = 0; idx < 3; idx++) lidar_msg->rsvd[idx] = 0; - lidar_msg->point_num = static_cast(points.size()); + lidar_msg->point_num = static_cast(num_points); lidar_msg->points = std::move(points); - timing::Scope scope(t_feed_lidar); - fast_lio.feed_lidar(lidar_msg); + if (fastlio_debug) { + fprintf(stderr, "[pointlio] feed_lidar frame: %zu points\n", num_points); + } + point_lio.feed_lidar(lidar_msg); } // One Point-LIO IESKF step (cheap when queues empty). - { - timing::Scope scope(t_process); - fast_lio.process(); - } + point_lio.process(); - auto pose = fast_lio.get_pose(); + auto pose = point_lio.get_pose(); if (!pose.empty() && (pose[0] != 0.0 || pose[1] != 0.0 || pose[2] != 0.0)) { double ts = get_publish_ts(); @@ -500,30 +484,29 @@ int main(int argc, char** argv) { // get_body_cloud + filter_cloud (SOR) is the loop's costliest step, // so build it only when a publish is due. if (lidar_due) { - auto body_cloud = ([&]() { - timing::Scope scope(t_get_world_cloud); - return fast_lio.get_body_cloud(); - })(); + auto body_cloud = point_lio.get_body_cloud(); if (body_cloud && !body_cloud->empty()) { - auto filtered = ([&]() { - timing::Scope scope(t_filter_cloud); - return filter_cloud(body_cloud, filter_cfg); - })(); - timing::Scope scope(t_publish_lidar); + auto filtered = filter_cloud(body_cloud, filter_cfg); publish_lidar(filtered, ts); last_pc_publish = now; + if (fastlio_debug) { + fprintf(stderr, + "[pointlio] publish lidar: %zu points pose=(%.3f, %.3f, %.3f)\n", + filtered ? filtered->size() : 0, pose[0], pose[1], pose[2]); + } } } // Pose + covariance at odom_freq. if (!g_odometry_topic.empty() && now - *last_odom_publish >= odom_interval) { - timing::Scope scope(t_publish_odom); - publish_odometry(fast_lio.get_odometry(), ts); + publish_odometry(point_lio.get_odometry(), ts); last_odom_publish = now; + if (fastlio_debug) { + fprintf(stderr, "[pointlio] publish odom: pose=(%.3f, %.3f, %.3f)\n", + pose[0], pose[1], pose[2]); + } } } - - timing::maybe_flush(std::chrono::steady_clock::now()); }; // Packet source: Livox SDK callbacks from its own threads feed the @@ -557,7 +540,7 @@ int main(int argc, char** argv) { } if (debug) printf("[pointlio] Shutting down...\n"); - g_fastlio = nullptr; + g_point_lio = nullptr; LivoxLidarSdkUninit(); g_lcm = nullptr; diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp b/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp deleted file mode 100644 index ca915484bb..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/timing.hpp +++ /dev/null @@ -1,114 +0,0 @@ -// Copyright 2026 Dimensional Inc. -// SPDX-License-Identifier: Apache-2.0 -// -// Lightweight per-section timing for `run_main_iter`. Active only when the -// global `fastlio_debug` flag is set, so non-debug runs pay one branch per -// scope. -// -// Usage: -// static timing::Section sec{"filter_cloud"}; -// { timing::Scope s(sec); /* work */ } -// timing::maybe_flush(now); // periodically - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "fast_lio_debug.hpp" - -namespace timing { - -struct Section { - const char* name; - std::atomic count{0}; - std::atomic total_ns{0}; - std::atomic max_ns{0}; - - explicit Section(const char* section_name); - - void add(uint64_t ns) { - count.fetch_add(1, std::memory_order_relaxed); - total_ns.fetch_add(ns, std::memory_order_relaxed); - uint64_t prev = max_ns.load(std::memory_order_relaxed); - while (ns > prev && - !max_ns.compare_exchange_weak(prev, ns, std::memory_order_relaxed)) { - } - } -}; - -inline std::vector& registry() { - static std::vector sections; - return sections; -} - -inline Section::Section(const char* section_name) : name(section_name) { - registry().push_back(this); -} - -struct Scope { - Section& sec; - std::chrono::steady_clock::time_point t0; - bool active; - - explicit Scope(Section& section) : sec(section), active(fastlio_debug) { - if (active) { - t0 = std::chrono::steady_clock::now(); - } - } - - ~Scope() { - if (!active) { - return; - } - auto dt = std::chrono::duration_cast( - std::chrono::steady_clock::now() - t0).count(); - sec.add(static_cast(dt)); - } -}; - -// Print one line per section to stderr every FLUSH_INTERVAL, then reset. -// Mutex serialises flushes across threads (SDK callbacks vs main loop). -inline void maybe_flush(std::chrono::steady_clock::time_point now) { - if (!fastlio_debug) { - return; - } - constexpr auto FLUSH_INTERVAL = std::chrono::seconds(1); - static std::mutex mtx; - static std::chrono::steady_clock::time_point last; - std::lock_guard lock(mtx); - if (last.time_since_epoch().count() == 0) { - last = now; - return; - } - if (now - last < FLUSH_INTERVAL) { - return; - } - auto dt_ms = std::chrono::duration(now - last).count(); - last = now; - - for (Section* section : registry()) { - uint64_t count = section->count.exchange(0, std::memory_order_relaxed); - uint64_t tot = section->total_ns.exchange(0, std::memory_order_relaxed); - uint64_t mx = section->max_ns.exchange(0, std::memory_order_relaxed); - if (count == 0) { - std::fprintf(stderr, "[timing] %-24s n=0\n", section->name); - continue; - } - double mean_us = static_cast(tot) / static_cast(count) / 1e3; - double max_us = static_cast(mx) / 1e3; - double total_ms = static_cast(tot) / 1e6; - double rate_hz = static_cast(count) * 1000.0 / dt_ms; - std::fprintf(stderr, - "[timing] %-24s n=%5lu rate=%7.1fHz mean=%8.3fus max=%9.3fus tot=%7.2fms\n", - section->name, - static_cast(count), - rate_hz, mean_us, max_us, total_ms); - } -} - -} // namespace timing From 6fb4b093ac0ddef1262ff322836ef9077dc453b5 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 23:25:38 -0700 Subject: [PATCH 086/185] pointlio: review fixes + required network IPs + frame-id rename - pcap_to_db: restore db ownership even if netns setup raises (chown+setup now inside the try); scope cleanup kills to this run's netns via 'ip netns pids' (was system-wide pkill); bound the startup wait so a non-starting binary fails instead of hanging the poll loop forever. - Config frame ids: odom_parent_frame_id->body_start_frame_id, odom_frame_id-> body_frame_id. - host_ip/lidar_ip: no baked-in default (network-specific) -> None + env vars DIMOS_POINTLIO_HOST_IP/LIDAR_IP, required at start(); netns demo supplies the harness values. - Drop the yaml-cpp restate comment. --- .../lidar/livox/virtual_mid360/blueprints.py | 4 +- .../hardware/sensors/lidar/pointlio/module.py | 40 +++++++----- .../lidar/pointlio/tools/pcap_to_db.py | 61 ++++++++++++++----- 3 files changed, 76 insertions(+), 29 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py index f93cf7036f..e8f402797e 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py @@ -49,6 +49,8 @@ host_ip="192.168.1.5", lidar_netns="lidar", ), - PointLio.blueprint(), + # PointLio's host_ip/lidar_ip have no default — supply the harness's values + # so the two ends agree on the (virtual) Mid-360's address. + PointLio.blueprint(host_ip="192.168.1.5", lidar_ip="192.168.1.155"), vis_module("rerun"), ).global_config(n_workers=3, robot_model="virtual_mid360_pointlio") diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index 459941dc10..8482e5bd8e 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -24,7 +24,7 @@ from dimos.core.coordination.module_coordinator import ModuleCoordinator ModuleCoordinator.build(autoconnect( - PointLio.blueprint(host_ip="192.168.1.5"), + PointLio.blueprint(host_ip="192.168.1.5", lidar_ip="192.168.1.155"), SomeConsumer.blueprint(), )).loop() """ @@ -32,11 +32,13 @@ from __future__ import annotations import ipaddress +import os from pathlib import Path import socket import time from typing import TYPE_CHECKING, Annotated +from pydantic import Field from pydantic.experimental.pipeline import validate_as from reactivex.disposable import Disposable @@ -73,19 +75,21 @@ class PointLioConfig(NativeModuleConfig): cwd: str | None = "cpp" executable: str = "result/bin/pointlio_native" build_command: str | None = "nix build .#pointlio_native" - # Livox SDK hardware config - host_ip: str = "192.168.1.5" - lidar_ip: str = "192.168.1.155" + # host_ip/lidar_ip are machine/network-specific, so there's no baked-in + # default. They come from the config or the DIMOS_POINTLIO_HOST_IP / + # DIMOS_POINTLIO_LIDAR_IP env vars; start() errors if neither supplies them. + host_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_POINTLIO_HOST_IP")) + lidar_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_POINTLIO_LIDAR_IP")) frequency: float = 10.0 # frame_id is the header frame for BOTH the point cloud and the odometry # message (the Mid-360 sensor frame). The TF published by the module is a - # separate odom_parent_frame_id -> odom_frame_id transform. + # separate body_start_frame_id -> body_frame_id transform. frame_id: str = "mid360_link" - # TF publish frames (odom -> base_link): the sensor pose expressed as the - # base_link pose in the odom frame. - odom_parent_frame_id: str = FRAME_ODOM - odom_frame_id: str = "base_link" + # TF publish frames (body_start -> body): the sensor pose expressed as the + # body_frame pose in the body_start frame. + body_start_frame_id: str = FRAME_ODOM + body_frame_id: str = "base_link" # Point-LIO internal processing rates msr_freq: float = 50.0 @@ -100,8 +104,7 @@ class PointLioConfig(NativeModuleConfig): sor_mean_k: int = 50 sor_stddev: float = 1.0 - # Point-LIO YAML config (relative to config/ dir, or absolute path) - # C++ binary reads YAML directly via yaml-cpp + # Point-LIO YAML config (relative to config/ dir, or absolute path). config: Annotated[ Path, validate_as(...).transform(lambda path: path if path.is_absolute() else _CONFIG_DIR / path), @@ -124,7 +127,7 @@ class PointLioConfig(NativeModuleConfig): # Resolved in __post_init__, passed as --config_path to the binary config_path: str | None = None - cli_exclude: frozenset[str] = frozenset({"config", "odom_parent_frame_id"}) + cli_exclude: frozenset[str] = frozenset({"config", "body_start_frame_id"}) def model_post_init(self, __context: object) -> None: """Resolve the Point-LIO YAML config to an absolute config_path.""" @@ -152,8 +155,8 @@ def start(self) -> None: def _on_odom_for_tf(self, msg: Odometry) -> None: self.tf.publish( Transform( - frame_id=self.config.odom_parent_frame_id, - child_frame_id=self.config.odom_frame_id, + frame_id=self.config.body_start_frame_id, + child_frame_id=self.config.body_frame_id, translation=Vector3( msg.pose.position.x, msg.pose.position.y, @@ -176,6 +179,15 @@ def stop(self) -> None: def _validate_network(self) -> None: host_ip = self.config.host_ip lidar_ip = self.config.lidar_ip + if not host_ip or not lidar_ip: + missing = [ + name for name, value in (("host_ip", host_ip), ("lidar_ip", lidar_ip)) if not value + ] + raise RuntimeError( + f"PointLio: {' and '.join(missing)} not set — these are network-specific and " + "have no default. Set them in the config, or via the DIMOS_POINTLIO_HOST_IP / " + "DIMOS_POINTLIO_LIDAR_IP env vars." + ) local_ips = [ip for ip, _iface in get_local_ips()] _logger.info( diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py index 18ec4da652..5b8f8ba5a9 100644 --- a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py @@ -77,6 +77,11 @@ _POLL_SEC = 1.0 # Stop after the odom stream has been stagnant this long (pcap fully drained). _STAGNANT_SEC = 5.0 +# Give up if no odometry row appears within this long after start — a binary +# that never produces output (missing artifact, bad pcap, SLAM init crash) would +# otherwise hang the poll loop forever (the stagnation timeout only arms after +# the first row). Generous: covers Point-LIO's IMU-init + first-frame latency. +_STARTUP_TIMEOUT_SEC = 60.0 # virtual_mid360 crate dir (its `nix build .#default` produces result/bin/virtual_mid360). # .../sensors/lidar/pointlio/tools/pcap_to_db.py -> parents[2] == .../sensors/lidar _VM_DIR = Path(__file__).resolve().parents[2] / "livox" / "virtual_mid360" @@ -317,20 +322,25 @@ def _run_outer(args: argparse.Namespace) -> int: flush=True, ) - # An existing db is user-owned; hand it (and its WAL sidecars) to root so the - # root inner can write it (SQLite WAL refuses cross-uid writes). Restored below. - if db_existed: - for suffix in ("", "-wal", "-shm"): - sidecar = Path(f"{db_path}{suffix}") - if sidecar.exists(): - _ns(["chown", "0:0", str(sidecar)], check=False) - - _setup_netns( - args.drv_ns, args.lidar_ns, args.veth_drv, args.veth_lidar, args.host_ip, args.lidar_ip - ) vm_proc: subprocess.Popen[bytes] | None = None inner: subprocess.Popen[bytes] | None = None + # The first chown + netns setup live inside the try so the finally's + # ownership-restore always runs — even if _setup_netns raises (e.g. missing + # CAP_NET_ADMIN), the db must not be left root-owned. try: + # An existing db is user-owned; hand it (and its WAL sidecars) to root so + # the root inner can write it (SQLite WAL refuses cross-uid writes). + # Restored to the invoking user in the finally below. + if db_existed: + for suffix in ("", "-wal", "-shm"): + sidecar = Path(f"{db_path}{suffix}") + if sidecar.exists(): + _ns(["chown", "0:0", str(sidecar)], check=False) + + _setup_netns( + args.drv_ns, args.lidar_ns, args.veth_drv, args.veth_lidar, args.host_ip, args.lidar_ip + ) + # Recorder + live Point-LIO run together in the drv ns (one coordinator). inner_cmd = [ *_sudo(), @@ -385,10 +395,19 @@ def _run_outer(args: argparse.Namespace) -> int: # The inner exits itself once the odom stream goes stagnant (pcap drained). inner.wait() finally: + # Kill ONLY this run's processes — the ones living in its two (uniquely + # named) network namespaces — as root, since the binaries run under sudo. + # `ip netns pids` scopes precisely to this run, so a concurrent run in + # other namespaces (which the docstring promises is supported) is left + # alone; a name-based `pkill virtual_mid360/pointlio_native` would kill + # its binaries too. This also catches the netns children regardless of + # how sudo / ip-netns-exec session or group them. + for ns in (args.lidar_ns, args.drv_ns): + pids = _ns(["ip", "netns", "pids", ns], check=False).stdout.decode().split() + if pids: + _ns(["kill", "-9", *pids], check=False) for proc in (vm_proc, inner): if proc and proc.poll() is None: - _ns(["pkill", "-9", "-f", "virtual_mid360"], check=False) - _ns(["pkill", "-9", "-f", "pointlio_native"], check=False) try: proc.wait(timeout=5) except subprocess.TimeoutExpired: @@ -449,11 +468,25 @@ def _run_inner(args: argparse.Namespace) -> int: last_max = 0.0 first_max: float | None = None stagnant_since: float | None = None + start_time = time.time() + startup_failed = False try: while True: time.sleep(_POLL_SEC) cnt, min_ts, max_ts = _table_stats(db_path, "pointlio_odometry") if cnt == 0: + # Bound the no-output wait so a binary that never starts fails + # cleanly instead of hanging (stagnation timeout only arms once + # the first row exists). + if time.time() - start_time > _STARTUP_TIMEOUT_SEC: + print( + f"[pcap_to_db] no odometry after {_STARTUP_TIMEOUT_SEC:.0f}s — Point-LIO " + "failed to start (check the binary, pcap path, and netns wiring).", + file=sys.stderr, + flush=True, + ) + startup_failed = True + break continue if first_max is None: first_max = min_ts @@ -472,7 +505,7 @@ def _run_inner(args: argparse.Namespace) -> int: stagnant_since = None finally: coord.stop() - return 0 + return 1 if startup_failed else 0 def main(argv: list[str]) -> int: From 4eadca8cd578d9318c020cf96a8e37f92846868b Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 23:26:44 -0700 Subject: [PATCH 087/185] pointlio cpp: bump fast-lio flake to 36a3eef (PointLio class rename) Pairs with the FastLio->PointLio rename so main.cpp compiles against the renamed class. fcbd1c2 -> 36a3eef on dimos-module-fastlio2@pointlio. --- dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock index b319cd9875..8b96a1a50c 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock @@ -37,11 +37,11 @@ "fast-lio": { "flake": false, "locked": { - "lastModified": 1781343224, - "narHash": "sha256-1CBt6felqK7VUbiDijAcjLzNI8A0sMieJdb1NQ1l3yk=", + "lastModified": 1781503486, + "narHash": "sha256-3eT0sBNCKQ0q4z9oKRDNeLRJDxsRL48Lkfk5C2vm47w=", "owner": "dimensionalOS", "repo": "dimos-module-fastlio2", - "rev": "fcbd1c229b6f7eb221df33c00e1ed53fd5c03126", + "rev": "36a3eef261a4a2329a60ab26d474a10adac092d7", "type": "github" }, "original": { From 2a0f51c70395f0c3e4d0c10755a79129e6e2fd51 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 23:29:34 -0700 Subject: [PATCH 088/185] pointlio: TF timestamp = odometry ts exactly (drop falsy-0.0 wall-clock fallback) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit `msg.ts or time.time()` would replace a legitimate ts of 0.0 with wall time — the same pattern the PR fixed in pcap_to_db. Use msg.ts directly so the TF and the odometry pose always share one timestamp (Odometry already substitutes a wall ts for a 0 at construction, so no fallback is needed here). --- dimos/hardware/sensors/lidar/pointlio/module.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index 8482e5bd8e..2fe36bfd90 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -35,7 +35,6 @@ import os from pathlib import Path import socket -import time from typing import TYPE_CHECKING, Annotated from pydantic import Field @@ -168,7 +167,10 @@ def _on_odom_for_tf(self, msg: Odometry) -> None: msg.pose.orientation.z, msg.pose.orientation.w, ), - ts=msg.ts or time.time(), + # Match the odometry message's own timestamp exactly so the TF + # and the pose can't drift apart. No `or time.time()` fallback: a + # real ts of 0.0 must not be silently replaced with wall time. + ts=msg.ts, ) ) From 948c3f52a200c085d513bab0abde2bfe7337c536 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 23:33:24 -0700 Subject: [PATCH 089/185] virtual_mid360: give terse rust names meaningful ones buf->buffer, sock->socket, off->offset, incl->captured_len, ihl->ip_header_len, udp->udp_offset, bcast->broadcast_addr, dst/dst_ip->dest_port/dest_ip, and the ensure_interface locals ns/lip/hip/mcast->netns/lidar_addr/host_addr/mcast_group. clippy -D warnings + fmt clean. --- .../lidar/livox/virtual_mid360/src/main.rs | 120 +++++++++--------- 1 file changed, 63 insertions(+), 57 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs index 557dedbd6d..bd8c75a380 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs @@ -17,7 +17,7 @@ const SOF: u8 = 0xAA; const WRAPPER: usize = 24; // bytes before data[] const CMD_PORT: u16 = 56100; const DISCOVERY_PORT: u16 = 56000; -// data plane: lidar src port -> host dst port +// data plane: lidar source port -> host destination port const PORT_POINT: u16 = 56300; const PORT_IMU: u16 = 56400; const PORT_STATUS: u16 = 56200; @@ -132,41 +132,42 @@ struct Pkt { } fn parse_pcap(path: &str) -> std::io::Result> { - let buf = std::fs::read(path)?; - if buf.len() < 24 || buf[0..4] != [0xd4, 0xc3, 0xb2, 0xa1] { + let buffer = std::fs::read(path)?; + if buffer.len() < 24 || buffer[0..4] != [0xd4, 0xc3, 0xb2, 0xa1] { return Err(std::io::Error::new( std::io::ErrorKind::InvalidData, format!("unsupported pcap (need classic little-endian, magic d4c3b2a1) at {path}"), )); } let mut out = Vec::new(); - let mut off = 24usize; - while off + 16 <= buf.len() { - let ts_sec = u32::from_le_bytes(buf[off..off + 4].try_into().unwrap()); - let ts_usec = u32::from_le_bytes(buf[off + 4..off + 8].try_into().unwrap()); - let incl = u32::from_le_bytes(buf[off + 8..off + 12].try_into().unwrap()) as usize; - off += 16; - if off + incl > buf.len() { + let mut offset = 24usize; + while offset + 16 <= buffer.len() { + let ts_sec = u32::from_le_bytes(buffer[offset..offset + 4].try_into().unwrap()); + let ts_usec = u32::from_le_bytes(buffer[offset + 4..offset + 8].try_into().unwrap()); + let captured_len = + u32::from_le_bytes(buffer[offset + 8..offset + 12].try_into().unwrap()) as usize; + offset += 16; + if offset + captured_len > buffer.len() { break; } - let frame = &buf[off..off + incl]; - off += incl; + let frame = &buffer[offset..offset + captured_len]; + offset += captured_len; // Ethernet(14) -> IPv4 -> UDP if frame.len() < 14 + 20 + 8 || frame[12] != 0x08 || frame[13] != 0x00 { continue; } - let ihl = ((frame[14] & 0x0f) as usize) * 4; + let ip_header_len = ((frame[14] & 0x0f) as usize) * 4; if frame[14 + 9] != 17 { continue; // not UDP } - let udp = 14 + ihl; - if frame.len() < udp + 8 { + let udp_offset = 14 + ip_header_len; + if frame.len() < udp_offset + 8 { continue; } - let src_port = u16::from_be_bytes([frame[udp], frame[udp + 1]]); - let udp_len = u16::from_be_bytes([frame[udp + 4], frame[udp + 5]]) as usize; - let payload_start = udp + 8; - let payload_end = (udp + udp_len).min(frame.len()); + let src_port = u16::from_be_bytes([frame[udp_offset], frame[udp_offset + 1]]); + let udp_len = u16::from_be_bytes([frame[udp_offset + 4], frame[udp_offset + 5]]) as usize; + let payload_start = udp_offset + 8; + let payload_end = (udp_offset + udp_len).min(frame.len()); if payload_end <= payload_start { continue; } @@ -190,32 +191,32 @@ fn ensure_interface(cfg: &Config) -> Result { // (or we're in the wrong namespace). let probe = UdpSocket::bind(SocketAddrV4::new(lidar_ip, CMD_PORT)); if probe.is_err() { - let ns = &cfg.lidar_netns; - let lip = &cfg.lidar_ip; - let hip = &cfg.host_ip; - let mcast = &cfg.mcast_data; + let netns = &cfg.lidar_netns; + let lidar_addr = &cfg.lidar_ip; + let host_addr = &cfg.host_ip; + let mcast_group = &cfg.mcast_data; return Err(format!( - "cannot bind {lip}:{CMD_PORT} — the virtual network interface isn't set up \ - (or this process isn't in the '{ns}' netns).\n\ + "cannot bind {lidar_addr}:{CMD_PORT} — the virtual network interface isn't set up \ + (or this process isn't in the '{netns}' netns).\n\ Run this once (creates the lidar/drv veth pair), then re-run the module:\n\ - \n sudo ip netns add drv\n sudo ip netns add {ns}\n \ + \n sudo ip netns add drv\n sudo ip netns add {netns}\n \ sudo ip link add veth-drv type veth peer name veth-lidar\n \ sudo ip link set veth-drv netns drv\n \ - sudo ip link set veth-lidar netns {ns}\n \ - sudo ip netns exec drv ip addr add {hip}/24 dev veth-drv\n \ - sudo ip netns exec {ns} ip addr add {lip}/24 dev veth-lidar\n \ + sudo ip link set veth-lidar netns {netns}\n \ + sudo ip netns exec drv ip addr add {host_addr}/24 dev veth-drv\n \ + sudo ip netns exec {netns} ip addr add {lidar_addr}/24 dev veth-lidar\n \ sudo ip netns exec drv ip link set veth-drv up\n \ - sudo ip netns exec {ns} ip link set veth-lidar up\n \ + sudo ip netns exec {netns} ip link set veth-lidar up\n \ sudo ip netns exec drv ip link set lo up\n \ - sudo ip netns exec {ns} ip link set lo up\n \ + sudo ip netns exec {netns} ip link set lo up\n \ sudo ip netns exec drv ip link set veth-drv multicast on\n \ - sudo ip netns exec {ns} ip link set veth-lidar multicast on\n \ - sudo ip netns exec {ns} ip route add 255.255.255.255/32 dev veth-lidar\n \ - sudo ip netns exec {ns} ip route add {mcast}/32 dev veth-lidar # point/IMU multicast\n \ + sudo ip netns exec {netns} ip link set veth-lidar multicast on\n \ + sudo ip netns exec {netns} ip route add 255.255.255.255/32 dev veth-lidar\n \ + sudo ip netns exec {netns} ip route add {mcast_group}/32 dev veth-lidar # point/IMU multicast\n \ sudo ip netns exec drv ip route add 224.0.0.0/4 dev lo # LCM (dimos transport)\n \ - sudo ip netns exec {ns} ip route add 224.0.0.0/4 dev lo # LCM (dimos transport)\n\ + sudo ip netns exec {netns} ip route add 224.0.0.0/4 dev lo # LCM (dimos transport)\n\ \nThen launch this module inside the lidar netns:\n \ - sudo ip netns exec {ns} " + sudo ip netns exec {netns} " )); } Ok(lidar_ip) @@ -286,63 +287,68 @@ impl VirtualMid360 { fn spawn_discovery(lidar_ip: Ipv4Addr, stop: Arc) { std::thread::spawn(move || { - let sock = match UdpSocket::bind(SocketAddrV4::new(Ipv4Addr::UNSPECIFIED, DISCOVERY_PORT)) { + let socket = match UdpSocket::bind(SocketAddrV4::new(Ipv4Addr::UNSPECIFIED, DISCOVERY_PORT)) + { Ok(socket) => socket, Err(err) => { tracing::error!("discovery bind :{DISCOVERY_PORT} failed: {err}"); return; } }; - let _ = sock.set_broadcast(true); - sock.set_read_timeout(Some(Duration::from_millis(500))).ok(); - let bcast = SocketAddrV4::new(Ipv4Addr::BROADCAST, DISCOVERY_PORT); - let mut buf = [0u8; 2048]; + let _ = socket.set_broadcast(true); + socket + .set_read_timeout(Some(Duration::from_millis(500))) + .ok(); + let broadcast_addr = SocketAddrV4::new(Ipv4Addr::BROADCAST, DISCOVERY_PORT); + let mut buffer = [0u8; 2048]; while !stop.load(Ordering::Relaxed) { - let len = match sock.recv_from(&mut buf) { + let len = match socket.recv_from(&mut buffer) { Ok((len, _)) => len, Err(_) => continue, }; - if len < WRAPPER || buf[0] != SOF { + if len < WRAPPER || buffer[0] != SOF { continue; } - let cmd_id = u16::from_le_bytes([buf[8], buf[9]]); - let cmd_type = buf[10]; + let cmd_id = u16::from_le_bytes([buffer[8], buffer[9]]); + let cmd_type = buffer[10]; if cmd_id != 0x0000 || cmd_type != 0 { continue; } - let seq = u32::from_le_bytes([buf[4], buf[5], buf[6], buf[7]]); + let seq = u32::from_le_bytes([buffer[4], buffer[5], buffer[6], buffer[7]]); // ACK describes the device (dev_type, serial, lidar_ip, cmd port). let ack = build_ack(0x0000, seq, &discovery_ack_payload(lidar_ip)); - let _ = sock.send_to(&ack, bcast); + let _ = socket.send_to(&ack, broadcast_addr); } }); } fn spawn_control(lidar_ip: Ipv4Addr, armed: Arc, stop: Arc) { std::thread::spawn(move || { - let sock = match UdpSocket::bind(SocketAddrV4::new(lidar_ip, CMD_PORT)) { + let socket = match UdpSocket::bind(SocketAddrV4::new(lidar_ip, CMD_PORT)) { Ok(socket) => socket, Err(err) => { tracing::error!("control bind {lidar_ip}:{CMD_PORT} failed: {err}"); return; } }; - sock.set_read_timeout(Some(Duration::from_millis(500))).ok(); - let mut buf = [0u8; 2048]; + socket + .set_read_timeout(Some(Duration::from_millis(500))) + .ok(); + let mut buffer = [0u8; 2048]; while !stop.load(Ordering::Relaxed) { - let (len, from) = match sock.recv_from(&mut buf) { + let (len, from) = match socket.recv_from(&mut buffer) { Ok(received) => received, Err(_) => continue, }; - if len < WRAPPER || buf[0] != SOF { + if len < WRAPPER || buffer[0] != SOF { continue; } - let seq = u32::from_le_bytes([buf[4], buf[5], buf[6], buf[7]]); - let cmd_id = u16::from_le_bytes([buf[8], buf[9]]); + let seq = u32::from_le_bytes([buffer[4], buffer[5], buffer[6], buffer[7]]); + let cmd_id = u16::from_le_bytes([buffer[8], buffer[9]]); // Per-cmd_id ACK data (control_ack_payload): QueryFwType echoes a // key-value param; the rest reply ret_code(u8)=0 (success). let ack = build_ack(cmd_id, seq, &control_ack_payload(cmd_id)); - let _ = sock.send_to(&ack, from); + let _ = socket.send_to(&ack, from); tracing::info!( cmd_id = format!("0x{cmd_id:04x}"), seq, @@ -417,7 +423,7 @@ fn spawn_stream( // The Mid-360 MULTICASTS point/IMU to mcast_data:port (the SDK joins that // group — confirmed via `ss -ulnp` showing 224.1.1.5:56301/56401); status // is unicast to the host. Sending point/IMU unicast is silently dropped. - let (sock, dst_ip, dst) = match pkt.src_port { + let (socket, dest_ip, dest_port) = match pkt.src_port { PORT_POINT => (&point, mcast_data, DST_POINT), PORT_IMU => (&imu, mcast_data, DST_IMU), PORT_STATUS => (&status, host_ip, DST_STATUS), @@ -433,7 +439,7 @@ fn spawn_stream( if matches!(pkt.src_port, PORT_POINT | PORT_IMU) { rewrite_ts(&mut out, ts_shift); } - let _ = sock.send_to(&out, SocketAddrV4::new(dst_ip, dst)); + let _ = socket.send_to(&out, SocketAddrV4::new(dest_ip, dest_port)); } tracing::info!("data stream finished"); }); From b4a455be4a6e98b9630080e729d7bf5784b1139a Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 23:37:43 -0700 Subject: [PATCH 090/185] virtual_mid360/pcap_to_db: condense verbose comments Trim the multi-line Config field docs, the build_ack/handshake/timestamp/payload protocol comments in main.rs, and the pcap_to_db startup-timeout note down to the essential facts. clippy/fmt + ruff clean; no code changes. --- .../lidar/livox/virtual_mid360/src/main.rs | 60 +++++++------------ .../lidar/pointlio/tools/pcap_to_db.py | 7 +-- 2 files changed, 25 insertions(+), 42 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs index bd8c75a380..7a53eaab2d 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs @@ -30,35 +30,27 @@ const CMD_WORKMODE: u16 = 0x0100; #[derive(Debug, Deserialize, Validate)] #[serde(deny_unknown_fields)] struct Config { - /// Recorded Mid-360 pcap (data plane: point/IMU/status UDP). Read fully into RAM. + /// Recorded Mid-360 pcap (point/IMU/status UDP). Read fully into RAM. pcap: String, - /// Replay-speed multiplier; 1.0 = original inter-packet timing, >1 = faster. + /// Replay speed; 1.0 = original timing, >1 = faster. #[validate(range(min = 0.01, max = 1000.0))] rate: f64, - /// Seconds to wait after start before streaming begins. + /// Seconds to wait before streaming begins. #[serde(default)] #[validate(range(min = 0.0, max = 3600.0))] delay: f64, - /// IP the fake lidar sends from (must be assigned to this netns's veth). - /// Network-specific — required, no default. + /// IP the fake lidar sends from (on this netns's veth). Required. lidar_ip: String, - /// Host IP the recorded data is delivered to (where pointlio's SDK listens). - /// Machine-specific — required, no default. + /// Host IP the data is delivered to (where the SDK listens). Required. host_ip: String, - /// Network namespace the fake lidar runs inside (named in the setup-help - /// error). Deployment-specific — required, no default. + /// Network namespace the fake lidar runs in. Required. lidar_netns: String, - /// Multicast group the point/IMU streams are sent to. A real Mid-360 - /// multicasts these and the Livox SDK joins whatever `multicast_ip` is in - /// its host config; 224.1.1.5 is the Livox default (what pointlio uses), so - /// it's the default here. Override only to match a consumer configured with - /// a different `multicast_ip`. (Needs a `/32 dev ` route.) + /// Multicast group for point/IMU. 224.1.1.5 is the Livox default the SDK + /// joins; override only to match a differently-configured consumer. #[serde(default = "default_mcast_data")] mcast_data: String, } -// 224.1.1.5 is the Livox Mid-360 default multicast_ip (a genuine Livox default, -// unlike the lidar/host IP + netns, which are deployment-specific and required). fn default_mcast_data() -> String { "224.1.1.5".into() } @@ -101,9 +93,8 @@ fn crc32_ieee(data: &[u8]) -> u32 { !crc } -/// Build a Livox SDK2 ACK frame from scratch (synthesized, not replayed): -/// header[0:18] (SOF, version=0, length, seq, cmd_id, cmd_type=1 ACK, sender_type=1) -/// + crc16_h@18 + data[] + crc32_d. `data` is the per-cmd ACK payload. +/// Synthesize a Livox SDK2 ACK frame: 18-byte header (SOF, ver, len, seq, cmd_id, +/// cmd_type=1, sender=1) + crc16@18 + `data` (per-cmd payload) + crc32@20. fn build_ack(cmd_id: u16, seq: u32, data: &[u8]) -> Vec { let length = (WRAPPER + data.len()) as u16; let mut frame = vec![0u8; WRAPPER + data.len()]; @@ -271,13 +262,11 @@ impl VirtualMid360 { let rate = cfg.rate; let delay = cfg.delay; - // discovery responder (:56000 broadcast) — synthesize the 0x0000 ACK + // discovery responder (:56000) — answers the 0x0000 broadcast spawn_discovery(lidar_ip, stop.clone()); - // control responder (:56100) — synthesize per-cmd ACKs; arm streaming - // when the host issues the work-mode/config command (0x0100) + // control responder (:56100) — per-cmd ACKs; arms streaming on 0x0100 spawn_control(lidar_ip, armed.clone(), stop.clone()); - // data streamer — point/IMU/status, paced at `rate`, timestamps rewritten - // to now, armed by the handshake (`delay` as a startup floor / fallback) + // data streamer — point/IMU/status paced at `rate`, timestamps shifted to now spawn_stream( lidar_ip, host_ip, mcast_data, packets, rate, delay, armed, stop, ); @@ -400,9 +389,8 @@ fn spawn_stream( std::thread::sleep(Duration::from_secs_f64(delay.max(0.0))); tracing::info!("streaming {} packets at {rate}x", packets.len()); - // Shift every packet's Livox sensor timestamp by a constant so the first - // emitted packet reads ≈ now and the original inter-packet spacing (used for - // intra-scan deskew) is preserved — the stream looks current/live. + // Shift every packet's sensor timestamp so the first reads ≈ now, + // preserving inter-packet spacing — the stream looks live. let now_ns = SystemTime::now() .duration_since(UNIX_EPOCH) .unwrap() @@ -420,9 +408,8 @@ fn spawn_stream( if stop.load(Ordering::Relaxed) { break; } - // The Mid-360 MULTICASTS point/IMU to mcast_data:port (the SDK joins that - // group — confirmed via `ss -ulnp` showing 224.1.1.5:56301/56401); status - // is unicast to the host. Sending point/IMU unicast is silently dropped. + // Mid-360 multicasts point/IMU to mcast_data:port (the SDK joins it); + // status is unicast. Unicasting point/IMU is silently dropped. let (socket, dest_ip, dest_port) = match pkt.src_port { PORT_POINT => (&point, mcast_data, DST_POINT), PORT_IMU => (&imu, mcast_data, DST_IMU), @@ -475,10 +462,9 @@ const FW_TYPE_APP: u8 = 1; /// per-cmd response struct, which are #pragma pack(1) (packed, no padding). fn control_ack_payload(cmd_id: u16) -> Vec { match cmd_id { - // GetInternalInfo (0x0101): LivoxLidarDiagInternalInfoResponse (packed) — - // ret_code:u8 @0, param_num:u16 @1, data @3 (= LivoxLidarKeyValueParam: - // key:u16 @0, length:u16 @2, value @4). QueryFwType expects one param - // keyed kKeyFwType (0x8010) with a 1-byte fw_type value (non-zero = app). + // GetInternalInfo (0x0101), packed: ret_code:u8 @0, param_num:u16 @1, then + // one KeyValueParam (key:u16, len:u16, value). QueryFwType wants kKeyFwType + // (0x8010) -> 1-byte fw_type != 0. 0x0101 => { let mut payload = vec![0u8; 8]; // payload[0] ret_code = 0 @@ -493,10 +479,8 @@ fn control_ack_payload(cmd_id: u16) -> Vec { _ => vec![0u8; 3], } } -// LivoxLidarEthernetPacket.timestamp[8] sits at payload offset 28 (packed: -// version@0,len@1,time_interval@3,dot_num@5,udp_cnt@7,frame_cnt@9,data_type@10, -// time_type@11,rsvd@12,crc32@24,timestamp@28). The SDK casts the UDP payload -// directly to LivoxLidarEthernetPacket*, so offset 28 is in the payload. +// LivoxLidarEthernetPacket.timestamp[8] is at payload offset 28 (the SDK casts +// the UDP payload straight to that packed struct), so rewrite 8 bytes there. const TS_OFFSET: usize = 28; fn read_ts_ns(payload: &[u8]) -> u64 { diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py index 5b8f8ba5a9..f38e10d04c 100644 --- a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py @@ -77,10 +77,9 @@ _POLL_SEC = 1.0 # Stop after the odom stream has been stagnant this long (pcap fully drained). _STAGNANT_SEC = 5.0 -# Give up if no odometry row appears within this long after start — a binary -# that never produces output (missing artifact, bad pcap, SLAM init crash) would -# otherwise hang the poll loop forever (the stagnation timeout only arms after -# the first row). Generous: covers Point-LIO's IMU-init + first-frame latency. +# No odometry row within this long after start = binary failed to come up +# (missing artifact, bad pcap, SLAM-init crash); bounds the poll loop. Generous +# to cover Point-LIO's IMU-init latency. _STARTUP_TIMEOUT_SEC = 60.0 # virtual_mid360 crate dir (its `nix build .#default` produces result/bin/virtual_mid360). # .../sensors/lidar/pointlio/tools/pcap_to_db.py -> parents[2] == .../sensors/lidar From 76743e05777bdd812a0271308c71d7bb5d412619 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 23:45:42 -0700 Subject: [PATCH 091/185] pointlio cpp: forward Point-LIO velocity in odometry twist (was zeroed) publish_odometry zeroed the twist with a stale 'FAST-LIO doesn't output velocity' comment, discarding Point-LIO's IESKF velocity estimate (its key output). Copy odom.twist.twist (linear from the kf vel state, angular when available) through to the published nav_msgs::Odometry. Verified upstream get_odometry() populates it via set_twist (dimos-module-fastlio2@pointlio laserMapping.hpp:444-471). --- dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 99ea887b31..10f88593fc 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -160,13 +160,13 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times msg.pose.covariance[idx] = odom.pose.covariance[idx]; } - // Twist zeroed — Point-LIO doesn't output velocity. - msg.twist.twist.linear.x = 0; - msg.twist.twist.linear.y = 0; - msg.twist.twist.linear.z = 0; - msg.twist.twist.angular.x = 0; - msg.twist.twist.angular.y = 0; - msg.twist.twist.angular.z = 0; + // Velocity from Point-LIO's IESKF state (its key output over FAST-LIO). + msg.twist.twist.linear.x = odom.twist.twist.linear.x; + msg.twist.twist.linear.y = odom.twist.twist.linear.y; + msg.twist.twist.linear.z = odom.twist.twist.linear.z; + msg.twist.twist.angular.x = odom.twist.twist.angular.x; + msg.twist.twist.angular.y = odom.twist.twist.angular.y; + msg.twist.twist.angular.z = odom.twist.twist.angular.z; std::memset(msg.twist.covariance, 0, sizeof(msg.twist.covariance)); g_lcm->publish(g_odometry_topic, &msg); From 5fdaf5658cc58c077b909f2e0dad4bba38c3004b Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sun, 14 Jun 2026 23:53:00 -0700 Subject: [PATCH 092/185] virtual_mid360 demo: take addresses from env vars, no hardcoded IPs pcap/lidar_ip/host_ip/netns now come from VIRTUAL_MID360_* env vars (the IPs default to empty so they're effectively required), passed to both ends so they agree. Dropped the verbose comments. --- .../lidar/livox/virtual_mid360/blueprints.py | 36 +++++-------------- 1 file changed, 9 insertions(+), 27 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py index e8f402797e..30ca1fe8cc 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py @@ -12,17 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Blueprint: PointLio fed by a VirtualMid360 replaying a recorded pcap. +"""PointLio fed by a VirtualMid360 replaying a recorded pcap (live SDK path). -VirtualMid360 stands up a fake Mid-360 on a virtual NIC and replays the pcap -over the Livox wire protocol; PointLio connects to it exactly as it would to -real hardware (no replay_pcap — it runs in live SDK mode and never knows the -sensor is synthetic). Use this to re-run a recorded session through the live -SLAM path, e.g. to confirm a clip does not diverge. - -The two talk over UDP on lidar_ip/host_ip, so they need a network where those -IPs are reachable (the e2e harness runs VirtualMid360 in a `lidar` netns and -PointLio in a `drv` netns joined by a veth carrying lidar_ip). +Configured via env vars; the two ends must agree on the addresses: +VIRTUAL_MID360_PCAP, VIRTUAL_MID360_LIDAR_IP, VIRTUAL_MID360_HOST_IP, +VIRTUAL_MID360_NETNS. """ import os @@ -32,25 +26,13 @@ from dimos.hardware.sensors.lidar.pointlio.module import PointLio from dimos.visualization.vis_module import vis_module -# Point this at a recorded Mid-360 capture via the env var, e.g. the ruwik2_part3 -# LFS sample: -# VIRTUAL_MID360_PCAP="$(python -c 'from dimos.utils.data import get_data; \ -# print(get_data("ruwik2_part3/ruwik2_part3.pcap"))')" dimos run ... -# Read here (not get_data at import) so registering the blueprint never triggers -# an LFS pull. _PCAP = os.environ.get("VIRTUAL_MID360_PCAP", "") +_LIDAR_IP = os.environ.get("VIRTUAL_MID360_LIDAR_IP", "") +_HOST_IP = os.environ.get("VIRTUAL_MID360_HOST_IP", "") +_NETNS = os.environ.get("VIRTUAL_MID360_NETNS", "lidar") -# lidar_ip/host_ip/lidar_netns are deployment-specific (required, no defaults); -# these are the values the e2e netns harness assigns (drv/lidar veth on .1.x). demo_virtual_mid360_pointlio = autoconnect( - VirtualMid360.blueprint( - pcap=_PCAP, - lidar_ip="192.168.1.155", - host_ip="192.168.1.5", - lidar_netns="lidar", - ), - # PointLio's host_ip/lidar_ip have no default — supply the harness's values - # so the two ends agree on the (virtual) Mid-360's address. - PointLio.blueprint(host_ip="192.168.1.5", lidar_ip="192.168.1.155"), + VirtualMid360.blueprint(pcap=_PCAP, lidar_ip=_LIDAR_IP, host_ip=_HOST_IP, lidar_netns=_NETNS), + PointLio.blueprint(host_ip=_HOST_IP, lidar_ip=_LIDAR_IP), vis_module("rerun"), ).global_config(n_workers=3, robot_model="virtual_mid360_pointlio") From 3ec490de3b8dfcb7b75fa8805ba152c5889ecaae Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 00:28:22 -0700 Subject: [PATCH 093/185] pointlio/virtual_mid360: strip remaining verbose/useless comments Drop decorative section banners (main.cpp Global state/Helpers/Livox SDK callbacks + the LCM-headers label), restate-the-code labels (cloud_filter PCL stage labels, CMake find_package labels, default.yaml yaml-cpp note), the duplicated Cargo.toml crate blurb, and condense the virtual_mid360 Config field docs. Comment-only; kept license headers, why-comments, protocol/magic-number rationale. --- .../lidar/livox/virtual_mid360/Cargo.toml | 3 --- .../lidar/livox/virtual_mid360/module.py | 17 ++++++----------- .../sensors/lidar/pointlio/config/default.yaml | 7 +++---- .../sensors/lidar/pointlio/cpp/CMakeLists.txt | 8 -------- .../sensors/lidar/pointlio/cpp/cloud_filter.hpp | 2 -- .../sensors/lidar/pointlio/cpp/main.cpp | 14 -------------- dimos/hardware/sensors/lidar/pointlio/module.py | 1 - 7 files changed, 9 insertions(+), 43 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml b/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml index c01c38cdae..edbfe42b43 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml @@ -3,9 +3,6 @@ name = "virtual-mid360" version = "0.1.0" edition = "2021" -# Fake Livox Mid-360: replays a recorded pcap over a virtual NIC and synthesizes -# the Livox SDK2 control handshake so an unmodified live-mode pointlio ingests it -# as if from a real sensor. Inverse of pointlio's in-process --replay_pcap. [[bin]] name = "virtual_mid360" path = "src/main.rs" diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py index 5ccbda0374..6f4b23baa2 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py @@ -47,22 +47,17 @@ class VirtualMid360Config(NativeModuleConfig): # Recorded Mid-360 pcap (point/IMU/status UDP). Read fully into RAM. pcap: str = "" - # Replay-speed multiplier; 1.0 = original inter-packet timing. + # Replay speed; 1.0 = original timing. rate: float = 1.0 - # Seconds to wait after start before streaming begins. + # Seconds to wait before streaming begins. delay: float = 0.0 - # IP the fake lidar sends from (must be on this netns's veth). Network- - # specific, so required (no default). + # IP the fake lidar sends from (on this netns's veth). Required. lidar_ip: str - # Host IP the recorded data is delivered to (where the SDK listens). Machine- - # specific, so required (no default). + # Host IP the data is delivered to (where the SDK listens). Required. host_ip: str - # Network namespace the fake lidar runs inside. Deployment-specific, so - # required (no default). + # Network namespace the fake lidar runs in. Required. lidar_netns: str - # Multicast group the point/IMU streams are sent to. 224.1.1.5 is the Livox - # default the SDK joins (a genuine Livox default), so it stays defaulted; - # override only to match a consumer with a different multicast_ip. + # Multicast group for point/IMU. 224.1.1.5 is the Livox default the SDK joins. mcast_data: str = "224.1.1.5" diff --git a/dimos/hardware/sensors/lidar/pointlio/config/default.yaml b/dimos/hardware/sensors/lidar/pointlio/config/default.yaml index b4b93405d1..5c27305e5c 100644 --- a/dimos/hardware/sensors/lidar/pointlio/config/default.yaml +++ b/dimos/hardware/sensors/lidar/pointlio/config/default.yaml @@ -1,7 +1,6 @@ -# Point-LIO config, parsed by parameters.cpp via yaml-cpp. -# Mid-360-specific values: preprocess.lidar_type (Livox), blind/scan_line, -# mapping.extrinsic_T/R (Mid-360 IMU->lidar mount), det_range, fov_degree. -# Retune those for a different sensor; the rest is platform-agnostic. +# Point-LIO config. Mid-360-specific values to retune for a different sensor: +# preprocess.lidar_type (Livox), blind/scan_line, mapping.extrinsic_T/R (Mid-360 +# IMU->lidar mount), det_range, fov_degree. common: con_frame: false con_frame_num: 1 diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt index 28a43e6fc9..27ad294a3b 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt @@ -10,7 +10,6 @@ if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT) set(CMAKE_INSTALL_PREFIX "${CMAKE_SOURCE_DIR}/result" CACHE PATH "" FORCE) endif() -# OpenMP for parallel processing find_package(OpenMP QUIET) if(OpenMP_CXX_FOUND) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") @@ -32,7 +31,6 @@ else() add_definitions(-DMP_PROC_NUM=1) endif() -# Fetch dependencies include(FetchContent) # FAST-LIO-NON-ROS (pass -DFASTLIO_DIR= or use local copy) @@ -40,7 +38,6 @@ if(NOT FASTLIO_DIR) set(FASTLIO_DIR ${CMAKE_CURRENT_SOURCE_DIR}/fast-lio-non-ros) endif() -# dimos-lcm C++ message headers FetchContent_Declare(dimos_lcm GIT_REPOSITORY https://github.com/dimensionalOS/dimos-lcm.git GIT_TAG main @@ -48,23 +45,19 @@ FetchContent_Declare(dimos_lcm ) FetchContent_MakeAvailable(dimos_lcm) -# LCM find_package(PkgConfig REQUIRED) pkg_check_modules(LCM REQUIRED lcm) -# Eigen3 find_package(Eigen3 REQUIRED) # PCL (only components we need — avoid full PCL which drags in VTK via io) find_package(PCL 1.8 REQUIRED COMPONENTS common filters) -# yaml-cpp (FAST-LIO config parsing — standard YAML format) find_package(yaml-cpp REQUIRED) # glog (iVox map backend — include/ivox/ivox3d.h needs glog) find_package(glog REQUIRED) -# Livox SDK2 (from nix or /usr/local fallback) find_library(LIVOX_SDK livox_lidar_sdk_shared) if(NOT LIVOX_SDK) message(FATAL_ERROR "Livox SDK2 not found. Available via nix flake in lidar/livox/") @@ -80,7 +73,6 @@ add_executable(pointlio_native ${FASTLIO_DIR}/src/parameters.cpp ) -# Shared Livox common headers (livox_sdk_config.hpp etc.) if(NOT LIVOX_COMMON_DIR) set(LIVOX_COMMON_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../common) endif() diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/cloud_filter.hpp b/dimos/hardware/sensors/lidar/pointlio/cpp/cloud_filter.hpp index 352ba9bef5..b6973ac5b8 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/cloud_filter.hpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/cloud_filter.hpp @@ -27,14 +27,12 @@ typename pcl::PointCloud::Ptr filter_cloud( if (!input || input->empty()) return input; - // Voxel grid downsample typename pcl::PointCloud::Ptr voxelized(new pcl::PointCloud()); pcl::VoxelGrid vg; vg.setInputCloud(input); vg.setLeafSize(cfg.voxel_size, cfg.voxel_size, cfg.voxel_size); vg.filter(*voxelized); - // Statistical outlier removal if (cfg.sor_mean_k > 0 && voxelized->size() > static_cast(cfg.sor_mean_k)) { typename pcl::PointCloud::Ptr cleaned(new pcl::PointCloud()); pcl::StatisticalOutlierRemoval sor; diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 10f88593fc..99df7e958e 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -34,7 +34,6 @@ #include "cloud_filter.hpp" #include "dimos_native_module.hpp" -// dimos LCM message headers #include "geometry_msgs/Quaternion.hpp" #include "geometry_msgs/Vector3.hpp" #include "nav_msgs/Odometry.hpp" @@ -51,10 +50,6 @@ using livox_common::DATA_TYPE_IMU; using livox_common::DATA_TYPE_CARTESIAN_HIGH; using livox_common::DATA_TYPE_CARTESIAN_LOW; -// --------------------------------------------------------------------------- -// Global state -// --------------------------------------------------------------------------- - static std::atomic g_running{true}; static lcm::LCM* g_lcm = nullptr; static PointLio* g_point_lio = nullptr; @@ -76,10 +71,6 @@ static std::vector g_accumulated_points; static uint64_t g_frame_start_ns = 0; static bool g_frame_has_timestamp = false; -// --------------------------------------------------------------------------- -// Helpers -// --------------------------------------------------------------------------- - static uint64_t get_timestamp_ns(const LivoxLidarEthernetPacket* pkt) { uint64_t ns = 0; std::memcpy(&ns, pkt->timestamp, sizeof(uint64_t)); @@ -173,10 +164,6 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times } -// --------------------------------------------------------------------------- -// Livox SDK callbacks -// --------------------------------------------------------------------------- - static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/, LivoxLidarEthernetPacket* data, void* /*client_data*/) { if (!g_running.load() || data == nullptr) return; @@ -329,7 +316,6 @@ static void signal_handler(int /*sig*/) { int main(int argc, char** argv) { dimos::NativeModule mod(argc, argv); - // Required: LCM topics for output ports g_lidar_topic = mod.has("lidar") ? mod.topic("lidar") : ""; g_odometry_topic = mod.has("odometry") ? mod.topic("odometry") : ""; diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index 2fe36bfd90..90e06a24a9 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -94,7 +94,6 @@ class PointLioConfig(NativeModuleConfig): msr_freq: float = 50.0 main_freq: float = 5000.0 - # Output publish rates (Hz) pointcloud_freq: float = 10.0 odom_freq: float = 30.0 From e06c29f100c28c833b99e6fdd06e2103c85c4010 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 00:37:55 -0700 Subject: [PATCH 094/185] virtual_mid360: default Config from VIRTUAL_MID360_* env vars; slim the demo blueprint Move the env-var reads into VirtualMid360Config (pcap/lidar_ip/host_ip/lidar_netns default from VIRTUAL_MID360_*) so blueprints don't restate them. The demo is now just VirtualMid360.blueprint() + PointLio.blueprint(). --- .../lidar/livox/virtual_mid360/blueprints.py | 16 ++++--------- .../lidar/livox/virtual_mid360/module.py | 23 ++++++++++++------- 2 files changed, 19 insertions(+), 20 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py index 30ca1fe8cc..4a73171076 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py @@ -14,25 +14,17 @@ """PointLio fed by a VirtualMid360 replaying a recorded pcap (live SDK path). -Configured via env vars; the two ends must agree on the addresses: -VIRTUAL_MID360_PCAP, VIRTUAL_MID360_LIDAR_IP, VIRTUAL_MID360_HOST_IP, -VIRTUAL_MID360_NETNS. +Each module reads its own config from env vars (VIRTUAL_MID360_* for the sensor, +DIMOS_POINTLIO_* for PointLio); set the lidar/host IPs so the two ends agree. """ -import os - from dimos.core.coordination.blueprints import autoconnect from dimos.hardware.sensors.lidar.livox.virtual_mid360.module import VirtualMid360 from dimos.hardware.sensors.lidar.pointlio.module import PointLio from dimos.visualization.vis_module import vis_module -_PCAP = os.environ.get("VIRTUAL_MID360_PCAP", "") -_LIDAR_IP = os.environ.get("VIRTUAL_MID360_LIDAR_IP", "") -_HOST_IP = os.environ.get("VIRTUAL_MID360_HOST_IP", "") -_NETNS = os.environ.get("VIRTUAL_MID360_NETNS", "lidar") - demo_virtual_mid360_pointlio = autoconnect( - VirtualMid360.blueprint(pcap=_PCAP, lidar_ip=_LIDAR_IP, host_ip=_HOST_IP, lidar_netns=_NETNS), - PointLio.blueprint(host_ip=_HOST_IP, lidar_ip=_LIDAR_IP), + VirtualMid360.blueprint(), + PointLio.blueprint(), vis_module("rerun"), ).global_config(n_workers=3, robot_model="virtual_mid360_pointlio") diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py index 6f4b23baa2..b4f653da84 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py @@ -35,8 +35,11 @@ from __future__ import annotations +import os from typing import TYPE_CHECKING +from pydantic import Field + from dimos.core.native_module import NativeModule, NativeModuleConfig @@ -45,18 +48,22 @@ class VirtualMid360Config(NativeModuleConfig): executable: str = "result/bin/virtual_mid360" build_command: str | None = "nix build .#default" - # Recorded Mid-360 pcap (point/IMU/status UDP). Read fully into RAM. - pcap: str = "" + # pcap/lidar_ip/host_ip/lidar_netns default from VIRTUAL_MID360_* env vars so + # blueprints needn't restate them. pcap/lidar_ip/host_ip are required — empty + # makes the binary error. + pcap: str = Field(default_factory=lambda: os.environ.get("VIRTUAL_MID360_PCAP", "")) # Replay speed; 1.0 = original timing. rate: float = 1.0 # Seconds to wait before streaming begins. delay: float = 0.0 - # IP the fake lidar sends from (on this netns's veth). Required. - lidar_ip: str - # Host IP the data is delivered to (where the SDK listens). Required. - host_ip: str - # Network namespace the fake lidar runs in. Required. - lidar_netns: str + # IP the fake lidar sends from (on this netns's veth). + lidar_ip: str = Field(default_factory=lambda: os.environ.get("VIRTUAL_MID360_LIDAR_IP", "")) + # Host IP the data is delivered to (where the SDK listens). + host_ip: str = Field(default_factory=lambda: os.environ.get("VIRTUAL_MID360_HOST_IP", "")) + # Network namespace the fake lidar runs in. + lidar_netns: str = Field( + default_factory=lambda: os.environ.get("VIRTUAL_MID360_NETNS", "lidar") + ) # Multicast group for point/IMU. 224.1.1.5 is the Livox default the SDK joins. mcast_data: str = "224.1.1.5" From 95383f812b1e5c0242f1a174aadf011627930a39 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 00:41:44 -0700 Subject: [PATCH 095/185] virtual_mid360: rename env vars VIRTUAL_MID360_* -> DIMOS_MID360_* Includes the binary-path override (VIRTUAL_MID360_BIN -> DIMOS_MID360_BIN). --- .../sensors/lidar/livox/virtual_mid360/blueprints.py | 2 +- .../sensors/lidar/livox/virtual_mid360/module.py | 12 +++++------- .../sensors/lidar/pointlio/tools/pcap_to_db.py | 2 +- 3 files changed, 7 insertions(+), 9 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py index 4a73171076..cc5d04898b 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py @@ -14,7 +14,7 @@ """PointLio fed by a VirtualMid360 replaying a recorded pcap (live SDK path). -Each module reads its own config from env vars (VIRTUAL_MID360_* for the sensor, +Each module reads its own config from env vars (DIMOS_MID360_* for the sensor, DIMOS_POINTLIO_* for PointLio); set the lidar/host IPs so the two ends agree. """ diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py index b4f653da84..275d010ff5 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py @@ -48,22 +48,20 @@ class VirtualMid360Config(NativeModuleConfig): executable: str = "result/bin/virtual_mid360" build_command: str | None = "nix build .#default" - # pcap/lidar_ip/host_ip/lidar_netns default from VIRTUAL_MID360_* env vars so + # pcap/lidar_ip/host_ip/lidar_netns default from DIMOS_MID360_* env vars so # blueprints needn't restate them. pcap/lidar_ip/host_ip are required — empty # makes the binary error. - pcap: str = Field(default_factory=lambda: os.environ.get("VIRTUAL_MID360_PCAP", "")) + pcap: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_PCAP", "")) # Replay speed; 1.0 = original timing. rate: float = 1.0 # Seconds to wait before streaming begins. delay: float = 0.0 # IP the fake lidar sends from (on this netns's veth). - lidar_ip: str = Field(default_factory=lambda: os.environ.get("VIRTUAL_MID360_LIDAR_IP", "")) + lidar_ip: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_LIDAR_IP", "")) # Host IP the data is delivered to (where the SDK listens). - host_ip: str = Field(default_factory=lambda: os.environ.get("VIRTUAL_MID360_HOST_IP", "")) + host_ip: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_HOST_IP", "")) # Network namespace the fake lidar runs in. - lidar_netns: str = Field( - default_factory=lambda: os.environ.get("VIRTUAL_MID360_NETNS", "lidar") - ) + lidar_netns: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_NETNS", "lidar")) # Multicast group for point/IMU. 224.1.1.5 is the Livox default the SDK joins. mcast_data: str = "224.1.1.5" diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py index f38e10d04c..32c7327a35 100644 --- a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py @@ -269,7 +269,7 @@ def _setup_netns( def _resolve_vm_binary() -> str: """Path to the virtual_mid360 binary; build it via nix if not present.""" - env = os.environ.get("VIRTUAL_MID360_BIN") + env = os.environ.get("DIMOS_MID360_BIN") if env: return env out = _VM_DIR / "result" / "bin" / "virtual_mid360" From 0dd0071b3c5197e404ca3add0f9dd996b47cf96d Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 01:05:50 -0700 Subject: [PATCH 096/185] pointlio: make host_ip fully optional (derive from lidar_ip's subnet) host_ip is just the local NIC facing the lidar, so derive it as the local IP on lidar_ip's /24 when unset (or when the configured value isn't local). Only lidar_ip stays required. Errors clearly if no local NIC is on the lidar's subnet. --- .../hardware/sensors/lidar/pointlio/module.py | 69 ++++++++----------- 1 file changed, 29 insertions(+), 40 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index 90e06a24a9..aea91b9aec 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -74,9 +74,9 @@ class PointLioConfig(NativeModuleConfig): cwd: str | None = "cpp" executable: str = "result/bin/pointlio_native" build_command: str | None = "nix build .#pointlio_native" - # host_ip/lidar_ip are machine/network-specific, so there's no baked-in - # default. They come from the config or the DIMOS_POINTLIO_HOST_IP / - # DIMOS_POINTLIO_LIDAR_IP env vars; start() errors if neither supplies them. + # lidar_ip is required (network-specific); from the config or + # DIMOS_POINTLIO_LIDAR_IP. host_ip is optional — start() derives the local + # NIC on the lidar's subnet when unset (or via DIMOS_POINTLIO_HOST_IP). host_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_POINTLIO_HOST_IP")) lidar_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_POINTLIO_LIDAR_IP")) frequency: float = 10.0 @@ -178,60 +178,49 @@ def stop(self) -> None: super().stop() def _validate_network(self) -> None: - host_ip = self.config.host_ip lidar_ip = self.config.lidar_ip - if not host_ip or not lidar_ip: - missing = [ - name for name, value in (("host_ip", host_ip), ("lidar_ip", lidar_ip)) if not value - ] + if not lidar_ip: raise RuntimeError( - f"PointLio: {' and '.join(missing)} not set — these are network-specific and " - "have no default. Set them in the config, or via the DIMOS_POINTLIO_HOST_IP / " - "DIMOS_POINTLIO_LIDAR_IP env vars." + "PointLio: lidar_ip not set — it's network-specific. Set it in the config " + "or via the DIMOS_POINTLIO_LIDAR_IP env var." ) local_ips = [ip for ip, _iface in get_local_ips()] - _logger.info( - "PointLio network check", - host_ip=host_ip, - lidar_ip=lidar_ip, - local_ips=local_ips, - ) - - # Check if host_ip is actually assigned to this machine. - if host_ip not in local_ips: + # host_ip is optional — it's just the local NIC facing the lidar. When + # it's unset (or not one of our IPs) derive it as the local IP on the + # lidar's /24. + configured = self.config.host_ip + if configured and configured in local_ips: + host_ip = configured + else: try: lidar_net = ipaddress.IPv4Network(f"{lidar_ip}/24", strict=False) same_subnet = [ip for ip in local_ips if ipaddress.IPv4Address(ip) in lidar_net] except (ValueError, TypeError): same_subnet = [] - - if same_subnet: - picked = same_subnet[0] - _logger.warning( - f"PointLio: host_ip={host_ip!r} not found locally. " - f"Auto-correcting to {picked!r} (same subnet as lidar {lidar_ip}).", - configured_ip=host_ip, - corrected_ip=picked, - lidar_ip=lidar_ip, - local_ips=local_ips, - ) - self.config.host_ip = picked - host_ip = picked - else: + if not same_subnet: subnet_prefix = ".".join(lidar_ip.split(".")[:3]) msg = ( - f"PointLio: host_ip={host_ip!r} is not assigned to any local interface.\n" - f" Lidar IP: {lidar_ip}\n" + f"PointLio: cannot resolve host_ip — no local IP on the lidar's subnet " + f"(lidar {lidar_ip}).\n" f" Local IPs found: {', '.join(local_ips) or '(none)'}\n" - f" No local IP found on the same subnet as lidar ({lidar_ip}).\n" - f" The lidar network interface may be down or unconfigured.\n" + f" → Bring up the lidar NIC, or set host_ip explicitly.\n" f" → Check: ip addr | grep {subnet_prefix}\n" - f" → Or assign an IP: " - f"sudo ip addr add {subnet_prefix}.5/24 dev \n" + f" → Or assign: sudo ip addr add {subnet_prefix}.5/24 dev \n" ) _logger.error(msg) raise RuntimeError(msg) + host_ip = same_subnet[0] + self.config.host_ip = host_ip + if configured: + _logger.warning( + f"PointLio: host_ip={configured!r} not local; using {host_ip!r} " + f"(on lidar {lidar_ip}'s subnet).", + ) + + _logger.info( + "PointLio network check", host_ip=host_ip, lidar_ip=lidar_ip, local_ips=local_ips + ) # Check if we can bind a UDP socket on host_ip (port 0 = ephemeral). try: From 1f21defa29dcbce46deed4e8652a88c6afea114f Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 16:11:36 +0800 Subject: [PATCH 097/185] clean --- .../lidar/livox/virtual_mid360/recorder.py | 25 ++----------------- 1 file changed, 2 insertions(+), 23 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py index d3d7a202b4..6c764221c9 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py @@ -12,14 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""tcpdump-based recorder for raw Livox Mid-360 UDP — the input for VirtualMid360. - -Captures only the Mid-360's UDP traffic (point/IMU/status) off the wire into a -pcap, so it can be replayed later with the VirtualMid360 module. It records -nothing else — no dimos streams, no db, just the raw Livox packets. Needs -CAP_NET_RAW (grant tcpdump once: `sudo setcap cap_net_raw,cap_net_admin=eip -$(which tcpdump)`). -""" +"""tcpdump-based recorder for raw Livox Mid-360""" from __future__ import annotations @@ -108,28 +101,14 @@ def _kill(sig: str) -> str: class Mid360PcapRecorderConfig(ModuleConfig): - """Where/how to capture; the pcap defaults to recordings/mid360_.pcap.""" - pcap_path: Path = Field(default_factory=_default_pcap_path) - # Capture interface. Machine-specific, so it defaults from DIMOS_PCAP_IFACE - # (falling back to enp2s0) rather than hardcoding a host-only value. - iface: str = Field(default_factory=lambda: os.environ.get("DIMOS_PCAP_IFACE", "enp2s0")) - # The Mid-360's IP — network-specific, so required (no default). Only its - # UDP is captured (`src host and udp`). + iface: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_PCAP_IFACE", "")) lidar_ip: str snaplen: int = 2048 - # Grace period per stop signal (SIGINT -> SIGTERM -> SIGKILL) at teardown. stop_timeout: float = 5.0 class Mid360PcapRecorder(Module): - """Records the raw Livox Mid-360 UDP stream to a pcap via tcpdump. - - Owns nothing but the tcpdump process: on start() it captures every UDP - packet from the lidar into pcap_path; on stop() it flushes + tears it down. - The result replays bit-for-bit through VirtualMid360. - """ - config: Mid360PcapRecorderConfig # tcpdump fails fast (EPERM, bad iface) within a few ms; pause so poll() catches it. From 9a12a309d21291b336f57b94c3a4418c49e2986d Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 16:14:53 +0800 Subject: [PATCH 098/185] - --- .../sensors/lidar/livox/virtual_mid360/recorder.py | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py index 6c764221c9..562761e4c2 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py @@ -48,13 +48,7 @@ def _default_pcap_path() -> Path: def _stop_when_parent_dies(cmd: list[str], grace_sec: float) -> list[str]: - """Wrap cmd so it's reaped if this recorder dies — including via SIGKILL, - which it can't intercept — otherwise tcpdump would outlive its session. - - The kills fall back to an unconfined AppArmor label: tcpdump's profile - rejects signals from a confined (e.g. vscode-labeled) sender with EPERM, so - a plain kill silently fails there — `sudo -n aa-exec -p unconfined` re-issues - it from a label tcpdump accepts (a no-op where AppArmor isn't in the way).""" + """complicated because of AppArmor label. Must kill with `sudo -n aa-exec -p unconfined`""" parent_pid = os.getpid() quoted = " ".join(shlex.quote(arg) for arg in cmd) # Resolved here so the failure echo can show real paths + the long-term fix. @@ -111,13 +105,10 @@ class Mid360PcapRecorderConfig(ModuleConfig): class Mid360PcapRecorder(Module): config: Mid360PcapRecorderConfig - # tcpdump fails fast (EPERM, bad iface) within a few ms; pause so poll() catches it. _TCPDUMP_STARTUP_PROBE_SEC: float = 0.3 # Declare the capture dead if nothing landed after this long. _PCAP_WATCHDOG_SEC: float = 5.0 - # A libpcap file with zero packets is just its 24-byte global header. _PCAP_GLOBAL_HEADER_BYTES: int = 24 - # How long the diagnostic sniff listens for *any* UDP source on the iface. _PCAP_DIAGNOSTIC_SNIFF_SEC: float = 3.0 _pcap_proc: subprocess.Popen[bytes] | None = None @@ -376,7 +367,7 @@ def _scream_orphaned(self) -> None: rule = f"{user} ALL=(root) NOPASSWD: {aa} -p unconfined -- {kill} *" banner = textwrap.dedent(f""" ############################################################################ - [mid360_record] !!! tcpdump SURVIVED THE STOP — capture is ORPHANED !!! + !!! kill failed - mid360_record WILL EAT YOUR DISK IF YOU DONT KILL !!! ############################################################################ tcpdump pid={pid} is STILL RUNNING and writing {self.config.pcap_path}. AppArmor's tcpdump profile rejected the kill from this (confined) process, From 6f70e299a081f743f77769dc0d22e522b70276dd Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 16:20:44 +0800 Subject: [PATCH 099/185] pointlio cpp: bump fast-lio pin to 46b1a9e (macOS libc++/apple-sdk-26 build fix) --- dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock index 8b96a1a50c..f7d42f90d5 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock @@ -37,11 +37,11 @@ "fast-lio": { "flake": false, "locked": { - "lastModified": 1781503486, - "narHash": "sha256-3eT0sBNCKQ0q4z9oKRDNeLRJDxsRL48Lkfk5C2vm47w=", + "lastModified": 1781511489, + "narHash": "sha256-iRjpHNdGVPXXVSssYcOV1crSJ1IJ5k0EIiGSzXuXGtU=", "owner": "dimensionalOS", "repo": "dimos-module-fastlio2", - "rev": "36a3eef261a4a2329a60ab26d474a10adac092d7", + "rev": "46b1a9e48f749076b8137b51d372a1dcf4afa73c", "type": "github" }, "original": { From d234536cc4af21e4088119d32dec0d827fd9eeed Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 16:20:45 +0800 Subject: [PATCH 100/185] =?UTF-8?q?virtual=5Fmid360:=20re-lock=20dimos-rep?= =?UTF-8?q?o=20to=20repo=20root=20(stale=204-level=20path=20=E2=86=92=206-?= =?UTF-8?q?level)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../sensors/lidar/livox/virtual_mid360/flake.lock | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.lock b/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.lock index a548660557..63a40f41f0 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.lock +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.lock @@ -3,17 +3,17 @@ "dimos-repo": { "flake": false, "locked": { - "lastModified": 1779865691, - "narHash": "sha256-2CVWcov7DiC1qX/B/zFKDJiSYsnbrZ3FNT/viprFWTQ=", - "ref": "refs/heads/jeff/feat/g1_raycast", - "rev": "51666bcd298c1d08bdee179f176f45c0a7dd417d", - "revCount": 744, + "lastModified": 1781505942, + "narHash": "sha256-NAjf4/pEbZGbOVFfkbdZCBbZJMrpErcQuFmqSeSRbRM=", + "ref": "refs/heads/jeff/feat/pointlio_native", + "rev": "76743e05777bdd812a0271308c71d7bb5d412619", + "revCount": 860, "type": "git", - "url": "file:../../../.." + "url": "file:../../../../../.." }, "original": { "type": "git", - "url": "file:../../../.." + "url": "file:../../../../../.." } }, "flake-utils": { From be4c1f8c05e67659b90e11618c3cff2d208dcd25 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 01:28:52 -0700 Subject: [PATCH 101/185] pcap_to_db: make netns opt-in; default to no-netns aliases on one host MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Without --lidar-ns, alias host_ip+lidar_ip on a dummy interface (--alias-iface) and run both ends in the host namespace as the normal user (no sudo for the processes, no db chown dance — db stays user-owned). Multicast loopback delivers the fake lidar's point/IMU to the local SDK. Pass --lidar-ns/--drv-ns to use the isolated netns+veth path (needed for concurrent replays). Setup still needs CAP_NET_ADMIN (sudo) to add the aliases/routes. --- .../lidar/pointlio/tools/pcap_to_db.py | 204 +++++++++++------- 1 file changed, 126 insertions(+), 78 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py index 32c7327a35..05eeada809 100644 --- a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py @@ -23,13 +23,13 @@ * ``pointlio_odometry`` — the IESKF pose at the native odom rate. * ``pointlio_lidar`` — the sensor-frame point cloud at the native rate. -It stands up two network namespaces joined by a veth: ``virtual_mid360`` runs in -the ``lidar`` ns and streams the pcap; live Point-LIO + the recorder run together -in the ``drv`` ns (one coordinator, so their LCM streams wire up normally). Since -this creates network namespaces + veths, **it needs CAP_NET_ADMIN** — it shells -out to ``sudo`` for those steps (so passwordless sudo, or running as root, is -required). Replay is real time (Point-LIO is not deterministic), so two runs over -the same pcap will differ slightly. +By default both ends run in the host namespace with ``host_ip``/``lidar_ip`` +aliased on a dummy interface (``--alias-iface``); pass ``--lidar-ns``/``--drv-ns`` +to isolate them in separate netns + veth instead (needed to run two replays at +once). Either way it configures interfaces/routes, so **it needs CAP_NET_ADMIN** +and shells out to ``sudo`` for the setup. Live Point-LIO + the recorder run +together (one coordinator, so their LCM streams wire up normally). Replay is real +time (Point-LIO is not deterministic), so two runs over the same pcap differ. The ``--db`` is optional: with no existing db a fresh one is built from scratch (defaults to ``.db`` next to the pcap). With an existing db the two streams @@ -267,6 +267,30 @@ def _setup_netns( _ns(cmd) +def _teardown_aliases(iface: str) -> None: + _ns(["ip", "link", "del", iface], check=False) + + +def _setup_aliases(iface: str, host_ip: str, lidar_ip: str) -> None: + """No-netns setup: put host_ip + lidar_ip on a dummy interface in the same + /24 with the Livox multicast + discovery-broadcast routes. Both processes + then run in the host namespace; multicast loopback (on by default) delivers + the fake lidar's point/IMU to the local SDK. (Still iproute2/Linux — macOS + would need the ifconfig-alias equivalent.)""" + _teardown_aliases(iface) + steps = [ + ["ip", "link", "add", iface, "type", "dummy"], + ["ip", "addr", "add", f"{host_ip}/24", "dev", iface], + ["ip", "addr", "add", f"{lidar_ip}/24", "dev", iface], + ["ip", "link", "set", iface, "up"], + ["ip", "link", "set", iface, "multicast", "on"], + ["ip", "route", "add", "224.1.1.5/32", "dev", iface], + ["ip", "route", "add", "255.255.255.255/32", "dev", iface], + ] + for cmd in steps: + _ns(cmd) + + def _resolve_vm_binary() -> str: """Path to the virtual_mid360 binary; build it via nix if not present.""" env = os.environ.get("DIMOS_MID360_BIN") @@ -314,60 +338,76 @@ def _run_outer(args: argparse.Namespace) -> int: ref_start_ts = _db_ref_start_ts(db_path) vm_bin = _resolve_vm_binary() + net = ( + f"netns {args.drv_ns}/{args.lidar_ns}" + if args.lidar_ns + else f"aliases on {args.alias_iface}" + ) print( f"[pcap_to_db] pcap={pcap_path.name} db={db_path.name} " f"({'append' if db_existed else 'new'}) rate={args.rate} " - f"ns={args.drv_ns}/{args.lidar_ns} ips={args.host_ip}/{args.lidar_ip}", + f"{net} ips={args.host_ip}/{args.lidar_ip}", flush=True, ) + # netns is opt-in: with --lidar-ns the two ends get isolated namespaces (root + # inner); without it we alias both IPs on a dummy interface and run everything + # in the host ns as the normal user (so the db stays user-owned — no chown). + use_netns = bool(args.lidar_ns) vm_proc: subprocess.Popen[bytes] | None = None inner: subprocess.Popen[bytes] | None = None - # The first chown + netns setup live inside the try so the finally's - # ownership-restore always runs — even if _setup_netns raises (e.g. missing - # CAP_NET_ADMIN), the db must not be left root-owned. + # Setup lives inside the try so the finally always cleans up + (netns mode) + # restores db ownership, even if setup raises (e.g. missing CAP_NET_ADMIN). try: - # An existing db is user-owned; hand it (and its WAL sidecars) to root so - # the root inner can write it (SQLite WAL refuses cross-uid writes). - # Restored to the invoking user in the finally below. - if db_existed: - for suffix in ("", "-wal", "-shm"): - sidecar = Path(f"{db_path}{suffix}") - if sidecar.exists(): - _ns(["chown", "0:0", str(sidecar)], check=False) - - _setup_netns( - args.drv_ns, args.lidar_ns, args.veth_drv, args.veth_lidar, args.host_ip, args.lidar_ip + if use_netns: + # Root inner can't write a user-owned WAL db (SQLite refuses cross-uid + # writes), so hand it to root; restored in the finally. + if db_existed: + for suffix in ("", "-wal", "-shm"): + sidecar = Path(f"{db_path}{suffix}") + if sidecar.exists(): + _ns(["chown", "0:0", str(sidecar)], check=False) + _setup_netns( + args.drv_ns, + args.lidar_ns, + args.veth_drv, + args.veth_lidar, + args.host_ip, + args.lidar_ip, + ) + inner_prefix = [*_sudo(), "ip", "netns", "exec", args.drv_ns] + vm_prefix = [*_sudo(), "ip", "netns", "exec", args.lidar_ns] + else: + _setup_aliases(args.alias_iface, args.host_ip, args.lidar_ip) + inner_prefix = [] + vm_prefix = [] + + # Recorder + live Point-LIO run together (one coordinator). + inner = subprocess.Popen( + [ + *inner_prefix, + sys.executable, + "-m", + "dimos.hardware.sensors.lidar.pointlio.tools.pcap_to_db", + "--inner", + "--db", + str(db_path), + "--odom-freq", + str(args.odom_freq), + "--ref-start-ts", + repr(ref_start_ts), + "--time-offset", + "nan" if args.time_offset is None else repr(args.time_offset), + "--max-sensor-sec", + str(args.max_sensor_sec), + "--host-ip", + args.host_ip, + "--lidar-ip", + args.lidar_ip, + ], + cwd=os.getcwd(), ) - # Recorder + live Point-LIO run together in the drv ns (one coordinator). - inner_cmd = [ - *_sudo(), - "ip", - "netns", - "exec", - args.drv_ns, - sys.executable, - "-m", - "dimos.hardware.sensors.lidar.pointlio.tools.pcap_to_db", - "--inner", - "--db", - str(db_path), - "--odom-freq", - str(args.odom_freq), - "--ref-start-ts", - repr(ref_start_ts), - "--time-offset", - "nan" if args.time_offset is None else repr(args.time_offset), - "--max-sensor-sec", - str(args.max_sensor_sec), - "--host-ip", - args.host_ip, - "--lidar-ip", - args.lidar_ip, - ] - inner = subprocess.Popen(inner_cmd, cwd=os.getcwd()) - # Give Point-LIO a moment to come up before the sensor starts streaming. time.sleep(args.warmup_sec) vm_cfg = json.dumps( @@ -383,10 +423,7 @@ def _run_outer(args: argparse.Namespace) -> int: }, } ).encode() - vm_proc = subprocess.Popen( - [*_sudo(), "ip", "netns", "exec", args.lidar_ns, vm_bin], - stdin=subprocess.PIPE, - ) + vm_proc = subprocess.Popen([*vm_prefix, vm_bin], stdin=subprocess.PIPE) assert vm_proc.stdin is not None vm_proc.stdin.write(vm_cfg) vm_proc.stdin.close() @@ -394,31 +431,36 @@ def _run_outer(args: argparse.Namespace) -> int: # The inner exits itself once the odom stream goes stagnant (pcap drained). inner.wait() finally: - # Kill ONLY this run's processes — the ones living in its two (uniquely - # named) network namespaces — as root, since the binaries run under sudo. - # `ip netns pids` scopes precisely to this run, so a concurrent run in - # other namespaces (which the docstring promises is supported) is left - # alone; a name-based `pkill virtual_mid360/pointlio_native` would kill - # its binaries too. This also catches the netns children regardless of - # how sudo / ip-netns-exec session or group them. - for ns in (args.lidar_ns, args.drv_ns): - pids = _ns(["ip", "netns", "pids", ns], check=False).stdout.decode().split() - if pids: - _ns(["kill", "-9", *pids], check=False) + if use_netns: + # Kill ONLY this run's processes — those in its (uniquely named) + # namespaces — as root (the binaries run under sudo). `ip netns pids` + # scopes precisely, so a concurrent run elsewhere is untouched. + for ns in (args.lidar_ns, args.drv_ns): + pids = _ns(["ip", "netns", "pids", ns], check=False).stdout.decode().split() + if pids: + _ns(["kill", "-9", *pids], check=False) + else: + # Alias mode: our own user-owned children — signal them directly + # (pointlio_native dies with its parent via PR_SET_PDEATHSIG). + for proc in (vm_proc, inner): + if proc and proc.poll() is None: + proc.terminate() for proc in (vm_proc, inner): if proc and proc.poll() is None: try: proc.wait(timeout=5) except subprocess.TimeoutExpired: proc.kill() - _teardown(args.drv_ns, args.lidar_ns, args.veth_drv) - # The inner coordinator ran as root (netns entry needs it) → hand the db - # files back to the invoking user. - uid, gid = os.getuid(), os.getgid() - for suffix in ("", "-wal", "-shm"): - sidecar = Path(f"{db_path}{suffix}") - if sidecar.exists(): - _ns(["chown", f"{uid}:{gid}", str(sidecar)], check=False) + if use_netns: + _teardown(args.drv_ns, args.lidar_ns, args.veth_drv) + # Root inner owned the db files → hand them back to the invoking user. + uid, gid = os.getuid(), os.getgid() + for suffix in ("", "-wal", "-shm"): + sidecar = Path(f"{db_path}{suffix}") + if sidecar.exists(): + _ns(["chown", f"{uid}:{gid}", str(sidecar)], check=False) + else: + _teardown_aliases(args.alias_iface) o_cnt, o_min, o_max = _table_stats(db_path, "pointlio_odometry") l_cnt = _table_stats(db_path, "pointlio_lidar")[0] @@ -538,13 +580,19 @@ def main(argv: list[str]) -> int: parser.add_argument( "--warmup-sec", type=float, default=4.0, help="wait before streaming starts" ) - # Namespace / addressing knobs (override to run two replays at once). - parser.add_argument("--drv-ns", default="drv") - parser.add_argument("--lidar-ns", default="lidar") - parser.add_argument("--veth-drv", default="veth-drv") - parser.add_argument("--veth-lidar", default="veth-lidar") + # Addressing knobs (override to run two replays at once). parser.add_argument("--host-ip", default="192.168.1.5") parser.add_argument("--lidar-ip", default="192.168.1.155") + # netns is opt-in. Default (empty --lidar-ns) aliases both IPs on a dummy + # interface and runs in the host namespace; pass --lidar-ns/--drv-ns to + # isolate the two ends in separate namespaces instead. + parser.add_argument("--drv-ns", default="") + parser.add_argument("--lidar-ns", default="") + parser.add_argument("--veth-drv", default="veth-drv") + parser.add_argument("--veth-lidar", default="veth-lidar") + parser.add_argument( + "--alias-iface", default="dimos-mid360", help="dummy iface for no-netns mode" + ) # Internal: re-exec inside the drv netns to run the coordinator. parser.add_argument("--inner", action="store_true", help=argparse.SUPPRESS) parser.add_argument("--ref-start-ts", type=float, default=-1.0, help=argparse.SUPPRESS) From 460168c5e0edc489c46fe7b700b838f5393c8217 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 01:49:32 -0700 Subject: [PATCH 102/185] pointlio: human-readable filter config + raw_cloud bypass; expose pre-KF downsample; C++ style - Rename output-cloud filter config sor_mean_k/sor_stddev -> outlier_neighbor_count/ outlier_std_threshold; add raw_cloud bool that bypasses voxel+outlier entirely; voxel_size<=0 now skips downsampling. - Document the pre-KF (input) downsampling in default.yaml as distinct + how to disable (point_filter_num=1, space_down_sample=false). - main.cpp: unwrap the multi-line fn signatures; brace every if/for body. --- .../lidar/pointlio/config/default.yaml | 5 +- .../lidar/pointlio/cpp/cloud_filter.hpp | 37 ++++++------ .../sensors/lidar/pointlio/cpp/main.cpp | 57 ++++++++++--------- .../hardware/sensors/lidar/pointlio/module.py | 11 +++- 4 files changed, 63 insertions(+), 47 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/config/default.yaml b/dimos/hardware/sensors/lidar/pointlio/config/default.yaml index 5c27305e5c..ee307ebdb5 100644 --- a/dimos/hardware/sensors/lidar/pointlio/config/default.yaml +++ b/dimos/hardware/sensors/lidar/pointlio/config/default.yaml @@ -19,6 +19,7 @@ preprocess: scan_rate: 10 timestamp_unit: 3 # 3 = nanosecond blind: 0.5 + # Pre-KF input decimation: keep every Nth raw point. 1 = keep all (disable). point_filter_num: 3 mapping: @@ -26,12 +27,14 @@ mapping: prop_at_freq_of_imu: true check_satu: true init_map_size: 10 + # Pre-KF voxel downsample of each scan before the filter. false = feed the + # full scan (disable). Leaf size is filter_size_surf below. space_down_sample: true satu_acc: 3.0 # g; accel >= this is treated as saturated (residual zeroed) to bound velocity satu_gyro: 35.0 acc_norm: 1.0 # IMU accel unit: g plane_thr: 0.1 - filter_size_surf: 0.5 + filter_size_surf: 0.5 # pre-KF scan downsample leaf size (m), used iff space_down_sample filter_size_map: 0.5 ivox_grid_resolution: 2.0 # iVox local-map grid (m) ivox_nearby_type: 6 # NEARBY6 diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/cloud_filter.hpp b/dimos/hardware/sensors/lidar/pointlio/cpp/cloud_filter.hpp index b6973ac5b8..5d6a2f1673 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/cloud_filter.hpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/cloud_filter.hpp @@ -13,37 +13,42 @@ #include struct CloudFilterConfig { - float voxel_size = 0.1f; - int sor_mean_k = 50; - float sor_stddev = 1.0f; + float voxel_size = 0.1f; // downsample leaf size (m); <=0 skips downsampling + int outlier_neighbor_count = 50; // statistical-outlier-removal neighbors; <=0 skips it + float outlier_std_threshold = 1.0f; }; -/// Apply voxel grid downsample + statistical outlier removal in-place. -/// Returns the filtered cloud (new allocation). +/// Voxel-grid downsample + statistical outlier removal; each stage is skipped +/// when its config is <=0. Returns the filtered cloud (new allocation). template typename pcl::PointCloud::Ptr filter_cloud( const typename pcl::PointCloud::Ptr& input, const CloudFilterConfig& cfg) { - if (!input || input->empty()) return input; + if (!input || input->empty()) { return input; } - typename pcl::PointCloud::Ptr voxelized(new pcl::PointCloud()); - pcl::VoxelGrid vg; - vg.setInputCloud(input); - vg.setLeafSize(cfg.voxel_size, cfg.voxel_size, cfg.voxel_size); - vg.filter(*voxelized); + auto working = input; + if (cfg.voxel_size > 0.0f) { + typename pcl::PointCloud::Ptr voxelized(new pcl::PointCloud()); + pcl::VoxelGrid vg; + vg.setInputCloud(working); + vg.setLeafSize(cfg.voxel_size, cfg.voxel_size, cfg.voxel_size); + vg.filter(*voxelized); + working = voxelized; + } - if (cfg.sor_mean_k > 0 && voxelized->size() > static_cast(cfg.sor_mean_k)) { + if (cfg.outlier_neighbor_count > 0 && + working->size() > static_cast(cfg.outlier_neighbor_count)) { typename pcl::PointCloud::Ptr cleaned(new pcl::PointCloud()); pcl::StatisticalOutlierRemoval sor; - sor.setInputCloud(voxelized); - sor.setMeanK(cfg.sor_mean_k); - sor.setStddevMulThresh(cfg.sor_stddev); + sor.setInputCloud(working); + sor.setMeanK(cfg.outlier_neighbor_count); + sor.setStddevMulThresh(cfg.outlier_std_threshold); sor.filter(*cleaned); return cleaned; } - return voxelized; + return working; } #endif diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 99df7e958e..a845a45779 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -83,10 +83,9 @@ using dimos::make_header; // Publish the lidar point cloud in the sensor body frame (g_frame_id). // `cloud` is Point-LIO's undistorted scan in the sensor's own frame // (get_body_cloud), so points are published as-is with no world registration. -static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, - const std::string& topic = "") { +static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, const std::string& topic = "") { const std::string& chan = topic.empty() ? g_lidar_topic : topic; - if (!g_lcm || !cloud || cloud->empty() || chan.empty()) return; + if (!g_lcm || !cloud || cloud->empty() || chan.empty()) { return; } int num_points = static_cast(cloud->size()); @@ -132,7 +131,7 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, } static void publish_odometry(const custom_messages::Odometry& odom, double timestamp) { - if (!g_lcm) return; + if (!g_lcm) { return; } nav_msgs::Odometry msg; msg.header = make_header(g_frame_id, timestamp); @@ -164,9 +163,8 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times } -static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/, - LivoxLidarEthernetPacket* data, void* /*client_data*/) { - if (!g_running.load() || data == nullptr) return; +static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/, LivoxLidarEthernetPacket* data, void* /*client_data*/) { + if (!g_running.load() || data == nullptr) { return; } uint64_t ts_ns = get_timestamp_ns(data); uint16_t dot_num = data->dot_num; @@ -215,9 +213,8 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ } } -static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, - LivoxLidarEthernetPacket* data, void* /*client_data*/) { - if (!g_running.load() || data == nullptr || !g_point_lio) return; +static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, LivoxLidarEthernetPacket* data, void* /*client_data*/) { + if (!g_running.load() || data == nullptr || !g_point_lio) { return; } uint64_t pkt_ts_ns = get_timestamp_ns(data); // Live IMU-drop instrumentation: a dropped datagram shows as a sensor-ts @@ -269,14 +266,16 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, imu_msg->orientation.y = 0.0; imu_msg->orientation.z = 0.0; imu_msg->orientation.w = 1.0; - for (int cov_idx = 0; cov_idx < 9; ++cov_idx) + for (int cov_idx = 0; cov_idx < 9; ++cov_idx) { imu_msg->orientation_covariance[cov_idx] = 0.0; + } imu_msg->angular_velocity.x = static_cast(imu_pts[point_idx].gyro_x); imu_msg->angular_velocity.y = static_cast(imu_pts[point_idx].gyro_y); imu_msg->angular_velocity.z = static_cast(imu_pts[point_idx].gyro_z); - for (int cov_idx = 0; cov_idx < 9; ++cov_idx) + for (int cov_idx = 0; cov_idx < 9; ++cov_idx) { imu_msg->angular_velocity_covariance[cov_idx] = 0.0; + } // Point-LIO expects accel in g (EKF does its own scaling). SDK already // reports g, so feed raw — scaling by GRAVITY_MS2 would double-scale and @@ -284,16 +283,16 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, imu_msg->linear_acceleration.x = static_cast(imu_pts[point_idx].acc_x); imu_msg->linear_acceleration.y = static_cast(imu_pts[point_idx].acc_y); imu_msg->linear_acceleration.z = static_cast(imu_pts[point_idx].acc_z); - for (int cov_idx = 0; cov_idx < 9; ++cov_idx) + for (int cov_idx = 0; cov_idx < 9; ++cov_idx) { imu_msg->linear_acceleration_covariance[cov_idx] = 0.0; + } g_point_lio->feed_imu(imu_msg); } } -static void on_info_change(const uint32_t handle, const LivoxLidarInfo* info, - void* /*client_data*/) { - if (info == nullptr) return; +static void on_info_change(const uint32_t handle, const LivoxLidarInfo* info, void* /*client_data*/) { + if (info == nullptr) { return; } char sn[17] = {}; std::memcpy(sn, info->sn, 16); @@ -342,10 +341,11 @@ int main(int argc, char** argv) { g_child_frame_id = mod.arg_required("body_frame_id"); float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f); float odom_freq = mod.arg_float("odom_freq", 50.0f); + bool raw_cloud = mod.arg_bool("raw_cloud", false); CloudFilterConfig filter_cfg; filter_cfg.voxel_size = mod.arg_float("voxel_size", 0.1f); - filter_cfg.sor_mean_k = mod.arg_int("sor_mean_k", 50); - filter_cfg.sor_stddev = mod.arg_float("sor_stddev", 1.0f); + filter_cfg.outlier_neighbor_count = mod.arg_int("outlier_neighbor_count", 50); + filter_cfg.outlier_std_threshold = mod.arg_float("outlier_std_threshold", 1.0f); // Propagates to the Point-LIO core via the `fastlio_debug` global. bool debug = mod.arg_bool("debug", false); @@ -376,8 +376,10 @@ int main(int argc, char** argv) { host_ip.c_str(), lidar_ip.c_str(), g_frequency); printf("[pointlio] pointcloud_freq: %.1f Hz odom_freq: %.1f Hz\n", pointcloud_freq, odom_freq); - printf("[pointlio] voxel_size: %.3f sor_mean_k: %d sor_stddev: %.1f\n", - filter_cfg.voxel_size, filter_cfg.sor_mean_k, filter_cfg.sor_stddev); + printf("[pointlio] output cloud: raw=%d voxel_size=%.3f outlier_neighbors=%d " + "outlier_std=%.1f\n", + raw_cloud, filter_cfg.voxel_size, filter_cfg.outlier_neighbor_count, + filter_cfg.outlier_std_threshold); } signal(SIGTERM, signal_handler); @@ -390,10 +392,10 @@ int main(int argc, char** argv) { } g_lcm = &lcm; - if (debug) printf("[pointlio] Initializing Point-LIO...\n"); + if (debug) { printf("[pointlio] Initializing Point-LIO...\n"); } PointLio point_lio(config_path, msr_freq, main_freq); g_point_lio = &point_lio; - if (debug) printf("[pointlio] Point-LIO initialized.\n"); + if (debug) { printf("[pointlio] Point-LIO initialized.\n"); } // Main-loop state. Body lives in `run_main_iter`, driven by the wall-paced // main thread. @@ -448,7 +450,7 @@ int main(int argc, char** argv) { lidar_msg->header.frame_id = "livox_frame"; lidar_msg->timebase = frame_start; lidar_msg->lidar_id = 0; - for (int idx = 0; idx < 3; idx++) lidar_msg->rsvd[idx] = 0; + for (int idx = 0; idx < 3; idx++) { lidar_msg->rsvd[idx] = 0; } lidar_msg->point_num = static_cast(num_points); lidar_msg->points = std::move(points); if (fastlio_debug) { @@ -472,7 +474,8 @@ int main(int argc, char** argv) { if (lidar_due) { auto body_cloud = point_lio.get_body_cloud(); if (body_cloud && !body_cloud->empty()) { - auto filtered = filter_cloud(body_cloud, filter_cfg); + auto filtered = raw_cloud ? body_cloud + : filter_cloud(body_cloud, filter_cfg); publish_lidar(filtered, ts); last_pc_publish = now; if (fastlio_debug) { @@ -508,7 +511,7 @@ int main(int argc, char** argv) { LivoxLidarSdkUninit(); return 1; } - if (debug) printf("[pointlio] SDK started, waiting for device...\n"); + if (debug) { printf("[pointlio] SDK started, waiting for device...\n"); } while (g_running.load()) { auto loop_start = std::chrono::high_resolution_clock::now(); @@ -525,11 +528,11 @@ int main(int argc, char** argv) { } } - if (debug) printf("[pointlio] Shutting down...\n"); + if (debug) { printf("[pointlio] Shutting down...\n"); } g_point_lio = nullptr; LivoxLidarSdkUninit(); g_lcm = nullptr; - if (debug) printf("[pointlio] Done.\n"); + if (debug) { printf("[pointlio] Done.\n"); } return 0; } diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index aea91b9aec..fd8da8022b 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -97,10 +97,15 @@ class PointLioConfig(NativeModuleConfig): pointcloud_freq: float = 10.0 odom_freq: float = 30.0 - # Point cloud filtering + # Published-cloud cleanup. raw_cloud=True publishes the unfiltered scan + # (skips both downsample and outlier removal). + raw_cloud: bool = False + # Voxel-grid downsample leaf size (m); 0 keeps full resolution. voxel_size: float = 0.1 - sor_mean_k: int = 50 - sor_stddev: float = 1.0 + # Statistical outlier removal: neighbors examined per point (0 disables it) + # and how many std-devs from the mean neighbor distance is kept. + outlier_neighbor_count: int = 50 + outlier_std_threshold: float = 1.0 # Point-LIO YAML config (relative to config/ dir, or absolute path). config: Annotated[ From 3a8fc5596f0c723de138ba498906dff40a99b955 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 02:07:58 -0700 Subject: [PATCH 103/185] pointlio cpp: unwrap multi-line printf/statements onto single lines --- .../sensors/lidar/pointlio/cpp/main.cpp | 83 ++++++------------- 1 file changed, 25 insertions(+), 58 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index a845a45779..ccb6f59543 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -55,15 +55,12 @@ static lcm::LCM* g_lcm = nullptr; static PointLio* g_point_lio = nullptr; static double get_publish_ts() { - return std::chrono::duration( - std::chrono::system_clock::now().time_since_epoch()).count(); + return std::chrono::duration( std::chrono::system_clock::now().time_since_epoch()).count(); } static std::string g_lidar_topic; static std::string g_odometry_topic; -static std::string g_frame_id; // required via --frame_id -static std::string g_child_frame_id; // required via --child_frame_id -static float g_frequency = 10.0f; +static std::string g_frame_id; // required via --frame_id static std::string g_child_frame_id; // required via --child_frame_id static float g_frequency = 10.0f; // Frame accumulator (Livox SDK raw → CustomMsg) static std::mutex g_pc_mutex; @@ -172,8 +169,7 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ // Per-point intra-packet offset (matches livox_ros_driver2). Without it all // points share one timestamp and per-point deskew is lost. time_interval // unit is 0.1us, so *100 → ns. - const uint64_t point_interval_ns = - dot_num > 0 ? static_cast(data->time_interval) * 100 / dot_num : 0; + const uint64_t point_interval_ns = dot_num > 0 ? static_cast(data->time_interval) * 100 / dot_num : 0; std::lock_guard lock(g_pc_mutex); @@ -186,28 +182,23 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ auto* pts = reinterpret_cast(data->data); for (uint16_t point_idx = 0; point_idx < dot_num; ++point_idx) { custom_messages::CustomPoint cp; - cp.x = static_cast(pts[point_idx].x) / 1000.0; // mm → m - cp.y = static_cast(pts[point_idx].y) / 1000.0; + cp.x = static_cast(pts[point_idx].x) / 1000.0; // mm → m cp.y = static_cast(pts[point_idx].y) / 1000.0; cp.z = static_cast(pts[point_idx].z) / 1000.0; cp.reflectivity = pts[point_idx].reflectivity; cp.tag = pts[point_idx].tag; - cp.line = 0; // Mid-360: single line - cp.offset_time = - static_cast((ts_ns - g_frame_start_ns) + point_idx * point_interval_ns); + cp.line = 0; // Mid-360: single line cp.offset_time = static_cast((ts_ns - g_frame_start_ns) + point_idx * point_interval_ns); g_accumulated_points.push_back(cp); } } else if (data->data_type == DATA_TYPE_CARTESIAN_LOW) { auto* pts = reinterpret_cast(data->data); for (uint16_t point_idx = 0; point_idx < dot_num; ++point_idx) { custom_messages::CustomPoint cp; - cp.x = static_cast(pts[point_idx].x) / 100.0; // cm → m - cp.y = static_cast(pts[point_idx].y) / 100.0; + cp.x = static_cast(pts[point_idx].x) / 100.0; // cm → m cp.y = static_cast(pts[point_idx].y) / 100.0; cp.z = static_cast(pts[point_idx].z) / 100.0; cp.reflectivity = pts[point_idx].reflectivity; cp.tag = pts[point_idx].tag; cp.line = 0; - cp.offset_time = - static_cast((ts_ns - g_frame_start_ns) + point_idx * point_interval_ns); + cp.offset_time = static_cast((ts_ns - g_frame_start_ns) + point_idx * point_interval_ns); g_accumulated_points.push_back(cp); } } @@ -231,24 +222,18 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, L uint64_t pkt_count = imu_pkt_count.fetch_add(1) + 1; if (prev != 0 && pkt_ts_ns > prev) { uint64_t sensor_gap_us = (pkt_ts_ns - prev) / 1000; - uint64_t wall_gap_us = std::chrono::duration_cast( - now_wall - last_wall).count(); + uint64_t wall_gap_us = std::chrono::duration_cast( now_wall - last_wall).count(); uint64_t cur_max = max_sensor_gap_us.load(); while (sensor_gap_us > cur_max && !max_sensor_gap_us.compare_exchange_weak(cur_max, sensor_gap_us)) {} if (sensor_gap_us > 15000) { imu_gap_count.fetch_add(1); - fprintf(stderr, "[imu-gap] sensor_gap=%.1fms wall_gap=%.1fms pkt#%llu\n", - sensor_gap_us / 1000.0, wall_gap_us / 1000.0, - static_cast(pkt_count)); + fprintf(stderr, "[imu-gap] sensor_gap=%.1fms wall_gap=%.1fms pkt#%llu\n", sensor_gap_us / 1000.0, wall_gap_us / 1000.0, static_cast(pkt_count)); } } last_wall = now_wall; if (pkt_count % 1000 == 0) { - fprintf(stderr, "[imu-stats] pkts=%llu gaps>15ms=%llu max_sensor_gap=%.1fms\n", - static_cast(pkt_count), - static_cast(imu_gap_count.load()), - max_sensor_gap_us.load() / 1000.0); + fprintf(stderr, "[imu-stats] pkts=%llu gaps>15ms=%llu max_sensor_gap=%.1fms\n", static_cast(pkt_count), static_cast(imu_gap_count.load()), max_sensor_gap_us.load() / 1000.0); } } @@ -300,8 +285,7 @@ static void on_info_change(const uint32_t handle, const LivoxLidarInfo* info, vo std::memcpy(ip, info->lidar_ip, 16); if (fastlio_debug) { - printf("[pointlio] Device connected: handle=%u type=%u sn=%s ip=%s\n", - handle, info->dev_type, sn, ip); + printf("[pointlio] Device connected: handle=%u type=%u sn=%s ip=%s\n", handle, info->dev_type, sn, ip); } SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, nullptr, nullptr); @@ -367,19 +351,12 @@ int main(int argc, char** argv) { if (debug) { printf("[pointlio] Starting Point-LIO + Livox Mid-360 native module\n"); - printf("[pointlio] lidar topic: %s\n", - g_lidar_topic.empty() ? "(disabled)" : g_lidar_topic.c_str()); - printf("[pointlio] odometry topic: %s\n", - g_odometry_topic.empty() ? "(disabled)" : g_odometry_topic.c_str()); + printf("[pointlio] lidar topic: %s\n", g_lidar_topic.empty() ? "(disabled)" : g_lidar_topic.c_str()); + printf("[pointlio] odometry topic: %s\n", g_odometry_topic.empty() ? "(disabled)" : g_odometry_topic.c_str()); printf("[pointlio] config: %s\n", config_path.c_str()); - printf("[pointlio] host_ip: %s lidar_ip: %s frequency: %.1f Hz\n", - host_ip.c_str(), lidar_ip.c_str(), g_frequency); - printf("[pointlio] pointcloud_freq: %.1f Hz odom_freq: %.1f Hz\n", - pointcloud_freq, odom_freq); - printf("[pointlio] output cloud: raw=%d voxel_size=%.3f outlier_neighbors=%d " - "outlier_std=%.1f\n", - raw_cloud, filter_cfg.voxel_size, filter_cfg.outlier_neighbor_count, - filter_cfg.outlier_std_threshold); + printf("[pointlio] host_ip: %s lidar_ip: %s frequency: %.1f Hz\n", host_ip.c_str(), lidar_ip.c_str(), g_frequency); + printf("[pointlio] pointcloud_freq: %.1f Hz odom_freq: %.1f Hz\n", pointcloud_freq, odom_freq); + printf("[pointlio] output cloud: raw=%d voxel_size=%.3f outlier_neighbors=%d " "outlier_std=%.1f\n", raw_cloud, filter_cfg.voxel_size, filter_cfg.outlier_neighbor_count, filter_cfg.outlier_std_threshold); } signal(SIGTERM, signal_handler); @@ -399,15 +376,12 @@ int main(int argc, char** argv) { // Main-loop state. Body lives in `run_main_iter`, driven by the wall-paced // main thread. - auto frame_interval = std::chrono::microseconds( - static_cast(1e6 / g_frequency)); + auto frame_interval = std::chrono::microseconds( static_cast(1e6 / g_frequency)); std::optional last_emit; const double process_period_ms = 1000.0 / main_freq; - auto pc_interval = std::chrono::microseconds( - static_cast(1e6 / pointcloud_freq)); - auto odom_interval = std::chrono::microseconds( - static_cast(1e6 / odom_freq)); + auto pc_interval = std::chrono::microseconds( static_cast(1e6 / pointcloud_freq)); + auto odom_interval = std::chrono::microseconds( static_cast(1e6 / odom_freq)); std::optional last_pc_publish; std::optional last_odom_publish; @@ -445,8 +419,7 @@ int main(int argc, char** argv) { const size_t num_points = points.size(); auto lidar_msg = boost::make_shared(); lidar_msg->header.seq = 0; - lidar_msg->header.stamp = custom_messages::Time().fromSec( - static_cast(frame_start) / 1e9); + lidar_msg->header.stamp = custom_messages::Time().fromSec( static_cast(frame_start) / 1e9); lidar_msg->header.frame_id = "livox_frame"; lidar_msg->timebase = frame_start; lidar_msg->lidar_id = 0; @@ -466,22 +439,18 @@ int main(int argc, char** argv) { if (!pose.empty() && (pose[0] != 0.0 || pose[1] != 0.0 || pose[2] != 0.0)) { double ts = get_publish_ts(); - const bool lidar_due = - !g_lidar_topic.empty() && now - *last_pc_publish >= pc_interval; + const bool lidar_due = !g_lidar_topic.empty() && now - *last_pc_publish >= pc_interval; // get_body_cloud + filter_cloud (SOR) is the loop's costliest step, // so build it only when a publish is due. if (lidar_due) { auto body_cloud = point_lio.get_body_cloud(); if (body_cloud && !body_cloud->empty()) { - auto filtered = raw_cloud ? body_cloud - : filter_cloud(body_cloud, filter_cfg); + auto filtered = raw_cloud ? body_cloud : filter_cloud(body_cloud, filter_cfg); publish_lidar(filtered, ts); last_pc_publish = now; if (fastlio_debug) { - fprintf(stderr, - "[pointlio] publish lidar: %zu points pose=(%.3f, %.3f, %.3f)\n", - filtered ? filtered->size() : 0, pose[0], pose[1], pose[2]); + fprintf(stderr, "[pointlio] publish lidar: %zu points pose=(%.3f, %.3f, %.3f)\n", filtered ? filtered->size() : 0, pose[0], pose[1], pose[2]); } } } @@ -491,8 +460,7 @@ int main(int argc, char** argv) { publish_odometry(point_lio.get_odometry(), ts); last_odom_publish = now; if (fastlio_debug) { - fprintf(stderr, "[pointlio] publish odom: pose=(%.3f, %.3f, %.3f)\n", - pose[0], pose[1], pose[2]); + fprintf(stderr, "[pointlio] publish odom: pose=(%.3f, %.3f, %.3f)\n", pose[0], pose[1], pose[2]); } } } @@ -523,8 +491,7 @@ int main(int argc, char** argv) { auto loop_end = std::chrono::high_resolution_clock::now(); auto elapsed_ms = std::chrono::duration(loop_end - loop_start).count(); if (elapsed_ms < process_period_ms) { - std::this_thread::sleep_for(std::chrono::microseconds( - static_cast((process_period_ms - elapsed_ms) * 1000))); + std::this_thread::sleep_for(std::chrono::microseconds( static_cast((process_period_ms - elapsed_ms) * 1000))); } } From e2a4d85f602ae2dcf392184629513ec0c966b843 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 02:21:43 -0700 Subject: [PATCH 104/185] pointlio: drop last fastlio names (pointlio_debug + pointlio.hpp headers); filter_size_surf 0.5->0.2 - main.cpp: fastlio_debug->pointlio_debug, include fast_lio.hpp/fast_lio_debug.hpp -> pointlio.hpp/pointlio_debug.hpp; flake bumped to dimos-module-fastlio2@286c530 (the matching upstream rename). - filter_size_surf 0.5->0.2 (denser pre-KF scan). Verified bounded over 3 replays of ruwik2_part3: max|pos| 9.59 / 9.54 / 9.32 m (was ~15m at 0.5). --- .../sensors/lidar/pointlio/config/default.yaml | 2 +- .../sensors/lidar/pointlio/cpp/flake.lock | 6 +++--- .../hardware/sensors/lidar/pointlio/cpp/main.cpp | 16 ++++++++-------- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/config/default.yaml b/dimos/hardware/sensors/lidar/pointlio/config/default.yaml index ee307ebdb5..1fd09ec9fa 100644 --- a/dimos/hardware/sensors/lidar/pointlio/config/default.yaml +++ b/dimos/hardware/sensors/lidar/pointlio/config/default.yaml @@ -34,7 +34,7 @@ mapping: satu_gyro: 35.0 acc_norm: 1.0 # IMU accel unit: g plane_thr: 0.1 - filter_size_surf: 0.5 # pre-KF scan downsample leaf size (m), used iff space_down_sample + filter_size_surf: 0.2 # pre-KF scan downsample leaf size (m), used iff space_down_sample filter_size_map: 0.5 ivox_grid_resolution: 2.0 # iVox local-map grid (m) ivox_nearby_type: 6 # NEARBY6 diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock index f7d42f90d5..531b9d7a15 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock @@ -37,11 +37,11 @@ "fast-lio": { "flake": false, "locked": { - "lastModified": 1781511489, - "narHash": "sha256-iRjpHNdGVPXXVSssYcOV1crSJ1IJ5k0EIiGSzXuXGtU=", + "lastModified": 1781514593, + "narHash": "sha256-85zc03F2Ztw/pO1nUtVHnC+4yGzVi5z9pKk9lZcFnE8=", "owner": "dimensionalOS", "repo": "dimos-module-fastlio2", - "rev": "46b1a9e48f749076b8137b51d372a1dcf4afa73c", + "rev": "286c530db7661ca6874cd8c1381357adf83cd19f", "type": "github" }, "original": { diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index ccb6f59543..5d3310a68a 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -42,8 +42,8 @@ #include "sensor_msgs/PointField.hpp" // Point-LIO (header-only core, compiled sources linked via CMake) -#include "fast_lio.hpp" -#include "fast_lio_debug.hpp" +#include "pointlio.hpp" +#include "pointlio_debug.hpp" using livox_common::GRAVITY_MS2; using livox_common::DATA_TYPE_IMU; @@ -284,7 +284,7 @@ static void on_info_change(const uint32_t handle, const LivoxLidarInfo* info, vo char ip[17] = {}; std::memcpy(ip, info->lidar_ip, 16); - if (fastlio_debug) { + if (pointlio_debug) { printf("[pointlio] Device connected: handle=%u type=%u sn=%s ip=%s\n", handle, info->dev_type, sn, ip); } @@ -331,9 +331,9 @@ int main(int argc, char** argv) { filter_cfg.outlier_neighbor_count = mod.arg_int("outlier_neighbor_count", 50); filter_cfg.outlier_std_threshold = mod.arg_float("outlier_std_threshold", 1.0f); - // Propagates to the Point-LIO core via the `fastlio_debug` global. + // Propagates to the Point-LIO core via the `pointlio_debug` global. bool debug = mod.arg_bool("debug", false); - fastlio_debug = debug; + pointlio_debug = debug; // SDK network ports (defaults from SdkPorts struct in livox_sdk_config.hpp) livox_common::SdkPorts ports; @@ -426,7 +426,7 @@ int main(int argc, char** argv) { for (int idx = 0; idx < 3; idx++) { lidar_msg->rsvd[idx] = 0; } lidar_msg->point_num = static_cast(num_points); lidar_msg->points = std::move(points); - if (fastlio_debug) { + if (pointlio_debug) { fprintf(stderr, "[pointlio] feed_lidar frame: %zu points\n", num_points); } point_lio.feed_lidar(lidar_msg); @@ -449,7 +449,7 @@ int main(int argc, char** argv) { auto filtered = raw_cloud ? body_cloud : filter_cloud(body_cloud, filter_cfg); publish_lidar(filtered, ts); last_pc_publish = now; - if (fastlio_debug) { + if (pointlio_debug) { fprintf(stderr, "[pointlio] publish lidar: %zu points pose=(%.3f, %.3f, %.3f)\n", filtered ? filtered->size() : 0, pose[0], pose[1], pose[2]); } } @@ -459,7 +459,7 @@ int main(int argc, char** argv) { if (!g_odometry_topic.empty() && now - *last_odom_publish >= odom_interval) { publish_odometry(point_lio.get_odometry(), ts); last_odom_publish = now; - if (fastlio_debug) { + if (pointlio_debug) { fprintf(stderr, "[pointlio] publish odom: pose=(%.3f, %.3f, %.3f)\n", pose[0], pose[1], pose[2]); } } From 7ee0c31c59a6e9cea5c963c4b8978a63aac23045 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 02:24:44 -0700 Subject: [PATCH 105/185] pointlio module.py: tighten verbose config/comment lines --- .../hardware/sensors/lidar/pointlio/module.py | 27 +++++++------------ 1 file changed, 10 insertions(+), 17 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index fd8da8022b..6b080d556c 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -74,31 +74,26 @@ class PointLioConfig(NativeModuleConfig): cwd: str | None = "cpp" executable: str = "result/bin/pointlio_native" build_command: str | None = "nix build .#pointlio_native" - # lidar_ip is required (network-specific); from the config or - # DIMOS_POINTLIO_LIDAR_IP. host_ip is optional — start() derives the local - # NIC on the lidar's subnet when unset (or via DIMOS_POINTLIO_HOST_IP). + # lidar_ip required; host_ip optional (auto-derived from lidar_ip's subnet). + # Both fall back to DIMOS_POINTLIO_LIDAR_IP / DIMOS_POINTLIO_HOST_IP. host_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_POINTLIO_HOST_IP")) lidar_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_POINTLIO_LIDAR_IP")) frequency: float = 10.0 - # frame_id is the header frame for BOTH the point cloud and the odometry - # message (the Mid-360 sensor frame). The TF published by the module is a - # separate body_start_frame_id -> body_frame_id transform. + # Sensor frame for the cloud + odometry headers. frame_id: str = "mid360_link" - # TF publish frames (body_start -> body): the sensor pose expressed as the - # body_frame pose in the body_start frame. + # Published TF: body_start_frame_id -> body_frame_id. body_start_frame_id: str = FRAME_ODOM body_frame_id: str = "base_link" - # Point-LIO internal processing rates + # Point-LIO internal processing rates (Hz) msr_freq: float = 50.0 main_freq: float = 5000.0 pointcloud_freq: float = 10.0 odom_freq: float = 30.0 - # Published-cloud cleanup. raw_cloud=True publishes the unfiltered scan - # (skips both downsample and outlier removal). + # raw_cloud=True publishes the unfiltered scan (no downsample/outlier removal). raw_cloud: bool = False # Voxel-grid downsample leaf size (m); 0 keeps full resolution. voxel_size: float = 0.1 @@ -171,9 +166,8 @@ def _on_odom_for_tf(self, msg: Odometry) -> None: msg.pose.orientation.z, msg.pose.orientation.w, ), - # Match the odometry message's own timestamp exactly so the TF - # and the pose can't drift apart. No `or time.time()` fallback: a - # real ts of 0.0 must not be silently replaced with wall time. + # Match the odometry ts exactly; no `or time.time()` fallback (a + # real ts of 0.0 must not become wall time). ts=msg.ts, ) ) @@ -191,9 +185,8 @@ def _validate_network(self) -> None: ) local_ips = [ip for ip, _iface in get_local_ips()] - # host_ip is optional — it's just the local NIC facing the lidar. When - # it's unset (or not one of our IPs) derive it as the local IP on the - # lidar's /24. + # host_ip optional: derive the local NIC on lidar_ip's /24 when unset or + # not one of our IPs. configured = self.config.host_ip if configured and configured in local_ips: host_ip = configured From 226a330531d351147221ddcd7f5f357f1704ad19 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 18:04:58 +0800 Subject: [PATCH 106/185] - --- .../hardware/sensors/lidar/pointlio/module.py | 22 +++++-------------- 1 file changed, 5 insertions(+), 17 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index aea91b9aec..684a8ca274 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -14,9 +14,6 @@ """Python NativeModule wrapper for the Point-LIO + Livox Mid-360 binary. -Binds Livox SDK2 directly into Point-LIO for real-time LiDAR SLAM. -Outputs sensor-frame (mid360_link) point clouds and odometry with covariance. - Usage:: from dimos.hardware.sensors.lidar.pointlio.module import PointLio @@ -74,26 +71,14 @@ class PointLioConfig(NativeModuleConfig): cwd: str | None = "cpp" executable: str = "result/bin/pointlio_native" build_command: str | None = "nix build .#pointlio_native" - # lidar_ip is required (network-specific); from the config or - # DIMOS_POINTLIO_LIDAR_IP. host_ip is optional — start() derives the local - # NIC on the lidar's subnet when unset (or via DIMOS_POINTLIO_HOST_IP). host_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_POINTLIO_HOST_IP")) lidar_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_POINTLIO_LIDAR_IP")) frequency: float = 10.0 - # frame_id is the header frame for BOTH the point cloud and the odometry - # message (the Mid-360 sensor frame). The TF published by the module is a - # separate body_start_frame_id -> body_frame_id transform. frame_id: str = "mid360_link" - # TF publish frames (body_start -> body): the sensor pose expressed as the - # body_frame pose in the body_start frame. body_start_frame_id: str = FRAME_ODOM body_frame_id: str = "base_link" - # Point-LIO internal processing rates - msr_freq: float = 50.0 - main_freq: float = 5000.0 - pointcloud_freq: float = 10.0 odom_freq: float = 30.0 @@ -102,7 +87,11 @@ class PointLioConfig(NativeModuleConfig): sor_mean_k: int = 50 sor_stddev: float = 1.0 - # Point-LIO YAML config (relative to config/ dir, or absolute path). + # Point-LIO internal processing rates + msr_freq: float = 50.0 + main_freq: float = 5000.0 + + # relative to config/ dir, or absolute path config: Annotated[ Path, validate_as(...).transform(lambda path: path if path.is_absolute() else _CONFIG_DIR / path), @@ -128,7 +117,6 @@ class PointLioConfig(NativeModuleConfig): cli_exclude: frozenset[str] = frozenset({"config", "body_start_frame_id"}) def model_post_init(self, __context: object) -> None: - """Resolve the Point-LIO YAML config to an absolute config_path.""" super().model_post_init(__context) cfg = self.config if not cfg.is_absolute(): From 1e7bf28cf3c6cc5dd3f9740a29be293adcb4394f Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 03:10:28 -0700 Subject: [PATCH 107/185] pointlio cpp: fix statements swallowed into trailing // comments (P0) The earlier printf-unwrap pass collapsed lines that ended in a trailing comment, eating the next statement into the comment: cp.y (both CARTESIAN paths), cp.offset_time (HIGH path), and the g_child_frame_id/g_frequency globals were all silently dropped. Split them back onto their own lines so they execute again. --- dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 5d3310a68a..33b6e6599e 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -60,7 +60,9 @@ static double get_publish_ts() { static std::string g_lidar_topic; static std::string g_odometry_topic; -static std::string g_frame_id; // required via --frame_id static std::string g_child_frame_id; // required via --child_frame_id static float g_frequency = 10.0f; +static std::string g_frame_id; // required via --frame_id +static std::string g_child_frame_id; // required via --child_frame_id +static float g_frequency = 10.0f; // Frame accumulator (Livox SDK raw → CustomMsg) static std::mutex g_pc_mutex; @@ -182,18 +184,21 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ auto* pts = reinterpret_cast(data->data); for (uint16_t point_idx = 0; point_idx < dot_num; ++point_idx) { custom_messages::CustomPoint cp; - cp.x = static_cast(pts[point_idx].x) / 1000.0; // mm → m cp.y = static_cast(pts[point_idx].y) / 1000.0; + cp.x = static_cast(pts[point_idx].x) / 1000.0; // mm → m + cp.y = static_cast(pts[point_idx].y) / 1000.0; cp.z = static_cast(pts[point_idx].z) / 1000.0; cp.reflectivity = pts[point_idx].reflectivity; cp.tag = pts[point_idx].tag; - cp.line = 0; // Mid-360: single line cp.offset_time = static_cast((ts_ns - g_frame_start_ns) + point_idx * point_interval_ns); + cp.line = 0; // Mid-360: single line + cp.offset_time = static_cast((ts_ns - g_frame_start_ns) + point_idx * point_interval_ns); g_accumulated_points.push_back(cp); } } else if (data->data_type == DATA_TYPE_CARTESIAN_LOW) { auto* pts = reinterpret_cast(data->data); for (uint16_t point_idx = 0; point_idx < dot_num; ++point_idx) { custom_messages::CustomPoint cp; - cp.x = static_cast(pts[point_idx].x) / 100.0; // cm → m cp.y = static_cast(pts[point_idx].y) / 100.0; + cp.x = static_cast(pts[point_idx].x) / 100.0; // cm → m + cp.y = static_cast(pts[point_idx].y) / 100.0; cp.z = static_cast(pts[point_idx].z) / 100.0; cp.reflectivity = pts[point_idx].reflectivity; cp.tag = pts[point_idx].tag; From c4fa47b54d1c0343789869069f730e9fbf845d17 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 03:12:28 -0700 Subject: [PATCH 108/185] pointlio cpp: uninit Livox SDK before clearing globals (fix shutdown null-deref race) LivoxLidarSdkUninit() now runs before g_point_lio/g_lcm are nulled, so the SDK callback threads (on_imu_data/on_point_cloud) are stopped + joined first and can't race the assignment into a null dereference. --- dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 33b6e6599e..63e01f2e83 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -501,8 +501,11 @@ int main(int argc, char** argv) { } if (debug) { printf("[pointlio] Shutting down...\n"); } - g_point_lio = nullptr; + // Uninit (stops + joins the SDK callback threads) BEFORE clearing the + // pointers those callbacks read, so an in-flight on_imu_data/on_point_cloud + // can't race the assignment and dereference a null g_point_lio / g_lcm. LivoxLidarSdkUninit(); + g_point_lio = nullptr; g_lcm = nullptr; if (debug) { printf("[pointlio] Done.\n"); } From c35dd765be03fda1c9cb22241028b004a16aa75b Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 04:33:03 -0700 Subject: [PATCH 109/185] virtual_mid360: pull pointlio's latest (env-var config defaults, naming/comment cleanup) Grab pointlio's newest virtual_mid360: Config now defaults pcap/lidar_ip/host_ip/ lidar_netns from DIMOS_MID360_* env vars (field names unchanged, so the pcap_to_db orchestrator's explicit JSON still applies), meaningful Rust names, trimmed comments. Kept my flake.lock (pointlio's pins their branch's dimos-repo rev) and my FastLio2 demo blueprint. Rebuilt VM; --pcap interface verified. --- .../lidar/livox/virtual_mid360/Cargo.toml | 3 - .../lidar/livox/virtual_mid360/module.py | 32 +- .../lidar/livox/virtual_mid360/src/main.rs | 306 +++++++++--------- 3 files changed, 163 insertions(+), 178 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml b/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml index c01c38cdae..edbfe42b43 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml @@ -3,9 +3,6 @@ name = "virtual-mid360" version = "0.1.0" edition = "2021" -# Fake Livox Mid-360: replays a recorded pcap over a virtual NIC and synthesizes -# the Livox SDK2 control handshake so an unmodified live-mode pointlio ingests it -# as if from a real sensor. Inverse of pointlio's in-process --replay_pcap. [[bin]] name = "virtual_mid360" path = "src/main.rs" diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py index 5ccbda0374..275d010ff5 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py @@ -35,8 +35,11 @@ from __future__ import annotations +import os from typing import TYPE_CHECKING +from pydantic import Field + from dimos.core.native_module import NativeModule, NativeModuleConfig @@ -45,24 +48,21 @@ class VirtualMid360Config(NativeModuleConfig): executable: str = "result/bin/virtual_mid360" build_command: str | None = "nix build .#default" - # Recorded Mid-360 pcap (point/IMU/status UDP). Read fully into RAM. - pcap: str = "" - # Replay-speed multiplier; 1.0 = original inter-packet timing. + # pcap/lidar_ip/host_ip/lidar_netns default from DIMOS_MID360_* env vars so + # blueprints needn't restate them. pcap/lidar_ip/host_ip are required — empty + # makes the binary error. + pcap: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_PCAP", "")) + # Replay speed; 1.0 = original timing. rate: float = 1.0 - # Seconds to wait after start before streaming begins. + # Seconds to wait before streaming begins. delay: float = 0.0 - # IP the fake lidar sends from (must be on this netns's veth). Network- - # specific, so required (no default). - lidar_ip: str - # Host IP the recorded data is delivered to (where the SDK listens). Machine- - # specific, so required (no default). - host_ip: str - # Network namespace the fake lidar runs inside. Deployment-specific, so - # required (no default). - lidar_netns: str - # Multicast group the point/IMU streams are sent to. 224.1.1.5 is the Livox - # default the SDK joins (a genuine Livox default), so it stays defaulted; - # override only to match a consumer with a different multicast_ip. + # IP the fake lidar sends from (on this netns's veth). + lidar_ip: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_LIDAR_IP", "")) + # Host IP the data is delivered to (where the SDK listens). + host_ip: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_HOST_IP", "")) + # Network namespace the fake lidar runs in. + lidar_netns: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_NETNS", "lidar")) + # Multicast group for point/IMU. 224.1.1.5 is the Livox default the SDK joins. mcast_data: str = "224.1.1.5" diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs index 0e7ed80505..7a53eaab2d 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs @@ -17,7 +17,7 @@ const SOF: u8 = 0xAA; const WRAPPER: usize = 24; // bytes before data[] const CMD_PORT: u16 = 56100; const DISCOVERY_PORT: u16 = 56000; -// data plane: lidar src port -> host dst port +// data plane: lidar source port -> host destination port const PORT_POINT: u16 = 56300; const PORT_IMU: u16 = 56400; const PORT_STATUS: u16 = 56200; @@ -30,39 +30,27 @@ const CMD_WORKMODE: u16 = 0x0100; #[derive(Debug, Deserialize, Validate)] #[serde(deny_unknown_fields)] struct Config { - /// Recorded Mid-360 pcap (data plane: point/IMU/status UDP). Read fully into RAM. + /// Recorded Mid-360 pcap (point/IMU/status UDP). Read fully into RAM. pcap: String, - /// Replay-speed multiplier; 1.0 = original inter-packet timing, >1 = faster. - #[serde(default = "one")] + /// Replay speed; 1.0 = original timing, >1 = faster. #[validate(range(min = 0.01, max = 1000.0))] rate: f64, - /// Seconds to wait after start before streaming begins. + /// Seconds to wait before streaming begins. #[serde(default)] #[validate(range(min = 0.0, max = 3600.0))] delay: f64, - /// IP the fake lidar sends from (must be assigned to this netns's veth). - /// Network-specific — required, no default. + /// IP the fake lidar sends from (on this netns's veth). Required. lidar_ip: String, - /// Host IP the recorded data is delivered to (where pointlio's SDK listens). - /// Machine-specific — required, no default. + /// Host IP the data is delivered to (where the SDK listens). Required. host_ip: String, - /// Network namespace the fake lidar runs inside (named in the setup-help - /// error). Deployment-specific — required, no default. + /// Network namespace the fake lidar runs in. Required. lidar_netns: String, - /// Multicast group the point/IMU streams are sent to. A real Mid-360 - /// multicasts these and the Livox SDK joins whatever `multicast_ip` is in - /// its host config; 224.1.1.5 is the Livox default (what pointlio uses), so - /// it's the default here. Override only to match a consumer configured with - /// a different `multicast_ip`. (Needs a `/32 dev ` route.) + /// Multicast group for point/IMU. 224.1.1.5 is the Livox default the SDK + /// joins; override only to match a differently-configured consumer. #[serde(default = "default_mcast_data")] mcast_data: String, } -fn one() -> f64 { - 1.0 -} -// 224.1.1.5 is the Livox Mid-360 default multicast_ip (a genuine Livox default, -// unlike the lidar/host IP + netns, which are deployment-specific and required). fn default_mcast_data() -> String { "224.1.1.5".into() } @@ -77,8 +65,8 @@ struct VirtualMid360 { // ---- CRCs (Livox SDK2: CRC16-CCITT-FALSE over header[0:18], CRC32/IEEE over data[]) ---- fn crc16_ccitt_false(data: &[u8]) -> u16 { let mut crc: u16 = 0xFFFF; - for &b in data { - crc ^= (b as u16) << 8; + for &byte in data { + crc ^= (byte as u16) << 8; for _ in 0..8 { crc = if crc & 0x8000 != 0 { (crc << 1) ^ 0x1021 @@ -92,8 +80,8 @@ fn crc16_ccitt_false(data: &[u8]) -> u16 { fn crc32_ieee(data: &[u8]) -> u32 { let mut crc: u32 = 0xFFFF_FFFF; - for &b in data { - crc ^= b as u32; + for &byte in data { + crc ^= byte as u32; for _ in 0..8 { crc = if crc & 1 != 0 { (crc >> 1) ^ 0xEDB8_8320 @@ -105,27 +93,26 @@ fn crc32_ieee(data: &[u8]) -> u32 { !crc } -/// Build a Livox SDK2 ACK frame from scratch (synthesized, not replayed): -/// header[0:18] (SOF, version=0, length, seq, cmd_id, cmd_type=1 ACK, sender_type=1) -/// + crc16_h@18 + data[] + crc32_d. `data` is the per-cmd ACK payload. +/// Synthesize a Livox SDK2 ACK frame: 18-byte header (SOF, ver, len, seq, cmd_id, +/// cmd_type=1, sender=1) + crc16@18 + `data` (per-cmd payload) + crc32@20. fn build_ack(cmd_id: u16, seq: u32, data: &[u8]) -> Vec { let length = (WRAPPER + data.len()) as u16; - let mut f = vec![0u8; WRAPPER + data.len()]; - f[0] = SOF; - f[1] = 0; // version - f[2..4].copy_from_slice(&length.to_le_bytes()); - f[4..8].copy_from_slice(&seq.to_le_bytes()); - f[8..10].copy_from_slice(&cmd_id.to_le_bytes()); - f[10] = 1; // cmd_type = ACK - f[11] = 1; // sender_type = lidar - // f[12..18] reserved (0) - let crc16 = crc16_ccitt_false(&f[0..18]); - f[18..20].copy_from_slice(&crc16.to_le_bytes()); - // f[20..24] = crc32 of data[] - f[24..].copy_from_slice(data); + let mut frame = vec![0u8; WRAPPER + data.len()]; + frame[0] = SOF; + frame[1] = 0; // version + frame[2..4].copy_from_slice(&length.to_le_bytes()); + frame[4..8].copy_from_slice(&seq.to_le_bytes()); + frame[8..10].copy_from_slice(&cmd_id.to_le_bytes()); + frame[10] = 1; // cmd_type = ACK + frame[11] = 1; // sender_type = lidar + // frame[12..18] reserved (0) + let crc16 = crc16_ccitt_false(&frame[0..18]); + frame[18..20].copy_from_slice(&crc16.to_le_bytes()); + // frame[20..24] = crc32 of data[] + frame[24..].copy_from_slice(data); let crc32 = crc32_ieee(data); - f[20..24].copy_from_slice(&crc32.to_le_bytes()); - f + frame[20..24].copy_from_slice(&crc32.to_le_bytes()); + frame } // ---- classic pcap (LE, magic d4c3b2a1) parser -> data-plane UDP packets ---- @@ -136,41 +123,42 @@ struct Pkt { } fn parse_pcap(path: &str) -> std::io::Result> { - let buf = std::fs::read(path)?; - if buf.len() < 24 || buf[0..4] != [0xd4, 0xc3, 0xb2, 0xa1] { + let buffer = std::fs::read(path)?; + if buffer.len() < 24 || buffer[0..4] != [0xd4, 0xc3, 0xb2, 0xa1] { return Err(std::io::Error::new( std::io::ErrorKind::InvalidData, format!("unsupported pcap (need classic little-endian, magic d4c3b2a1) at {path}"), )); } let mut out = Vec::new(); - let mut off = 24usize; - while off + 16 <= buf.len() { - let ts_sec = u32::from_le_bytes(buf[off..off + 4].try_into().unwrap()); - let ts_usec = u32::from_le_bytes(buf[off + 4..off + 8].try_into().unwrap()); - let incl = u32::from_le_bytes(buf[off + 8..off + 12].try_into().unwrap()) as usize; - off += 16; - if off + incl > buf.len() { + let mut offset = 24usize; + while offset + 16 <= buffer.len() { + let ts_sec = u32::from_le_bytes(buffer[offset..offset + 4].try_into().unwrap()); + let ts_usec = u32::from_le_bytes(buffer[offset + 4..offset + 8].try_into().unwrap()); + let captured_len = + u32::from_le_bytes(buffer[offset + 8..offset + 12].try_into().unwrap()) as usize; + offset += 16; + if offset + captured_len > buffer.len() { break; } - let frame = &buf[off..off + incl]; - off += incl; + let frame = &buffer[offset..offset + captured_len]; + offset += captured_len; // Ethernet(14) -> IPv4 -> UDP if frame.len() < 14 + 20 + 8 || frame[12] != 0x08 || frame[13] != 0x00 { continue; } - let ihl = ((frame[14] & 0x0f) as usize) * 4; + let ip_header_len = ((frame[14] & 0x0f) as usize) * 4; if frame[14 + 9] != 17 { continue; // not UDP } - let udp = 14 + ihl; - if frame.len() < udp + 8 { + let udp_offset = 14 + ip_header_len; + if frame.len() < udp_offset + 8 { continue; } - let src_port = u16::from_be_bytes([frame[udp], frame[udp + 1]]); - let udp_len = u16::from_be_bytes([frame[udp + 4], frame[udp + 5]]) as usize; - let payload_start = udp + 8; - let payload_end = (udp + udp_len).min(frame.len()); + let src_port = u16::from_be_bytes([frame[udp_offset], frame[udp_offset + 1]]); + let udp_len = u16::from_be_bytes([frame[udp_offset + 4], frame[udp_offset + 5]]) as usize; + let payload_start = udp_offset + 8; + let payload_end = (udp_offset + udp_len).min(frame.len()); if payload_end <= payload_start { continue; } @@ -194,32 +182,32 @@ fn ensure_interface(cfg: &Config) -> Result { // (or we're in the wrong namespace). let probe = UdpSocket::bind(SocketAddrV4::new(lidar_ip, CMD_PORT)); if probe.is_err() { - let ns = &cfg.lidar_netns; - let lip = &cfg.lidar_ip; - let hip = &cfg.host_ip; - let mcast = &cfg.mcast_data; + let netns = &cfg.lidar_netns; + let lidar_addr = &cfg.lidar_ip; + let host_addr = &cfg.host_ip; + let mcast_group = &cfg.mcast_data; return Err(format!( - "cannot bind {lip}:{CMD_PORT} — the virtual network interface isn't set up \ - (or this process isn't in the '{ns}' netns).\n\ + "cannot bind {lidar_addr}:{CMD_PORT} — the virtual network interface isn't set up \ + (or this process isn't in the '{netns}' netns).\n\ Run this once (creates the lidar/drv veth pair), then re-run the module:\n\ - \n sudo ip netns add drv\n sudo ip netns add {ns}\n \ + \n sudo ip netns add drv\n sudo ip netns add {netns}\n \ sudo ip link add veth-drv type veth peer name veth-lidar\n \ sudo ip link set veth-drv netns drv\n \ - sudo ip link set veth-lidar netns {ns}\n \ - sudo ip netns exec drv ip addr add {hip}/24 dev veth-drv\n \ - sudo ip netns exec {ns} ip addr add {lip}/24 dev veth-lidar\n \ + sudo ip link set veth-lidar netns {netns}\n \ + sudo ip netns exec drv ip addr add {host_addr}/24 dev veth-drv\n \ + sudo ip netns exec {netns} ip addr add {lidar_addr}/24 dev veth-lidar\n \ sudo ip netns exec drv ip link set veth-drv up\n \ - sudo ip netns exec {ns} ip link set veth-lidar up\n \ + sudo ip netns exec {netns} ip link set veth-lidar up\n \ sudo ip netns exec drv ip link set lo up\n \ - sudo ip netns exec {ns} ip link set lo up\n \ + sudo ip netns exec {netns} ip link set lo up\n \ sudo ip netns exec drv ip link set veth-drv multicast on\n \ - sudo ip netns exec {ns} ip link set veth-lidar multicast on\n \ - sudo ip netns exec {ns} ip route add 255.255.255.255/32 dev veth-lidar\n \ - sudo ip netns exec {ns} ip route add {mcast}/32 dev veth-lidar # point/IMU multicast\n \ + sudo ip netns exec {netns} ip link set veth-lidar multicast on\n \ + sudo ip netns exec {netns} ip route add 255.255.255.255/32 dev veth-lidar\n \ + sudo ip netns exec {netns} ip route add {mcast_group}/32 dev veth-lidar # point/IMU multicast\n \ sudo ip netns exec drv ip route add 224.0.0.0/4 dev lo # LCM (dimos transport)\n \ - sudo ip netns exec {ns} ip route add 224.0.0.0/4 dev lo # LCM (dimos transport)\n\ + sudo ip netns exec {netns} ip route add 224.0.0.0/4 dev lo # LCM (dimos transport)\n\ \nThen launch this module inside the lidar netns:\n \ - sudo ip netns exec {ns} " + sudo ip netns exec {netns} " )); } Ok(lidar_ip) @@ -251,7 +239,7 @@ impl VirtualMid360 { }; let packets = match parse_pcap(&cfg.pcap) { - Ok(p) if !p.is_empty() => Arc::new(p), + Ok(parsed) if !parsed.is_empty() => Arc::new(parsed), Ok(_) => { eprintln!( "[virtual_mid360] pcap '{}' has no Livox UDP data packets. \ @@ -260,9 +248,9 @@ impl VirtualMid360 { ); std::process::exit(2); } - Err(e) => { + Err(err) => { eprintln!( - "[virtual_mid360] failed to read pcap '{}': {e}. Fix the path, then re-run.", + "[virtual_mid360] failed to read pcap '{}': {err}. Fix the path, then re-run.", cfg.pcap ); std::process::exit(2); @@ -274,13 +262,11 @@ impl VirtualMid360 { let rate = cfg.rate; let delay = cfg.delay; - // discovery responder (:56000 broadcast) — synthesize the 0x0000 ACK + // discovery responder (:56000) — answers the 0x0000 broadcast spawn_discovery(lidar_ip, stop.clone()); - // control responder (:56100) — synthesize per-cmd ACKs; arm streaming - // when the host issues the work-mode/config command (0x0100) + // control responder (:56100) — per-cmd ACKs; arms streaming on 0x0100 spawn_control(lidar_ip, armed.clone(), stop.clone()); - // data streamer — point/IMU/status, paced at `rate`, timestamps rewritten - // to now, armed by the handshake (`delay` as a startup floor / fallback) + // data streamer — point/IMU/status paced at `rate`, timestamps shifted to now spawn_stream( lidar_ip, host_ip, mcast_data, packets, rate, delay, armed, stop, ); @@ -290,65 +276,68 @@ impl VirtualMid360 { fn spawn_discovery(lidar_ip: Ipv4Addr, stop: Arc) { std::thread::spawn(move || { - let sock = match UdpSocket::bind(SocketAddrV4::new(Ipv4Addr::UNSPECIFIED, DISCOVERY_PORT)) { - Ok(s) => s, - Err(e) => { - tracing::error!("discovery bind :{DISCOVERY_PORT} failed: {e}"); + let socket = match UdpSocket::bind(SocketAddrV4::new(Ipv4Addr::UNSPECIFIED, DISCOVERY_PORT)) + { + Ok(socket) => socket, + Err(err) => { + tracing::error!("discovery bind :{DISCOVERY_PORT} failed: {err}"); return; } }; - let _ = sock.set_broadcast(true); - sock.set_read_timeout(Some(Duration::from_millis(500))).ok(); - let bcast = SocketAddrV4::new(Ipv4Addr::BROADCAST, DISCOVERY_PORT); - let mut buf = [0u8; 2048]; + let _ = socket.set_broadcast(true); + socket + .set_read_timeout(Some(Duration::from_millis(500))) + .ok(); + let broadcast_addr = SocketAddrV4::new(Ipv4Addr::BROADCAST, DISCOVERY_PORT); + let mut buffer = [0u8; 2048]; while !stop.load(Ordering::Relaxed) { - let n = match sock.recv_from(&mut buf) { - Ok((n, _)) => n, + let len = match socket.recv_from(&mut buffer) { + Ok((len, _)) => len, Err(_) => continue, }; - if n < WRAPPER || buf[0] != SOF { + if len < WRAPPER || buffer[0] != SOF { continue; } - let cmd_id = u16::from_le_bytes([buf[8], buf[9]]); - let cmd_type = buf[10]; + let cmd_id = u16::from_le_bytes([buffer[8], buffer[9]]); + let cmd_type = buffer[10]; if cmd_id != 0x0000 || cmd_type != 0 { continue; } - let seq = u32::from_le_bytes([buf[4], buf[5], buf[6], buf[7]]); - // TODO(payload): discovery ACK data describes the device (dev_type, serial, - // lidar_ip, cmd port). Enumerate the exact layout from livox-sdk2 source. + let seq = u32::from_le_bytes([buffer[4], buffer[5], buffer[6], buffer[7]]); + // ACK describes the device (dev_type, serial, lidar_ip, cmd port). let ack = build_ack(0x0000, seq, &discovery_ack_payload(lidar_ip)); - let _ = sock.send_to(&ack, bcast); + let _ = socket.send_to(&ack, broadcast_addr); } }); } fn spawn_control(lidar_ip: Ipv4Addr, armed: Arc, stop: Arc) { std::thread::spawn(move || { - let sock = match UdpSocket::bind(SocketAddrV4::new(lidar_ip, CMD_PORT)) { - Ok(s) => s, - Err(e) => { - tracing::error!("control bind {lidar_ip}:{CMD_PORT} failed: {e}"); + let socket = match UdpSocket::bind(SocketAddrV4::new(lidar_ip, CMD_PORT)) { + Ok(socket) => socket, + Err(err) => { + tracing::error!("control bind {lidar_ip}:{CMD_PORT} failed: {err}"); return; } }; - sock.set_read_timeout(Some(Duration::from_millis(500))).ok(); - let mut buf = [0u8; 2048]; + socket + .set_read_timeout(Some(Duration::from_millis(500))) + .ok(); + let mut buffer = [0u8; 2048]; while !stop.load(Ordering::Relaxed) { - let (n, from) = match sock.recv_from(&mut buf) { - Ok(x) => x, + let (len, from) = match socket.recv_from(&mut buffer) { + Ok(received) => received, Err(_) => continue, }; - if n < WRAPPER || buf[0] != SOF { + if len < WRAPPER || buffer[0] != SOF { continue; } - let seq = u32::from_le_bytes([buf[4], buf[5], buf[6], buf[7]]); - let cmd_id = u16::from_le_bytes([buf[8], buf[9]]); - // TODO(payload): per-cmd_id ACK data. Most replies = ret_code(u8)=0 (success); - // queries echo the requested fields. Enumerate cmd_ids + payloads from - // livox-sdk2 source (comm/command_impl) or one captured real handshake. + let seq = u32::from_le_bytes([buffer[4], buffer[5], buffer[6], buffer[7]]); + let cmd_id = u16::from_le_bytes([buffer[8], buffer[9]]); + // Per-cmd_id ACK data (control_ack_payload): QueryFwType echoes a + // key-value param; the rest reply ret_code(u8)=0 (success). let ack = build_ack(cmd_id, seq, &control_ack_payload(cmd_id)); - let _ = sock.send_to(&ack, from); + let _ = socket.send_to(&ack, from); tracing::info!( cmd_id = format!("0x{cmd_id:04x}"), seq, @@ -374,11 +363,15 @@ fn spawn_stream( stop: Arc, ) { std::thread::spawn(move || { - let mk = |sport: u16| -> std::io::Result { - UdpSocket::bind(SocketAddrV4::new(lidar_ip, sport)) + let bind_port = |src_port: u16| -> std::io::Result { + UdpSocket::bind(SocketAddrV4::new(lidar_ip, src_port)) }; - let (point, imu, status) = match (mk(PORT_POINT), mk(PORT_IMU), mk(PORT_STATUS)) { - (Ok(a), Ok(b), Ok(c)) => (a, b, c), + let (point, imu, status) = match ( + bind_port(PORT_POINT), + bind_port(PORT_IMU), + bind_port(PORT_STATUS), + ) { + (Ok(point_sock), Ok(imu_sock), Ok(status_sock)) => (point_sock, imu_sock, status_sock), _ => { tracing::error!("failed to bind data-plane source ports on {lidar_ip}"); return; @@ -396,46 +389,44 @@ fn spawn_stream( std::thread::sleep(Duration::from_secs_f64(delay.max(0.0))); tracing::info!("streaming {} packets at {rate}x", packets.len()); - // Shift every packet's Livox sensor timestamp by a constant so the first - // emitted packet reads ≈ now and the original inter-packet spacing (used for - // intra-scan deskew) is preserved — the stream looks current/live. + // Shift every packet's sensor timestamp so the first reads ≈ now, + // preserving inter-packet spacing — the stream looks live. let now_ns = SystemTime::now() .duration_since(UNIX_EPOCH) .unwrap() .as_nanos() as u64; let first_orig = packets .iter() - .find(|p| matches!(p.src_port, PORT_POINT | PORT_IMU)) - .map(|p| read_ts_ns(&p.payload)) + .find(|pkt| matches!(pkt.src_port, PORT_POINT | PORT_IMU)) + .map(|pkt| read_ts_ns(&pkt.payload)) .unwrap_or(0); let ts_shift = now_ns.wrapping_sub(first_orig); let t_wall0 = Instant::now(); let mut t_cap0: Option = None; - for p in packets.iter() { + for pkt in packets.iter() { if stop.load(Ordering::Relaxed) { break; } - // The Mid-360 MULTICASTS point/IMU to mcast_data:port (the SDK joins that - // group — confirmed via `ss -ulnp` showing 224.1.1.5:56301/56401); status - // is unicast to the host. Sending point/IMU unicast is silently dropped. - let (sock, dst_ip, dst) = match p.src_port { + // Mid-360 multicasts point/IMU to mcast_data:port (the SDK joins it); + // status is unicast. Unicasting point/IMU is silently dropped. + let (socket, dest_ip, dest_port) = match pkt.src_port { PORT_POINT => (&point, mcast_data, DST_POINT), PORT_IMU => (&imu, mcast_data, DST_IMU), PORT_STATUS => (&status, host_ip, DST_STATUS), _ => continue, }; - let t0 = *t_cap0.get_or_insert(p.ts); - let target = (p.ts - t0) / rate; + let t0 = *t_cap0.get_or_insert(pkt.ts); + let target = (pkt.ts - t0) / rate; let elapsed = t_wall0.elapsed().as_secs_f64(); if target > elapsed { std::thread::sleep(Duration::from_secs_f64(target - elapsed)); } - let mut out = p.payload.clone(); - if matches!(p.src_port, PORT_POINT | PORT_IMU) { + let mut out = pkt.payload.clone(); + if matches!(pkt.src_port, PORT_POINT | PORT_IMU) { rewrite_ts(&mut out, ts_shift); } - let _ = sock.send_to(&out, SocketAddrV4::new(dst_ip, dst)); + let _ = socket.send_to(&out, SocketAddrV4::new(dest_ip, dest_port)); } tracing::info!("data stream finished"); }); @@ -449,17 +440,17 @@ const DEV_TYPE_MID360: u8 = 9; /// ret_code:u8, dev_type:u8, sn[16], lidar_ip[4], cmd_port:u16 LE. /// The SDK's VerifyNetSegment requires lidar_ip on the host's /24 (192.168.1.x). fn discovery_ack_payload(lidar_ip: Ipv4Addr) -> Vec { - let mut d = Vec::with_capacity(24); - d.push(0); // ret_code = success - d.push(DEV_TYPE_MID360); + let mut payload = Vec::with_capacity(24); + payload.push(0); // ret_code = success + payload.push(DEV_TYPE_MID360); // sn[16] MUST be null-terminated within 16 bytes — the SDK treats it as a // C-string (strcpy), so a full-16 SN with no NUL overruns its buffer. let mut sn = [0u8; 16]; sn[..10].copy_from_slice(b"FAKEMID360"); // sn[10..]=0 -> NUL-terminated - d.extend_from_slice(&sn); - d.extend_from_slice(&lidar_ip.octets()); - d.extend_from_slice(&CMD_PORT.to_le_bytes()); - d + payload.extend_from_slice(&sn); + payload.extend_from_slice(&lidar_ip.octets()); + payload.extend_from_slice(&CMD_PORT.to_le_bytes()); + payload } // kKeyFwType (livox_lidar_def.h ParamKeyName = 0x8010); fw_type != 0 => app @@ -471,28 +462,25 @@ const FW_TYPE_APP: u8 = 1; /// per-cmd response struct, which are #pragma pack(1) (packed, no padding). fn control_ack_payload(cmd_id: u16) -> Vec { match cmd_id { - // GetInternalInfo (0x0101): LivoxLidarDiagInternalInfoResponse (packed) — - // ret_code:u8 @0, param_num:u16 @1, data @3 (= LivoxLidarKeyValueParam: - // key:u16 @0, length:u16 @2, value @4). QueryFwType expects one param - // keyed kKeyFwType (0x8010) with a 1-byte fw_type value (non-zero = app). + // GetInternalInfo (0x0101), packed: ret_code:u8 @0, param_num:u16 @1, then + // one KeyValueParam (key:u16, len:u16, value). QueryFwType wants kKeyFwType + // (0x8010) -> 1-byte fw_type != 0. 0x0101 => { - let mut d = vec![0u8; 8]; - // d[0] ret_code = 0 - d[1..3].copy_from_slice(&1u16.to_le_bytes()); // param_num = 1 - d[3..5].copy_from_slice(&KEY_FW_TYPE.to_le_bytes()); - d[5..7].copy_from_slice(&1u16.to_le_bytes()); // value length = 1 - d[7] = FW_TYPE_APP; - d + let mut payload = vec![0u8; 8]; + // payload[0] ret_code = 0 + payload[1..3].copy_from_slice(&1u16.to_le_bytes()); // param_num = 1 + payload[3..5].copy_from_slice(&KEY_FW_TYPE.to_le_bytes()); + payload[5..7].copy_from_slice(&1u16.to_le_bytes()); // value length = 1 + payload[7] = FW_TYPE_APP; + payload } // Others: LivoxLidarAsyncControlResponse (packed) { ret_code:u8 @0, // error_key:u16 @1 } = 3 bytes. ret_code=0 (success), error_key=0. _ => vec![0u8; 3], } } -// LivoxLidarEthernetPacket.timestamp[8] sits at payload offset 28 (packed: -// version@0,len@1,time_interval@3,dot_num@5,udp_cnt@7,frame_cnt@9,data_type@10, -// time_type@11,rsvd@12,crc32@24,timestamp@28). The SDK casts the UDP payload -// directly to LivoxLidarEthernetPacket*, so offset 28 is in the payload. +// LivoxLidarEthernetPacket.timestamp[8] is at payload offset 28 (the SDK casts +// the UDP payload straight to that packed struct), so rewrite 8 bytes there. const TS_OFFSET: usize = 28; fn read_ts_ns(payload: &[u8]) -> u64 { From b245098dfaf48c5d339c05fa7e6c163f365456e7 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 05:01:34 -0700 Subject: [PATCH 110/185] rename LFS sample ruwik2_part3 -> mid360_shake_stairs Re-tarred data/mid360_shake_stairs/ (the Mid-360 shake-on-stairs clip) to data/.lfs/mid360_shake_stairs.tar.gz (uploaded to LFS), removed the old ruwik2_part3 tarball, and updated the get_data() reference in pcap_to_db. --- data/.lfs/mid360_shake_stairs.tar.gz | 3 +++ data/.lfs/ruwik2_part3.tar.gz | 3 --- dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) create mode 100644 data/.lfs/mid360_shake_stairs.tar.gz delete mode 100644 data/.lfs/ruwik2_part3.tar.gz diff --git a/data/.lfs/mid360_shake_stairs.tar.gz b/data/.lfs/mid360_shake_stairs.tar.gz new file mode 100644 index 0000000000..785543127d --- /dev/null +++ b/data/.lfs/mid360_shake_stairs.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d4ceef532f63b72ec2860003451df19baeece8ddf4360d34cb93e022d1294c29 +size 135443762 diff --git a/data/.lfs/ruwik2_part3.tar.gz b/data/.lfs/ruwik2_part3.tar.gz deleted file mode 100644 index 0820c3e481..0000000000 --- a/data/.lfs/ruwik2_part3.tar.gz +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:574d14edc55e015b57248db93a91009d319e2498d9aaff5b2538657c276d5318 -size 135443751 diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py index 05eeada809..952ac324b0 100644 --- a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py @@ -42,9 +42,9 @@ source .venv/bin/activate PCAP=$(python -c "from dimos.utils.data import get_data; \ - print(get_data('ruwik2_part3/ruwik2_part3.pcap'))") + print(get_data('mid360_shake_stairs/mid360_shake_stairs.pcap'))") python -m dimos.hardware.sensors.lidar.pointlio.tools.pcap_to_db --pcap "$PCAP" - # -> writes ruwik2_part3.db next to the sample. + # -> writes mid360_shake_stairs.db next to the sample. Two simultaneous runs (e.g. alongside a fastlio replay) must use distinct namespaces/IPs — see --drv-ns / --lidar-ns / --host-ip / --lidar-ip. From c64c2bad5f5045997667f227a6782464d697ba48 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 05:13:14 -0700 Subject: [PATCH 111/185] pointlio: rip out output-cloud downsampling + denoising Remove cloud_filter.hpp (PCL voxel-grid + statistical-outlier-removal) and its config (raw_cloud/voxel_size/outlier_*); publish_lidar now emits get_body_cloud as-is. The pre-KF downsample (Point-LIO's own space_down_sample/filter_size_surf in default.yaml) is untouched. --- .../lidar/pointlio/cpp/cloud_filter.hpp | 54 ------------------- .../sensors/lidar/pointlio/cpp/main.cpp | 16 ++---- .../hardware/sensors/lidar/pointlio/module.py | 9 ---- .../lidar/pointlio/pointlio_blueprints.py | 2 +- 4 files changed, 5 insertions(+), 76 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/pointlio/cpp/cloud_filter.hpp diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/cloud_filter.hpp b/dimos/hardware/sensors/lidar/pointlio/cpp/cloud_filter.hpp deleted file mode 100644 index 5d6a2f1673..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/cloud_filter.hpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright 2026 Dimensional Inc. -// SPDX-License-Identifier: Apache-2.0 -// -// Point cloud filtering utilities: voxel grid downsampling and -// statistical outlier removal using PCL. - -#ifndef CLOUD_FILTER_HPP_ -#define CLOUD_FILTER_HPP_ - -#include -#include -#include -#include - -struct CloudFilterConfig { - float voxel_size = 0.1f; // downsample leaf size (m); <=0 skips downsampling - int outlier_neighbor_count = 50; // statistical-outlier-removal neighbors; <=0 skips it - float outlier_std_threshold = 1.0f; -}; - -/// Voxel-grid downsample + statistical outlier removal; each stage is skipped -/// when its config is <=0. Returns the filtered cloud (new allocation). -template -typename pcl::PointCloud::Ptr filter_cloud( - const typename pcl::PointCloud::Ptr& input, - const CloudFilterConfig& cfg) { - - if (!input || input->empty()) { return input; } - - auto working = input; - if (cfg.voxel_size > 0.0f) { - typename pcl::PointCloud::Ptr voxelized(new pcl::PointCloud()); - pcl::VoxelGrid vg; - vg.setInputCloud(working); - vg.setLeafSize(cfg.voxel_size, cfg.voxel_size, cfg.voxel_size); - vg.filter(*voxelized); - working = voxelized; - } - - if (cfg.outlier_neighbor_count > 0 && - working->size() > static_cast(cfg.outlier_neighbor_count)) { - typename pcl::PointCloud::Ptr cleaned(new pcl::PointCloud()); - pcl::StatisticalOutlierRemoval sor; - sor.setInputCloud(working); - sor.setMeanK(cfg.outlier_neighbor_count); - sor.setStddevMulThresh(cfg.outlier_std_threshold); - sor.filter(*cleaned); - return cleaned; - } - - return working; -} - -#endif diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 63e01f2e83..f40cfba931 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -31,7 +31,6 @@ #include "livox_sdk_config.hpp" -#include "cloud_filter.hpp" #include "dimos_native_module.hpp" #include "geometry_msgs/Quaternion.hpp" @@ -330,11 +329,6 @@ int main(int argc, char** argv) { g_child_frame_id = mod.arg_required("body_frame_id"); float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f); float odom_freq = mod.arg_float("odom_freq", 50.0f); - bool raw_cloud = mod.arg_bool("raw_cloud", false); - CloudFilterConfig filter_cfg; - filter_cfg.voxel_size = mod.arg_float("voxel_size", 0.1f); - filter_cfg.outlier_neighbor_count = mod.arg_int("outlier_neighbor_count", 50); - filter_cfg.outlier_std_threshold = mod.arg_float("outlier_std_threshold", 1.0f); // Propagates to the Point-LIO core via the `pointlio_debug` global. bool debug = mod.arg_bool("debug", false); @@ -361,7 +355,6 @@ int main(int argc, char** argv) { printf("[pointlio] config: %s\n", config_path.c_str()); printf("[pointlio] host_ip: %s lidar_ip: %s frequency: %.1f Hz\n", host_ip.c_str(), lidar_ip.c_str(), g_frequency); printf("[pointlio] pointcloud_freq: %.1f Hz odom_freq: %.1f Hz\n", pointcloud_freq, odom_freq); - printf("[pointlio] output cloud: raw=%d voxel_size=%.3f outlier_neighbors=%d " "outlier_std=%.1f\n", raw_cloud, filter_cfg.voxel_size, filter_cfg.outlier_neighbor_count, filter_cfg.outlier_std_threshold); } signal(SIGTERM, signal_handler); @@ -446,16 +439,15 @@ int main(int argc, char** argv) { const bool lidar_due = !g_lidar_topic.empty() && now - *last_pc_publish >= pc_interval; - // get_body_cloud + filter_cloud (SOR) is the loop's costliest step, - // so build it only when a publish is due. + // get_body_cloud is the loop's costliest step, so build it only when + // a publish is due. if (lidar_due) { auto body_cloud = point_lio.get_body_cloud(); if (body_cloud && !body_cloud->empty()) { - auto filtered = raw_cloud ? body_cloud : filter_cloud(body_cloud, filter_cfg); - publish_lidar(filtered, ts); + publish_lidar(body_cloud, ts); last_pc_publish = now; if (pointlio_debug) { - fprintf(stderr, "[pointlio] publish lidar: %zu points pose=(%.3f, %.3f, %.3f)\n", filtered ? filtered->size() : 0, pose[0], pose[1], pose[2]); + fprintf(stderr, "[pointlio] publish lidar: %zu points pose=(%.3f, %.3f, %.3f)\n", body_cloud->size(), pose[0], pose[1], pose[2]); } } } diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index 6b080d556c..9fea27233a 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -93,15 +93,6 @@ class PointLioConfig(NativeModuleConfig): pointcloud_freq: float = 10.0 odom_freq: float = 30.0 - # raw_cloud=True publishes the unfiltered scan (no downsample/outlier removal). - raw_cloud: bool = False - # Voxel-grid downsample leaf size (m); 0 keeps full resolution. - voxel_size: float = 0.1 - # Statistical outlier removal: neighbors examined per point (0 disables it) - # and how many std-devs from the mean neighbor distance is kept. - outlier_neighbor_count: int = 50 - outlier_std_threshold: float = 1.0 - # Point-LIO YAML config (relative to config/ dir, or absolute path). config: Annotated[ Path, diff --git a/dimos/hardware/sensors/lidar/pointlio/pointlio_blueprints.py b/dimos/hardware/sensors/lidar/pointlio/pointlio_blueprints.py index bbd2cc4272..90f0bd9d41 100644 --- a/dimos/hardware/sensors/lidar/pointlio/pointlio_blueprints.py +++ b/dimos/hardware/sensors/lidar/pointlio/pointlio_blueprints.py @@ -22,7 +22,7 @@ mid360_pointlio = autoconnect( - PointLio.blueprint(voxel_size=voxel_size), + PointLio.blueprint(), vis_module("rerun"), ).global_config(n_workers=2, robot_model="mid360_pointlio") From de3b32fe51beeaf2879d513ab3b09070dbbbeb12 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 08:09:47 -0700 Subject: [PATCH 112/185] fastlio2: rip out output-cloud downsampling + denoising (mirror pointlio) Remove cloud_filter.hpp (PCL voxel-grid + statistical-outlier-removal) and its config (voxel_size/sor_mean_k/sor_stddev); publish_lidar now emits the registered world-frame cloud as-is. The pre-KF downsample (FAST-LIO's own filter_size_surf in mid360.yaml) is untouched. Mirrors pointlio c64c2bad5. --- .../lidar/fastlio2/cpp/cloud_filter.hpp | 51 ------------------- .../sensors/lidar/fastlio2/cpp/main.cpp | 18 ++----- .../lidar/fastlio2/fastlio_blueprints.py | 6 +-- .../hardware/sensors/lidar/fastlio2/module.py | 5 -- 4 files changed, 6 insertions(+), 74 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/cloud_filter.hpp diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/cloud_filter.hpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/cloud_filter.hpp deleted file mode 100644 index 352ba9bef5..0000000000 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/cloud_filter.hpp +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright 2026 Dimensional Inc. -// SPDX-License-Identifier: Apache-2.0 -// -// Point cloud filtering utilities: voxel grid downsampling and -// statistical outlier removal using PCL. - -#ifndef CLOUD_FILTER_HPP_ -#define CLOUD_FILTER_HPP_ - -#include -#include -#include -#include - -struct CloudFilterConfig { - float voxel_size = 0.1f; - int sor_mean_k = 50; - float sor_stddev = 1.0f; -}; - -/// Apply voxel grid downsample + statistical outlier removal in-place. -/// Returns the filtered cloud (new allocation). -template -typename pcl::PointCloud::Ptr filter_cloud( - const typename pcl::PointCloud::Ptr& input, - const CloudFilterConfig& cfg) { - - if (!input || input->empty()) return input; - - // Voxel grid downsample - typename pcl::PointCloud::Ptr voxelized(new pcl::PointCloud()); - pcl::VoxelGrid vg; - vg.setInputCloud(input); - vg.setLeafSize(cfg.voxel_size, cfg.voxel_size, cfg.voxel_size); - vg.filter(*voxelized); - - // Statistical outlier removal - if (cfg.sor_mean_k > 0 && voxelized->size() > static_cast(cfg.sor_mean_k)) { - typename pcl::PointCloud::Ptr cleaned(new pcl::PointCloud()); - pcl::StatisticalOutlierRemoval sor; - sor.setInputCloud(voxelized); - sor.setMeanK(cfg.sor_mean_k); - sor.setStddevMulThresh(cfg.sor_stddev); - sor.filter(*cleaned); - return cleaned; - } - - return voxelized; -} - -#endif diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index c1ec26831b..c41f2b3230 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -31,7 +31,6 @@ #include "livox_sdk_config.hpp" -#include "cloud_filter.hpp" #include "dimos_native_module.hpp" #include "timing.hpp" @@ -425,10 +424,6 @@ int main(int argc, char** argv) { g_child_frame_id = mod.arg_required("child_frame_id"); float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f); float odom_freq = mod.arg_float("odom_freq", 50.0f); - CloudFilterConfig filter_cfg; - filter_cfg.voxel_size = mod.arg_float("voxel_size", 0.1f); - filter_cfg.sor_mean_k = mod.arg_int("sor_mean_k", 50); - filter_cfg.sor_stddev = mod.arg_float("sor_stddev", 1.0f); // Verbose logging — propagates to the FAST-LIO C++ core via the // `fastlio_debug` global. Default false → only real errors print. @@ -482,8 +477,6 @@ int main(int argc, char** argv) { host_ip.c_str(), lidar_ip.c_str(), g_frequency); printf("[fastlio2] pointcloud_freq: %.1f Hz odom_freq: %.1f Hz\n", pointcloud_freq, odom_freq); - printf("[fastlio2] voxel_size: %.3f sor_mean_k: %d sor_stddev: %.1f\n", - filter_cfg.voxel_size, filter_cfg.sor_mean_k, filter_cfg.sor_stddev); } // Signal handlers @@ -528,7 +521,6 @@ int main(int argc, char** argv) { static timing::Section t_feed_lidar{"fast_lio.feed_lidar"}; static timing::Section t_process{"fast_lio.process"}; static timing::Section t_get_world_cloud{"fast_lio.get_world_cloud"}; - static timing::Section t_filter_cloud{"filter_cloud"}; static timing::Section t_publish_lidar{"publish_lidar"}; static timing::Section t_publish_odom{"publish_odometry"}; @@ -600,15 +592,11 @@ int main(int argc, char** argv) { return fast_lio.get_world_cloud(); })(); if (world_cloud && !world_cloud->empty()) { - auto filtered = ([&]() { - timing::Scope s(t_filter_cloud); - return filter_cloud(world_cloud, filter_cfg); - })(); - - // Per-scan world-frame cloud at pointcloud_freq. + // Per-scan world-frame cloud at pointcloud_freq, published as-is + // (no output-cloud voxel downsampling / outlier removal). if (!g_lidar_topic.empty() && now - *last_pc_publish >= pc_interval) { timing::Scope s(t_publish_lidar); - publish_lidar(filtered, ts); + publish_lidar(world_cloud, ts); last_pc_publish = now; } } diff --git a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py index 6c848233f2..b4af6eda9d 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py +++ b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py @@ -23,7 +23,7 @@ mid360_fastlio = autoconnect( - FastLio2.blueprint(voxel_size=voxel_size), + FastLio2.blueprint(), vis_module("rerun"), ).global_config(n_workers=2, robot_model="mid360_fastlio2") @@ -41,7 +41,7 @@ ).global_config(n_workers=3, robot_model="mid360_fastlio2_voxels") mid360_fastlio_voxels_native = autoconnect( - FastLio2.blueprint(voxel_size=voxel_size), + FastLio2.blueprint(), vis_module( "rerun", rerun_config={ @@ -54,7 +54,7 @@ mid360_fastlio_ray_trace = autoconnect( - FastLio2.blueprint(voxel_size=voxel_size), + FastLio2.blueprint(), RayTracingVoxelMap.blueprint(voxel_size=voxel_size), vis_module( "rerun", diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 9a547c5952..2f8df2b556 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -97,11 +97,6 @@ class FastLio2Config(NativeModuleConfig): pointcloud_freq: float = 10.0 odom_freq: float = 30.0 - # Point cloud filtering - voxel_size: float = 0.1 - sor_mean_k: int = 50 - sor_stddev: float = 1.0 - # FAST-LIO YAML config (relative to config/ dir, or absolute path) # C++ binary reads YAML directly via yaml-cpp config: Annotated[ From 6f33458f7cacd5a013c427c1ef9411fd9bb69b4c Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 08:14:53 -0700 Subject: [PATCH 113/185] fastlio2: uninit Livox SDK before clearing globals (fix shutdown null-deref race) On shutdown, LivoxLidarSdkUninit() (which stops + joins the SDK callback threads) now runs before g_fastlio/g_lcm are nulled, so a late on_point/on_imu callback can't race the assignment and dereference null. Mirrors pointlio c4fa47b54. --- dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index c41f2b3230..1bcbbbd190 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -650,10 +650,12 @@ int main(int argc, char** argv) { } } - // Cleanup + // Cleanup. Uninit the SDK (stops + joins its callback threads) BEFORE + // clearing the globals the callbacks read, so a late on_point/on_imu can't + // race the assignment and dereference a null g_fastlio / g_lcm. if (debug) printf("[fastlio2] Shutting down...\n"); - g_fastlio = nullptr; LivoxLidarSdkUninit(); + g_fastlio = nullptr; g_lcm = nullptr; if (debug) printf("[fastlio2] Done.\n"); From 24cb75aff4577733a91ef20dcd8555926fe64d79 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 15 Jun 2026 22:54:18 -0700 Subject: [PATCH 114/185] fastlio2: trim verbose comments across the recorder files Condense the over-explanatory comments/docstrings on the fastlio recorder work (main.cpp, timing.hpp, module.py, recorder.py, pcap_to_db.py, pcap_recorder.py, virtual_mid360/blueprints.py), keeping license headers + non-obvious rationale (velocity cap, mutex atomicity, shutdown-race, chown-for-root, stop-file logic). Also fixes two now-stale references: timing.hpp's removed replay feeder and blueprints.py's deleted replay_via_virtual_mid360.sh (-> pcap_to_db.py). --- .../sensors/lidar/fastlio2/cpp/main.cpp | 48 ++++++----------- .../sensors/lidar/fastlio2/cpp/timing.hpp | 24 +++------ .../hardware/sensors/lidar/fastlio2/module.py | 21 ++------ .../sensors/lidar/fastlio2/recorder.py | 32 +++--------- .../lidar/fastlio2/tools/pcap_to_db.py | 52 +++++++------------ .../sensors/lidar/livox/pcap_recorder.py | 24 ++------- .../lidar/livox/virtual_mid360/blueprints.py | 14 ++--- 7 files changed, 59 insertions(+), 156 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index 1bcbbbd190..8ebebdf589 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -64,9 +64,7 @@ static double get_publish_ts() { std::chrono::system_clock::now().time_since_epoch()).count(); } -// Clock for the main loop's frame/publish rate limiters. Returns an optional -// for backward-compatibility with callers that check has_value(); the live -// path always populates it. +// Steady clock for the main loop's frame/publish rate limiters. static std::optional virtual_now() { return std::chrono::steady_clock::now(); } @@ -179,12 +177,8 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, pc.data_length = pc.row_step; pc.data.resize(pc.data_length); - // Apply the full init_pose transform (rotation + translation) to point clouds. - // FAST-LIO's map origin is at the sensor's initial position. The rotation - // corrects axis direction (e.g. 180° X for upside-down mount) and the - // translation shifts the origin so that ground sits at z≈0 (e.g. z=1.2 - // for a sensor mounted 1.2m above ground). This matches the odometry - // frame, which also gets the full init_pose applied. + // Apply init_pose (rotation + translation) to match the odometry frame: + // rotation corrects the mount axis, translation shifts the origin to ground z≈0. const bool apply_init_pose = has_init_pose(); for (int i = 0; i < num_points; ++i) { float* dst = reinterpret_cast(pc.data.data() + i * 16); @@ -408,12 +402,9 @@ int main(int argc, char** argv) { double msr_freq = mod.arg_float("msr_freq", 50.0f); double main_freq = mod.arg_float("main_freq", 5000.0f); - // Post-IESKF-update velocity cap. When |v_world| exceeds this value - // the EKF state is restored to the last accepted scan with vel=0 and - // map_incremental is skipped. Breaks the reinforcing-loop divergence - // that gives FAST-LIO multi-km/s velocity runaway on aggressive - // motion or large IMU gaps. Zero disables. Defaults sized to the - // Go2 quadruped envelope (~3.1 m/s); raise for faster platforms. + // Post-IESKF velocity cap: if |v_world| exceeds this, restore the EKF to the + // last accepted scan (vel=0) and skip map_incremental, breaking the multi-km/s + // divergence runaway on aggressive motion / IMU gaps. Zero disables. double max_velocity_norm_ms = mod.arg_float("max_velocity_norm_ms", 0.0f); // Livox hardware config @@ -511,11 +502,7 @@ int main(int argc, char** argv) { std::optional last_pc_publish; std::optional last_odom_publish; - // Per-section timing counters for `run_main_iter`. Active only when - // --debug is on (Scope's constructor reads `fastlio_debug` and no-ops - // otherwise). `timing::maybe_flush(now)` at the bottom prints a per- - // section summary every second of wall clock so we can see both how - // often each part fires and how long each call takes. + // Per-section timing counters for `run_main_iter` (see timing.hpp; --debug only). static timing::Section t_iter{"run_main_iter"}; static timing::Section t_emit_check{"emit.lock+swap"}; static timing::Section t_feed_lidar{"fast_lio.feed_lidar"}; @@ -526,9 +513,8 @@ int main(int argc, char** argv) { auto run_main_iter = [&](std::chrono::steady_clock::time_point now) { timing::Scope iter_scope(t_iter); - // Lazy-seed all rate-limit bookmarks on the first iteration so they - // line up with the wall clock and don't fire immediately based on an - // arbitrary "since program start" delta. + // Lazy-seed the rate-limit bookmarks on the first iteration so they line + // up with the wall clock instead of firing immediately. auto seed = now; if (!last_emit.has_value()) { last_emit = seed; @@ -540,11 +526,9 @@ int main(int argc, char** argv) { last_odom_publish = seed; } - // At frame rate: drain accumulated raw points into a CustomMsg - // and feed to FAST-LIO. Hold g_pc_mutex across the rate-limit - // CHECK as well as the swap so the accumulator is observed - // atomically with no other thread able to slip a packet in - // between the decision and the swap. + // At frame rate, drain accumulated raw points into a CustomMsg and feed + // FAST-LIO. Hold g_pc_mutex across the rate-limit check + swap so a + // callback can't slip a packet in between the decision and the swap. std::vector points; uint64_t frame_start = 0; { @@ -592,8 +576,7 @@ int main(int argc, char** argv) { return fast_lio.get_world_cloud(); })(); if (world_cloud && !world_cloud->empty()) { - // Per-scan world-frame cloud at pointcloud_freq, published as-is - // (no output-cloud voxel downsampling / outlier removal). + // World-frame cloud at pointcloud_freq, published as-is (no downsampling). if (!g_lidar_topic.empty() && now - *last_pc_publish >= pc_interval) { timing::Scope s(t_publish_lidar); publish_lidar(world_cloud, ts); @@ -613,9 +596,8 @@ int main(int argc, char** argv) { timing::maybe_flush(std::chrono::steady_clock::now()); }; - // Source of point/IMU packets: the Livox SDK opens UDP sockets and - // dispatches via callbacks from its own threads. The main thread drives - // run_main_iter at main_freq, consuming whatever the SDK queued. + // The Livox SDK opens UDP sockets and dispatches via its own callback + // threads; the main thread drives run_main_iter, consuming what's queued. if (!livox_common::init_livox_sdk(host_ip, lidar_ip, ports, debug)) { return 1; } diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/timing.hpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/timing.hpp index d1fbe8ded4..759daf3c2e 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/timing.hpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/timing.hpp @@ -2,23 +2,13 @@ // SPDX-License-Identifier: Apache-2.0 // // Lightweight per-section timing for diagnosing where wall time goes in -// `run_main_iter`. Active only when --debug is on (i.e. the global -// `fastlio_debug` flag is true) so non-debug runs pay only a single -// branch per scope. +// `run_main_iter`. Active only when --debug (the global `fastlio_debug` flag) +// is on, so non-debug runs pay one branch per scope. // // Usage: -// // static timing::Section sec{"filter_cloud"}; -// { -// timing::Scope s(sec); -// // ...do work... -// } -// // and periodically: -// timing::maybe_flush(now); -// -// The flush prints one line per section to stderr every flush interval -// (1 second of wall clock) summarising count / mean / max / total, then -// resets the accumulators. The flush is cheap when nothing was recorded. +// { timing::Scope s(sec); /* ...do work... */ } +// timing::maybe_flush(now); // periodically #pragma once @@ -83,10 +73,8 @@ struct Scope { }; // Print one summary line per section to stderr every FLUSH_INTERVAL wall -// seconds, then reset accumulators. The check is cheap: a single time -// comparison guarded by the fastlio_debug flag. The mutex serialises the -// flush between threads (replay's feeder vs live's main loop) so we -// never see torn output. +// seconds, then reset accumulators. Mutex-guarded so concurrent threads don't +// produce torn output. inline void maybe_flush(std::chrono::steady_clock::time_point now) { if (!fastlio_debug) { return; diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 2f8df2b556..5d59aa3cac 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -14,19 +14,8 @@ """Python NativeModule wrapper for the FAST-LIO2 + Livox Mid-360 binary. -Binds Livox SDK2 directly into FAST-LIO-NON-ROS for real-time LiDAR SLAM. -Outputs registered (world-frame) point clouds and odometry with covariance. - -Usage:: - - from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 - from dimos.core.coordination.blueprints import autoconnect - - from dimos.core.coordination.module_coordinator import ModuleCoordinator - ModuleCoordinator.build(autoconnect( - FastLio2.blueprint(host_ip="192.168.1.5"), - SomeConsumer.blueprint(), - )).loop() +Binds Livox SDK2 into FAST-LIO-NON-ROS for real-time LiDAR SLAM; outputs +registered (world-frame) point clouds and odometry with covariance. """ from __future__ import annotations @@ -83,9 +72,8 @@ class FastLio2Config(NativeModuleConfig): # Converted to init_pose CLI arg [x, y, z, qx, qy, qz, qw] in model_post_init. mount: Pose = Pose() - # Frame IDs for output messages. "odom" reflects that FastLio2 provides - # locally-smooth, continuous odometry (no loop-closure jumps). PGO - # publishes the map→odom correction via TF. + # "odom" frame: FastLio2 gives smooth continuous odometry; PGO publishes the + # map→odom correction via TF. frame_id: str = FRAME_ODOM child_frame_id: str = FRAME_BODY @@ -193,7 +181,6 @@ def _validate_network(self) -> None: local_ips=local_ips, ) - # Check if host_ip is actually assigned to this machine. if host_ip not in local_ips: try: lidar_net = ipaddress.IPv4Network(f"{lidar_ip}/24", strict=False) diff --git a/dimos/hardware/sensors/lidar/fastlio2/recorder.py b/dimos/hardware/sensors/lidar/fastlio2/recorder.py index 24ec96e473..9d9140d71c 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/recorder.py +++ b/dimos/hardware/sensors/lidar/fastlio2/recorder.py @@ -12,21 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""FAST-LIO output recorder with timestamp rewriting. - -Records the two FastLio2 output streams — ``fastlio_odometry`` and -``fastlio_lidar`` — into a memory2 SQLite db, rewriting *only those streams'* -timestamps onto the db's clock. This is what makes offline pcap replay line up -with a live recording: FAST-LIO replayed from a pcap publishes on the pcap's -capture clock, and this recorder shifts that onto whatever clock the db already -uses, while leaving every other stream in the db untouched. - -The timestamp conversion is the only thing it does beyond a plain recorder, and -it applies it to fastlio messages exclusively (the two declared inputs), so a -recording that mixes fastlio replay with already-correct streams stays -coherent. Used by ``tools/pcap_to_db.py``; also usable directly in a blueprint -that wires FastLio2's ``odometry``/``lidar`` into this module's inputs. -""" +"""Record FAST-LIO's odometry + lidar into a memory2 SQLite db, rewriting only +those streams' timestamps onto the db clock so offline pcap replay lines up with +a live recording. Used by tools/pcap_to_db.py.""" from __future__ import annotations @@ -56,13 +44,7 @@ class FastLio2RecorderConfig(ModuleConfig): class FastLio2Recorder(Module): - """Record FAST-LIO odometry + lidar into a SQLite db, rewriting their ts. - - Only the fastlio streams declared here are time-converted; nothing else in - the db is touched. The offset is auto-derived (same clock family -> 0; - cross-clock -> start-align the replay's first ts onto the db's first), or - forced via ``time_offset``. - """ + """Offset auto-derives: same clock family -> 0; cross-clock -> start-align.""" config: FastLio2RecorderConfig fastlio_odometry: In[Odometry] @@ -90,8 +72,7 @@ def _resolve_offset(self, first_ts: float) -> float: ref = self.config.ref_start_ts if ref < 0.0: return 0.0 - # Same clock family (both wall, or both sensor) -> already aligned. - # Cross-clock -> start-align the replay's first ts onto the db's first. + # Same clock family -> aligned; cross-clock -> start-align onto db's first. if (first_ts > _SENSOR_CLOCK_MAX) == (ref > _SENSOR_CLOCK_MAX): return 0.0 return ref - first_ts @@ -104,8 +85,7 @@ def _aligned_ts(self, raw_ts: float, last_ts: float) -> float: @staticmethod def _raw_ts(v: object) -> float: - # A genuine sensor ts of 0.0 is falsy, so guard on None explicitly rather - # than `ts or time.time()` (which would misclassify a 0.0 stamp as wall). + # Guard on None: a genuine 0.0 ts is falsy, so `ts or time.time()` would drop it. ts = getattr(v, "ts", None) return ts if ts is not None else time.time() diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index 49c9bac1fb..1abd17e7e2 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -14,25 +14,17 @@ """Replay a Livox Mid-360 pcap through FAST-LIO and record its output to a .db. -The pcap is replayed over the wire by ``virtual_mid360`` — a fake Mid-360 on a -virtual NIC that synthesizes the Livox SDK2 handshake — and FastLio2 connects to -it in live SDK mode, exactly as it would to real hardware (there is no in-process -pcap reader). This tool orchestrates the whole thing behind a simple CLI: - -* ``pcap_to_db --pcap capture.pcap`` -> writes ``capture.db`` from scratch -* ``pcap_to_db --pcap capture.pcap --db mem2.db`` -> appends into an existing db - -It records two streams — ``fastlio_odometry`` and ``fastlio_lidar`` — and -time-aligns them onto the db's clock (``--time-offset`` overrides the auto -choice; ``--force`` overwrites pre-existing fastlio streams). Replay runs at -real time and stops automatically when the pcap is drained. - -It stands up two network namespaces joined by a veth (the fake lidar in one, the -FastLio2 consumer in the other), which needs root: set ``$SUDO`` to a -privilege-escalation command that runs ``ip``/``pkill``/``chown`` without a -password prompt (default ``sudo``). Netns/veth names default to -``fl_drv``/``fl_lidar`` + ``veth-fl-*`` (distinct from pointlio's harness so the -two can run at once); override via ``$DRV_NS``/``$LIDAR_NS``/``$VETH_*``. +``virtual_mid360`` replays the pcap over the wire (a fake Mid-360 on a virtual +NIC) and FastLio2 connects in live SDK mode, exactly as to real hardware: + +* ``pcap_to_db --pcap capture.pcap`` -> writes ``capture.db`` +* ``pcap_to_db --pcap capture.pcap --db mem2.db`` -> appends into an existing db + +Records ``fastlio_odometry`` + ``fastlio_lidar``, time-aligned onto the db clock +(``--time-offset`` overrides; ``--force`` overwrites pre-existing fastlio streams). +Stands up two netns joined by a veth, so it needs root — set ``$SUDO`` to a +passwordless escalation (default ``sudo``); netns/veth names override via +``$DRV_NS``/``$LIDAR_NS``/``$VETH_*``. Build the virtual_mid360 binary once:: @@ -58,8 +50,7 @@ # Let FastLio2 drain its scan backlog after the pcap finishes before stopping. _DRAIN_SEC = 10.0 -# Privilege-escalation command + network namespace / veth names. The lidar ns -# runs virtual_mid360; the drv ns runs the FastLio2 consumer. Defaults are +# lidar ns runs virtual_mid360; drv ns runs the FastLio2 consumer. Defaults are # distinct from pointlio's harness so both can run concurrently. _SUDO = os.environ.get("SUDO", "sudo") _DRV_NS = os.environ.get("DRV_NS", "fl_drv") @@ -226,10 +217,8 @@ def _orchestrate(args: argparse.Namespace) -> int: cmd += ["--force"] if args.time_offset is not None: cmd += ["--time-offset", str(args.time_offset)] - # SQLite won't let root write a db owned by another user, and the - # recorder runs as root inside the netns. So if we're appending into an - # existing (user-owned) db, take ownership for the run — the chown back - # to the caller at the end restores it. + # SQLite won't let root (the in-netns recorder) write a user-owned db, so + # take ownership for the run; the chown back at the end restores it. for suffix in ("", "-wal", "-shm"): q = Path(str(db) + suffix) if q.exists(): @@ -261,11 +250,10 @@ def _orchestrate(args: argparse.Namespace) -> int: vm.stdin.write((vm_cfg + "\n").encode()) vm.stdin.close() - # virtual_mid360 streams the pcap exactly once, then logs "data - # stream finished". Wait for that (bounded by --duration), let - # FastLio2 drain its scan backlog, then stop the recorder via the - # stop-file. (Stagnation-watching the db is unreliable: a diverging - # FastLio2 keeps emitting long after the sensor goes quiet.) + # virtual_mid360 streams the pcap once, then logs "data stream + # finished"; wait for that (capped by --duration), drain, then stop. + # (Watching the db for stagnation is unreliable — a diverging FastLio2 + # keeps emitting long after the sensor goes quiet.) deadline = time.time() + args.duration while time.time() < deadline: if vm.poll() is not None: @@ -356,9 +344,7 @@ def _consume(args: argparse.Namespace) -> int: ).global_config(n_workers=4, robot_model="mid360_fastlio2_pcap_to_db") coord = ModuleCoordinator.build(blueprint) - # The orchestrator drives the lifetime: it watches virtual_mid360 finish - # streaming, lets the backlog drain, then touches the stop-file. --duration - # is just a safety cap. + # The orchestrator signals stop via the stop-file; --duration is a safety cap. stopfile = Path(args._stopfile) if args._stopfile else None t0 = time.time() try: diff --git a/dimos/hardware/sensors/lidar/livox/pcap_recorder.py b/dimos/hardware/sensors/lidar/livox/pcap_recorder.py index 954c46ca3c..31f27e801f 100644 --- a/dimos/hardware/sensors/lidar/livox/pcap_recorder.py +++ b/dimos/hardware/sensors/lidar/livox/pcap_recorder.py @@ -14,16 +14,9 @@ """Standalone Livox pcap recorder. -Captures the raw Livox Mid-360 UDP packets (point + IMU) to a libpcap file via -tcpdump. This is the ground-truth sensor input the FastLio2 binary can be -replayed against bit-for-bit (see fastlio2/tools/pcap_to_db.py). - -Decoupled from FAST-LIO on purpose: the lidar driver / SLAM module should not -own a packet capture. Drop this module into any blueprint that needs the raw -wire recorded alongside the live run:: - - from dimos.hardware.sensors.lidar.livox.pcap_recorder import LivoxPcapRecorder - autoconnect(Mid360.blueprint(...), LivoxPcapRecorder.blueprint(pcap_path="raw.pcap")) +Captures the raw Livox Mid-360 UDP packets to a libpcap file via tcpdump — the +ground-truth sensor input FastLio2 can be replayed against (see +fastlio2/tools/pcap_to_db.py). Kept separate from the SLAM module on purpose. tcpdump needs capture capability once per host: sudo setcap cap_net_raw,cap_net_admin=eip $(which tcpdump) @@ -81,9 +74,7 @@ class LivoxPcapRecorderConfig(ModuleConfig): """Where and how to capture the raw Livox UDP stream.""" pcap_path: str | Path = "raw_mid360.pcap" - # Capture interface for tcpdump. Machine-specific, so it defaults from the - # DIMOS_PCAP_IFACE env var (falling back to enp2s0) to avoid hardcoding a - # value that's only correct on one host. + # Machine-specific, so defaults from DIMOS_PCAP_IFACE env (fallback enp2s0). record_pcap_iface: str = Field( default_factory=lambda: os.environ.get("DIMOS_PCAP_IFACE", "enp2s0") ) @@ -95,12 +86,7 @@ class LivoxPcapRecorderConfig(ModuleConfig): class LivoxPcapRecorder(Module): - """Owns a tcpdump process capturing raw Mid-360 UDP packets to a pcap. - - Single responsibility: write the wire. Pairs with a memory2 recorder that - captures the decoded/derived streams; together they make a session that can - be replayed offline. - """ + """Owns a tcpdump process capturing raw Mid-360 UDP packets to a pcap.""" config: LivoxPcapRecorderConfig diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py index 6a95688003..03822a5919 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py @@ -14,16 +14,10 @@ """Blueprint: FastLio2 fed by a VirtualMid360 replaying a recorded pcap. -VirtualMid360 stands up a fake Mid-360 on a virtual NIC and replays the pcap -over the Livox wire protocol; FastLio2 connects to it exactly as it would to -real hardware (no replay_pcap — it runs in live SDK mode and never knows the -sensor is synthetic). Use this to re-run a recorded session through the live -SLAM path, e.g. to reproduce (or rule out) the velocity-spike divergence. - -The two talk over UDP on lidar_ip/host_ip, so they need a network where those -IPs are reachable: the e2e harness runs VirtualMid360 in a `lidar` netns and -FastLio2 in a `drv` netns joined by a veth carrying lidar_ip. See -tools/replay_via_virtual_mid360.sh for the full netns setup + a db recording. +VirtualMid360 replays the pcap over the Livox wire protocol on a virtual NIC; +FastLio2 connects in live SDK mode, unaware the sensor is synthetic. They talk +over UDP on lidar_ip/host_ip, so the harness puts them in separate netns joined +by a veth — see fastlio2/tools/pcap_to_db.py. """ from dimos.core.coordination.blueprints import autoconnect From 45bffc397205e376476488d479d3a7b7863986db Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Tue, 16 Jun 2026 03:23:13 -0700 Subject: [PATCH 115/185] fastlio2: condense verbose acc_cov / filter_size comments in mid360.yaml --- .../sensors/lidar/fastlio2/config/mid360.yaml | 18 +++--------------- 1 file changed, 3 insertions(+), 15 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml b/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml index dd6ddab0c2..b3cca62af9 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml +++ b/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml @@ -11,25 +11,13 @@ preprocess: blind: 0.5 mapping: - # acc_cov is the divergence guard. At the upstream default (0.1) FAST-LIO - # odometry runs away to km/s on aggressive Go2 motion — the IMU acceleration - # prediction dominates the IESKF and diverges. Raising acc_cov down-weights - # that prediction. The divergence is stochastic (FAST-LIO is not bit-exact), - # so acc_cov shifts the bounded *probability* rather than being a hard switch: - # on the ruwik2_pt3 raw pcap, 0.3 diverges, 0.5 is borderline (bounds most - # runs but still diverged ~1 in 3), and 1.0 held bounded across every rep - # (peak ~4 m/s, matching Point-LIO). 1.0 for margin. A hard guarantee would - # need the velocity guardrail. See jhist dimos-fastlio-velocity-spike.md. + # acc_cov down-weights the IMU accel prediction; upstream 0.1 lets Go2 odom + # diverge to km/s, 1.0 holds bounded. See jhist dimos-fastlio-velocity-spike.md. acc_cov: 1.0 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 - # Scan voxel that feeds the IESKF. NOTE: an earlier finding that voxel 0.1 m - # bounds the velocity divergence turned out to be a re-encoding artifact - # (measured on a bag with synthesized per-point timestamps). On the raw pcap, - # voxel size does NOT bound divergence at any value — acc_cov above is the - # real guard. 0.1 m retained only for scan density, not for stability. - # See jhist dimos-fastlio-velocity-spike.md (2026-06-13 correction). + # Scan voxel for the IESKF; does NOT affect divergence (acc_cov is the guard). filter_size_surf: 0.1 filter_size_map: 0.1 fov_degree: 360 From bf858c495023401ff36c61dcdbfac0c7ed01833a Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Tue, 16 Jun 2026 04:16:32 -0700 Subject: [PATCH 116/185] fastlio2: remove mount/init_pose machinery + timing.hpp (mirror pointlio) - Drop the mount Pose config + init_pose computation/CLI/application (publish_lidar and publish_odometry now emit the raw SLAM frame, like pointlio). Removes the mount= args from the mobile/alfred/g1 blueprints (extrinsic handled via TF). - Delete cpp/timing.hpp + all timing::Section/Scope/maybe_flush usage. - cloud_filter.hpp was already removed earlier. --- dimos/control/blueprints/mobile.py | 6 - .../sensors/lidar/fastlio2/cpp/main.cpp | 149 ++---------------- .../sensors/lidar/fastlio2/cpp/timing.hpp | 116 -------------- .../hardware/sensors/lidar/fastlio2/module.py | 21 +-- .../robot/diy/alfred/blueprints/alfred_nav.py | 3 +- .../primitive/unitree_g1_onboard.py | 2 - 6 files changed, 15 insertions(+), 282 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/timing.hpp diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py index f16a277734..6a673e8207 100644 --- a/dimos/control/blueprints/mobile.py +++ b/dimos/control/blueprints/mobile.py @@ -35,10 +35,7 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.transport import LCMTransport from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.JointState import JointState from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.navigation.nav_stack.main import create_nav_stack, nav_stack_rerun_config @@ -138,14 +135,11 @@ def _flowbase_twist_base( # FlowBase + Livox MID-360 + FastLio2 SLAM + nav stack with click-to-drive in Rerun. The velocity # sink is ControlCoordinator + FlowBaseAdapter -_flowbase_mid360_mount = Pose(0.20, -0.20, 0.10, *Quaternion.from_euler(Vector3(0, 0, 0))) - coordinator_flowbase_nav = ( autoconnect( FastLio2.blueprint( host_ip=os.getenv("LIDAR_HOST_IP", "192.168.1.5"), lidar_ip=os.getenv("LIDAR_IP", "192.168.1.189"), - mount=_flowbase_mid360_mount, config="default.yaml", ), create_nav_stack( diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index 8ebebdf589..d8df8310c1 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -32,7 +32,6 @@ #include "livox_sdk_config.hpp" #include "dimos_native_module.hpp" -#include "timing.hpp" // dimos LCM message headers #include "geometry_msgs/Quaternion.hpp" @@ -75,47 +74,6 @@ static std::string g_frame_id; // required via --frame_id static std::string g_child_frame_id; // required via --child_frame_id static float g_frequency = 10.0f; -// Initial pose offset (applied to all SLAM outputs) -// Position offset -static double g_init_x = 0.0; -static double g_init_y = 0.0; -static double g_init_z = 0.0; -// Orientation offset as quaternion (identity = no rotation) -static double g_init_qx = 0.0; -static double g_init_qy = 0.0; -static double g_init_qz = 0.0; -static double g_init_qw = 1.0; - -// Helper: quaternion multiply (Hamilton product) q_out = q1 * q2 -static void quat_mul(double ax, double ay, double az, double aw, - double bx, double by, double bz, double bw, - double& ox, double& oy, double& oz, double& ow) { - ow = aw*bw - ax*bx - ay*by - az*bz; - ox = aw*bx + ax*bw + ay*bz - az*by; - oy = aw*by - ax*bz + ay*bw + az*bx; - oz = aw*bz + ax*by - ay*bx + az*bw; -} - -// Helper: rotate a vector by a quaternion v_out = q * v * q_inv -static void quat_rotate(double qx, double qy, double qz, double qw, - double vx, double vy, double vz, - double& ox, double& oy, double& oz) { - // t = 2 * cross(q_xyz, v) - double tx = 2.0 * (qy*vz - qz*vy); - double ty = 2.0 * (qz*vx - qx*vz); - double tz = 2.0 * (qx*vy - qy*vx); - // v_out = v + qw*t + cross(q_xyz, t) - ox = vx + qw*tx + (qy*tz - qz*ty); - oy = vy + qw*ty + (qz*tx - qx*tz); - oz = vz + qw*tz + (qx*ty - qy*tx); -} - -// Check if initial pose is non-identity -static bool has_init_pose() { - return g_init_x != 0.0 || g_init_y != 0.0 || g_init_z != 0.0 || - g_init_qx != 0.0 || g_init_qy != 0.0 || g_init_qz != 0.0 || g_init_qw != 1.0; -} - // Frame accumulator (Livox SDK raw → CustomMsg) static std::mutex g_pc_mutex; static std::vector g_accumulated_points; @@ -177,24 +135,11 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, pc.data_length = pc.row_step; pc.data.resize(pc.data_length); - // Apply init_pose (rotation + translation) to match the odometry frame: - // rotation corrects the mount axis, translation shifts the origin to ground z≈0. - const bool apply_init_pose = has_init_pose(); for (int i = 0; i < num_points; ++i) { float* dst = reinterpret_cast(pc.data.data() + i * 16); - if (apply_init_pose) { - double rx, ry, rz; - quat_rotate(g_init_qx, g_init_qy, g_init_qz, g_init_qw, - cloud->points[i].x, cloud->points[i].y, cloud->points[i].z, - rx, ry, rz); - dst[0] = static_cast(rx + g_init_x); - dst[1] = static_cast(ry + g_init_y); - dst[2] = static_cast(rz + g_init_z); - } else { - dst[0] = cloud->points[i].x; - dst[1] = cloud->points[i].y; - dst[2] = cloud->points[i].z; - } + dst[0] = cloud->points[i].x; + dst[1] = cloud->points[i].y; + dst[2] = cloud->points[i].z; dst[3] = cloud->points[i].intensity; } @@ -212,38 +157,13 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times msg.header = make_header(g_frame_id, timestamp); msg.child_frame_id = g_child_frame_id; - // Pose (apply initial pose offset: p_out = R_init * p_slam + t_init) - if (has_init_pose()) { - double rx, ry, rz; - quat_rotate(g_init_qx, g_init_qy, g_init_qz, g_init_qw, - odom.pose.pose.position.x, - odom.pose.pose.position.y, - odom.pose.pose.position.z, - rx, ry, rz); - msg.pose.pose.position.x = rx + g_init_x; - msg.pose.pose.position.y = ry + g_init_y; - msg.pose.pose.position.z = rz + g_init_z; - - double ox, oy, oz, ow; - quat_mul(g_init_qx, g_init_qy, g_init_qz, g_init_qw, - odom.pose.pose.orientation.x, - odom.pose.pose.orientation.y, - odom.pose.pose.orientation.z, - odom.pose.pose.orientation.w, - ox, oy, oz, ow); - msg.pose.pose.orientation.x = ox; - msg.pose.pose.orientation.y = oy; - msg.pose.pose.orientation.z = oz; - msg.pose.pose.orientation.w = ow; - } else { - msg.pose.pose.position.x = odom.pose.pose.position.x; - msg.pose.pose.position.y = odom.pose.pose.position.y; - msg.pose.pose.position.z = odom.pose.pose.position.z; - msg.pose.pose.orientation.x = odom.pose.pose.orientation.x; - msg.pose.pose.orientation.y = odom.pose.pose.orientation.y; - msg.pose.pose.orientation.z = odom.pose.pose.orientation.z; - msg.pose.pose.orientation.w = odom.pose.pose.orientation.w; - } + msg.pose.pose.position.x = odom.pose.pose.position.x; + msg.pose.pose.position.y = odom.pose.pose.position.y; + msg.pose.pose.position.z = odom.pose.pose.position.z; + msg.pose.pose.orientation.x = odom.pose.pose.orientation.x; + msg.pose.pose.orientation.y = odom.pose.pose.orientation.y; + msg.pose.pose.orientation.z = odom.pose.pose.orientation.z; + msg.pose.pose.orientation.w = odom.pose.pose.orientation.w; // Covariance (fixed-size double[36]) for (int i = 0; i < 36; ++i) { @@ -435,30 +355,8 @@ int main(int argc, char** argv) { ports.host_imu_data = mod.arg_int("host_imu_data_port", port_defaults.host_imu_data); ports.host_log_data = mod.arg_int("host_log_data_port", port_defaults.host_log_data); - // Initial pose offset [x, y, z, qx, qy, qz, qw] - { - std::string init_str = mod.arg("init_pose", ""); - if (!init_str.empty()) { - double vals[7] = {0, 0, 0, 0, 0, 0, 1}; - int n = 0; - size_t pos = 0; - while (pos < init_str.size() && n < 7) { - size_t comma = init_str.find(',', pos); - if (comma == std::string::npos) comma = init_str.size(); - vals[n++] = std::stod(init_str.substr(pos, comma - pos)); - pos = comma + 1; - } - g_init_x = vals[0]; g_init_y = vals[1]; g_init_z = vals[2]; - g_init_qx = vals[3]; g_init_qy = vals[4]; g_init_qz = vals[5]; g_init_qw = vals[6]; - } - } - if (debug) { printf("[fastlio2] Starting FAST-LIO2 + Livox Mid-360 native module\n"); - if (has_init_pose()) { - printf("[fastlio2] init_pose: xyz=(%.3f, %.3f, %.3f) quat=(%.4f, %.4f, %.4f, %.4f)\n", - g_init_x, g_init_y, g_init_z, g_init_qx, g_init_qy, g_init_qz, g_init_qw); - } printf("[fastlio2] lidar topic: %s\n", g_lidar_topic.empty() ? "(disabled)" : g_lidar_topic.c_str()); printf("[fastlio2] odometry topic: %s\n", @@ -502,17 +400,7 @@ int main(int argc, char** argv) { std::optional last_pc_publish; std::optional last_odom_publish; - // Per-section timing counters for `run_main_iter` (see timing.hpp; --debug only). - static timing::Section t_iter{"run_main_iter"}; - static timing::Section t_emit_check{"emit.lock+swap"}; - static timing::Section t_feed_lidar{"fast_lio.feed_lidar"}; - static timing::Section t_process{"fast_lio.process"}; - static timing::Section t_get_world_cloud{"fast_lio.get_world_cloud"}; - static timing::Section t_publish_lidar{"publish_lidar"}; - static timing::Section t_publish_odom{"publish_odometry"}; - auto run_main_iter = [&](std::chrono::steady_clock::time_point now) { - timing::Scope iter_scope(t_iter); // Lazy-seed the rate-limit bookmarks on the first iteration so they line // up with the wall clock instead of firing immediately. auto seed = now; @@ -532,7 +420,6 @@ int main(int argc, char** argv) { std::vector points; uint64_t frame_start = 0; { - timing::Scope s(t_emit_check); std::lock_guard lock(g_pc_mutex); auto check_now = now; if (check_now - *last_emit >= frame_interval) { @@ -556,29 +443,21 @@ int main(int argc, char** argv) { for (int i = 0; i < 3; i++) lidar_msg->rsvd[i] = 0; lidar_msg->point_num = static_cast(points.size()); lidar_msg->points = std::move(points); - timing::Scope s(t_feed_lidar); fast_lio.feed_lidar(lidar_msg); } // Run one FAST-LIO IESKF step. Cheap when the IMU/lidar queues // are empty; the heavy work happens after a feed_lidar above. - { - timing::Scope s(t_process); - fast_lio.process(); - } + fast_lio.process(); // Check for new SLAM results and publish (rate-limited). auto pose = fast_lio.get_pose(); if (!pose.empty() && (pose[0] != 0.0 || pose[1] != 0.0 || pose[2] != 0.0)) { double ts = get_publish_ts(); - auto world_cloud = ([&]() { - timing::Scope s(t_get_world_cloud); - return fast_lio.get_world_cloud(); - })(); + auto world_cloud = fast_lio.get_world_cloud(); if (world_cloud && !world_cloud->empty()) { // World-frame cloud at pointcloud_freq, published as-is (no downsampling). if (!g_lidar_topic.empty() && now - *last_pc_publish >= pc_interval) { - timing::Scope s(t_publish_lidar); publish_lidar(world_cloud, ts); last_pc_publish = now; } @@ -586,14 +465,10 @@ int main(int argc, char** argv) { // Pose + covariance, rate-limited to odom_freq. if (!g_odometry_topic.empty() && now - *last_odom_publish >= odom_interval) { - timing::Scope s(t_publish_odom); publish_odometry(fast_lio.get_odometry(), ts); last_odom_publish = now; } } - - // Periodic per-section summary to stderr (no-op when --debug off). - timing::maybe_flush(std::chrono::steady_clock::now()); }; // The Livox SDK opens UDP sockets and dispatches via its own callback diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/timing.hpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/timing.hpp deleted file mode 100644 index 759daf3c2e..0000000000 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/timing.hpp +++ /dev/null @@ -1,116 +0,0 @@ -// Copyright 2026 Dimensional Inc. -// SPDX-License-Identifier: Apache-2.0 -// -// Lightweight per-section timing for diagnosing where wall time goes in -// `run_main_iter`. Active only when --debug (the global `fastlio_debug` flag) -// is on, so non-debug runs pay one branch per scope. -// -// Usage: -// static timing::Section sec{"filter_cloud"}; -// { timing::Scope s(sec); /* ...do work... */ } -// timing::maybe_flush(now); // periodically - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "fast_lio_debug.hpp" // for the global `fastlio_debug` flag - -namespace timing { - -struct Section { - const char* name; - std::atomic count{0}; - std::atomic total_ns{0}; - std::atomic max_ns{0}; - - explicit Section(const char* n); - - void add(uint64_t ns) { - count.fetch_add(1, std::memory_order_relaxed); - total_ns.fetch_add(ns, std::memory_order_relaxed); - uint64_t prev = max_ns.load(std::memory_order_relaxed); - while (ns > prev && - !max_ns.compare_exchange_weak(prev, ns, std::memory_order_relaxed)) { - // prev is updated on failure by compare_exchange_weak. - } - } -}; - -inline std::vector& registry() { - static std::vector r; - return r; -} - -inline Section::Section(const char* n) : name(n) { - registry().push_back(this); -} - -struct Scope { - Section& sec; - std::chrono::steady_clock::time_point t0; - bool active; - - explicit Scope(Section& s) : sec(s), active(fastlio_debug) { - if (active) { - t0 = std::chrono::steady_clock::now(); - } - } - - ~Scope() { - if (!active) { - return; - } - auto dt = std::chrono::duration_cast( - std::chrono::steady_clock::now() - t0).count(); - sec.add(static_cast(dt)); - } -}; - -// Print one summary line per section to stderr every FLUSH_INTERVAL wall -// seconds, then reset accumulators. Mutex-guarded so concurrent threads don't -// produce torn output. -inline void maybe_flush(std::chrono::steady_clock::time_point now) { - if (!fastlio_debug) { - return; - } - constexpr auto FLUSH_INTERVAL = std::chrono::seconds(1); - static std::mutex mtx; - static std::chrono::steady_clock::time_point last; - std::lock_guard lock(mtx); - if (last.time_since_epoch().count() == 0) { - last = now; - return; - } - if (now - last < FLUSH_INTERVAL) { - return; - } - auto dt_ms = std::chrono::duration(now - last).count(); - last = now; - - for (Section* s : registry()) { - uint64_t c = s->count.exchange(0, std::memory_order_relaxed); - uint64_t tot = s->total_ns.exchange(0, std::memory_order_relaxed); - uint64_t mx = s->max_ns.exchange(0, std::memory_order_relaxed); - if (c == 0) { - std::fprintf(stderr, "[timing] %-24s n=0\n", s->name); - continue; - } - double mean_us = static_cast(tot) / static_cast(c) / 1e3; - double max_us = static_cast(mx) / 1e3; - double total_ms = static_cast(tot) / 1e6; - double rate_hz = static_cast(c) * 1000.0 / dt_ms; - std::fprintf(stderr, - "[timing] %-24s n=%5lu rate=%7.1fHz mean=%8.3fus max=%9.3fus tot=%7.2fms\n", - s->name, - static_cast(c), - rate_hz, mean_us, max_us, total_ms); - } -} - -} // namespace timing diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 5d59aa3cac..104bad8d46 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -44,7 +44,6 @@ SDK_POINT_DATA_PORT, SDK_PUSH_MSG_PORT, ) -from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -68,10 +67,6 @@ class FastLio2Config(NativeModuleConfig): lidar_ip: str = "192.168.1.155" frequency: float = 10.0 - # Sensor mount pose — position + orientation of the sensor relative to ground. - # Converted to init_pose CLI arg [x, y, z, qx, qy, qz, qw] in model_post_init. - mount: Pose = Pose() - # "odom" frame: FastLio2 gives smooth continuous odometry; PGO publishes the # map→odom correction via TF. frame_id: str = FRAME_ODOM @@ -108,27 +103,15 @@ class FastLio2Config(NativeModuleConfig): # Resolved in __post_init__, passed as --config_path to the binary config_path: str | None = None - # init_pose is computed from mount; config is resolved to config_path - init_pose: list[float] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] - cli_exclude: frozenset[str] = frozenset({"config", "mount"}) + cli_exclude: frozenset[str] = frozenset({"config"}) def model_post_init(self, __context: object) -> None: - """Resolve config_path and compute init_pose from mount.""" + """Resolve config_path.""" super().model_post_init(__context) cfg = self.config if not cfg.is_absolute(): cfg = _CONFIG_DIR / cfg self.config_path = str(cfg.resolve()) - m = self.mount - self.init_pose = [ - m.x, - m.y, - m.z, - m.orientation.x, - m.orientation.y, - m.orientation.z, - m.orientation.w, - ] class FastLio2(NativeModule, perception.Lidar, perception.Odometry): diff --git a/dimos/robot/diy/alfred/blueprints/alfred_nav.py b/dimos/robot/diy/alfred/blueprints/alfred_nav.py index d0853d7d08..7bff17dfc7 100644 --- a/dimos/robot/diy/alfred/blueprints/alfred_nav.py +++ b/dimos/robot/diy/alfred/blueprints/alfred_nav.py @@ -22,7 +22,7 @@ from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.navigation.nav_stack.main import create_nav_stack, nav_stack_rerun_config -from dimos.robot.diy.alfred.config import ALFRED, LOCAL_PLANNER_PRECOMPUTED_PATHS +from dimos.robot.diy.alfred.config import LOCAL_PLANNER_PRECOMPUTED_PATHS from dimos.robot.diy.alfred.effector_high_level import AlfredHighLevel from dimos.visualization.vis_module import vis_module @@ -54,7 +54,6 @@ FastLio2.blueprint( host_ip=os.getenv("LIDAR_HOST_IP", "192.168.1.5"), lidar_ip=os.getenv("LIDAR_IP", "192.168.1.189"), - mount=ALFRED.internal_odom_offsets["mid360_link"], config="default.yaml", ), create_nav_stack(**nav_config), diff --git a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py index fce8fe4efa..1738841eaf 100644 --- a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py +++ b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py @@ -20,7 +20,6 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 from dimos.robot.unitree.g1.blueprints.primitive.unitree_g1_vis import unitree_g1_vis -from dimos.robot.unitree.g1.config import G1 from dimos.robot.unitree.g1.effectors.high_level.dds_sdk import G1HighLevelDdsSdk # Underscore-prefixed: a shared sub-blueprint, not a runnable blueprint of its own. @@ -28,7 +27,6 @@ FastLio2.blueprint( host_ip=os.getenv("LIDAR_HOST_IP", "192.168.123.164"), lidar_ip=os.getenv("LIDAR_IP", "192.168.123.120"), - mount=G1.internal_odom_offsets["mid360_link"], config="default.yaml", ), G1HighLevelDdsSdk.blueprint(), From 94cf54fd1716cacf74f523a80a03beb5baa01e89 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Tue, 16 Jun 2026 04:46:48 -0700 Subject: [PATCH 117/185] fastlio2: drop // ---- section banners (mirror pointlio's single-line section comments) --- dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp | 14 -------------- .../sensors/lidar/fastlio2/tools/pcap_to_db.py | 4 ---- 2 files changed, 18 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index d8df8310c1..0771a005ff 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -50,9 +50,7 @@ using livox_common::DATA_TYPE_IMU; using livox_common::DATA_TYPE_CARTESIAN_HIGH; using livox_common::DATA_TYPE_CARTESIAN_LOW; -// --------------------------------------------------------------------------- // Global state -// --------------------------------------------------------------------------- static std::atomic g_running{true}; static lcm::LCM* g_lcm = nullptr; @@ -80,9 +78,7 @@ static std::vector g_accumulated_points; static uint64_t g_frame_start_ns = 0; static bool g_frame_has_timestamp = false; -// --------------------------------------------------------------------------- // Helpers -// --------------------------------------------------------------------------- static uint64_t get_timestamp_ns(const LivoxLidarEthernetPacket* pkt) { uint64_t ns = 0; @@ -93,9 +89,7 @@ static uint64_t get_timestamp_ns(const LivoxLidarEthernetPacket* pkt) { using dimos::time_from_seconds; using dimos::make_header; -// --------------------------------------------------------------------------- // Publish lidar (world-frame point cloud) -// --------------------------------------------------------------------------- static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, const std::string& topic = "") { @@ -146,9 +140,7 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, g_lcm->publish(chan, &pc); } -// --------------------------------------------------------------------------- // Publish odometry -// --------------------------------------------------------------------------- static void publish_odometry(const custom_messages::Odometry& odom, double timestamp) { if (!g_lcm) return; @@ -183,9 +175,7 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times } -// --------------------------------------------------------------------------- // Livox SDK callbacks -// --------------------------------------------------------------------------- static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/, LivoxLidarEthernetPacket* data, void* /*client_data*/) { @@ -287,17 +277,13 @@ static void on_info_change(const uint32_t handle, const LivoxLidarInfo* info, EnableLivoxLidarImuData(handle, nullptr, nullptr); } -// --------------------------------------------------------------------------- // Signal handling -// --------------------------------------------------------------------------- static void signal_handler(int /*sig*/) { g_running.store(false); } -// --------------------------------------------------------------------------- // Main -// --------------------------------------------------------------------------- int main(int argc, char** argv) { dimos::NativeModule mod(argc, argv); diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index 1abd17e7e2..7fc50ca561 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -109,9 +109,7 @@ def _table_stats(db_path: Path, table: str) -> tuple[int, float, float]: con.close() -# --------------------------------------------------------------------------- # Orchestrator: set up the netns + fake sensor, drive the consumer, tear down. -# --------------------------------------------------------------------------- def _sudo(*args: str, check: bool = True) -> subprocess.CompletedProcess[bytes]: @@ -291,9 +289,7 @@ def _orchestrate(args: argparse.Namespace) -> int: return rc -# --------------------------------------------------------------------------- # Consumer: FastLio2 live SDK + recorder. Runs inside the drv netns. -# --------------------------------------------------------------------------- def _consume(args: argparse.Namespace) -> int: From aca9e5685e66dd3e35c9922861bcf2c88b8ea812 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Tue, 16 Jun 2026 05:20:05 -0700 Subject: [PATCH 118/185] fastlio2: tighten the last multi-line pcap_to_db stop-logic comment --- dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index 7fc50ca561..9530e1ab57 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -248,10 +248,9 @@ def _orchestrate(args: argparse.Namespace) -> int: vm.stdin.write((vm_cfg + "\n").encode()) vm.stdin.close() - # virtual_mid360 streams the pcap once, then logs "data stream - # finished"; wait for that (capped by --duration), drain, then stop. - # (Watching the db for stagnation is unreliable — a diverging FastLio2 - # keeps emitting long after the sensor goes quiet.) + # virtual_mid360 logs "data stream finished" after one pcap pass; wait + # for that (capped by --duration), then drain + stop. (Watching db + # stagnation is unreliable — a diverging FastLio2 emits after quiet.) deadline = time.time() + args.duration while time.time() < deadline: if vm.poll() is not None: From 2fbdb7979023983b66690d57411ac88b3e7fb483 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 10:49:37 +0800 Subject: [PATCH 119/185] virtual_mid360: use tracing::error! instead of eprintln! for startup errors --- .../sensors/lidar/livox/virtual_mid360/src/main.rs | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs index 7a53eaab2d..9a4a9835fc 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs @@ -221,7 +221,6 @@ impl VirtualMid360 { Err(msg) => { // Exit non-zero so the coordinator surfaces the fix command. tracing::error!("{msg}"); - eprintln!("\n[virtual_mid360] {msg}\n"); std::process::exit(2); } }; @@ -229,7 +228,7 @@ impl VirtualMid360 { let mcast_data: Ipv4Addr = match cfg.mcast_data.parse() { Ok(ip) => ip, Err(_) => { - eprintln!( + tracing::error!( "[virtual_mid360] invalid mcast_data '{}' — expected an IPv4 multicast \ address matching the consumer's Livox multicast_ip (default 224.1.1.5).", cfg.mcast_data @@ -241,7 +240,7 @@ impl VirtualMid360 { let packets = match parse_pcap(&cfg.pcap) { Ok(parsed) if !parsed.is_empty() => Arc::new(parsed), Ok(_) => { - eprintln!( + tracing::error!( "[virtual_mid360] pcap '{}' has no Livox UDP data packets. \ Check the path / that it's a Mid-360 capture, then re-run.", cfg.pcap @@ -249,7 +248,7 @@ impl VirtualMid360 { std::process::exit(2); } Err(err) => { - eprintln!( + tracing::error!( "[virtual_mid360] failed to read pcap '{}': {err}. Fix the path, then re-run.", cfg.pcap ); From 6d0833a7755c0d2132a5942f52572116858c6c55 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 10:53:30 +0800 Subject: [PATCH 120/185] virtual_mid360 recorder: default lidar_ip from DIMOS_MID360_LIDAR_IP env (mirror VirtualMid360); clear error if empty --- .../sensors/lidar/livox/virtual_mid360/recorder.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py index 562761e4c2..7710601bad 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py +++ b/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py @@ -97,7 +97,7 @@ def _kill(sig: str) -> str: class Mid360PcapRecorderConfig(ModuleConfig): pcap_path: Path = Field(default_factory=_default_pcap_path) iface: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_PCAP_IFACE", "")) - lidar_ip: str + lidar_ip: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_LIDAR_IP", "")) snaplen: int = 2048 stop_timeout: float = 5.0 @@ -132,6 +132,11 @@ def _filter(self) -> str: def _start_pcap(self) -> None: cfg = self.config + if not cfg.lidar_ip: + raise ValueError( + "Mid360PcapRecorder requires lidar_ip — pass lidar_ip=... or set " + "DIMOS_MID360_LIDAR_IP. It's the real Mid-360's IP, used to filter the capture." + ) path = Path(cfg.pcap_path).expanduser() path.parent.mkdir(parents=True, exist_ok=True) From 8b4c1302b3c92fc0fff0f1a2377ef92e4a81cafd Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 10:55:20 +0800 Subject: [PATCH 121/185] livox Mid360: honor DIMOS_MID360_LIDAR_IP env for lidar_ip (fallback to factory-default IP) --- dimos/hardware/sensors/lidar/livox/module.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index e7507913c3..c0b284bd72 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -27,8 +27,11 @@ from __future__ import annotations +import os from typing import TYPE_CHECKING +from pydantic import Field + from dimos.core.core import rpc from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import Out @@ -56,7 +59,10 @@ class Mid360Config(NativeModuleConfig): executable: str = "result/bin/mid360_native" build_command: str | None = "nix build .#mid360_native" host_ip: str = "192.168.1.5" - lidar_ip: str = "192.168.1.155" + # DIMOS_MID360_LIDAR_IP overrides; falls back to the Livox factory-default IP. + lidar_ip: str = Field( + default_factory=lambda: os.environ.get("DIMOS_MID360_LIDAR_IP", "192.168.1.155") + ) frequency: float = 10.0 enable_imu: bool = True frame_id: str = "lidar_link" From ca9d276cf04546f3b42128788b62cacca8bf363d Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 11:00:16 +0800 Subject: [PATCH 122/185] livox: share host_ip auto-detect between Mid360 driver and Point-LIO Extract Point-LIO's subnet-derive + UDP-bind-test host_ip logic into livox/net.py:resolve_host_ip and call it from Mid360.start() too, so the C++ driver gets the same auto-detection instead of a hardcoded host_ip. --- dimos/hardware/sensors/lidar/livox/module.py | 6 ++ dimos/hardware/sensors/lidar/livox/net.py | 85 +++++++++++++++++++ .../hardware/sensors/lidar/pointlio/module.py | 64 +------------- 3 files changed, 94 insertions(+), 61 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/livox/net.py diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index c0b284bd72..dce5b9fb07 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -35,6 +35,7 @@ from dimos.core.core import rpc from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import Out +from dimos.hardware.sensors.lidar.livox.net import resolve_host_ip from dimos.hardware.sensors.lidar.livox.ports import ( SDK_CMD_DATA_PORT, SDK_HOST_CMD_DATA_PORT, @@ -96,6 +97,11 @@ class Mid360(NativeModule, perception.Lidar, perception.IMU): @rpc def start(self) -> None: + # Auto-derive host_ip from a local NIC on the lidar's subnet (shared with + # Point-LIO) when the configured value isn't one of our IPs. + self.config.host_ip = resolve_host_ip( + self.config.lidar_ip, self.config.host_ip, label="Mid360" + ) super().start() @rpc diff --git a/dimos/hardware/sensors/lidar/livox/net.py b/dimos/hardware/sensors/lidar/livox/net.py new file mode 100644 index 0000000000..d7f98d8814 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/net.py @@ -0,0 +1,85 @@ +# 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. + +"""Shared host_ip auto-detection for Livox SDK modules (Mid360 driver, Point-LIO).""" + +from __future__ import annotations + +import ipaddress +import socket + +from dimos.utils.generic import get_local_ips +from dimos.utils.logging_config import setup_logger + +_logger = setup_logger() + + +def resolve_host_ip(lidar_ip: str, configured: str | None, *, label: str) -> str: + """Resolve the local host IP the Livox SDK should bind to. + + Uses ``configured`` when it's one of our local IPs; otherwise auto-derives + the local NIC on the lidar's /24 subnet. The chosen IP is UDP-bind-tested + before returning. Raises ``RuntimeError`` with an actionable message when no + local IP is on the lidar's subnet or the bind fails. ``label`` prefixes log + and error messages (e.g. ``"PointLio"``, ``"Mid360"``). + """ + local_ips = [ip for ip, _iface in get_local_ips()] + + if configured and configured in local_ips: + host_ip = configured + else: + try: + lidar_net = ipaddress.IPv4Network(f"{lidar_ip}/24", strict=False) + same_subnet = [ip for ip in local_ips if ipaddress.IPv4Address(ip) in lidar_net] + except (ValueError, TypeError): + same_subnet = [] + if not same_subnet: + subnet_prefix = ".".join(lidar_ip.split(".")[:3]) + msg = ( + f"{label}: cannot resolve host_ip — no local IP on the lidar's subnet " + f"(lidar {lidar_ip}).\n" + f" Local IPs found: {', '.join(local_ips) or '(none)'}\n" + f" → Bring up the lidar NIC, or set host_ip explicitly.\n" + f" → Check: ip addr | grep {subnet_prefix}\n" + f" → Or assign: sudo ip addr add {subnet_prefix}.5/24 dev \n" + ) + _logger.error(msg) + raise RuntimeError(msg) + host_ip = same_subnet[0] + if configured: + _logger.warning( + f"{label}: host_ip={configured!r} not local; using {host_ip!r} " + f"(on lidar {lidar_ip}'s subnet).", + ) + + _logger.info(f"{label} network check", host_ip=host_ip, lidar_ip=lidar_ip, local_ips=local_ips) + + # Bind a UDP socket on host_ip (port 0 = ephemeral) to catch a host already + # holding the Livox SDK ports before the native binary starts. + try: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.bind((host_ip, 0)) + except OSError as err: + _logger.error( + f"{label}: Cannot bind UDP socket on host_ip={host_ip!r}: {err}\n" + f" Another process may be using the Livox SDK ports.\n" + f" → Check: ss -ulnp | grep {host_ip}" + ) + raise RuntimeError( + f"{label}: Cannot bind UDP on {host_ip}: {err}. " + f"Check if another Livox/PointLio process is running." + ) from err + + _logger.info(f"{label} network check passed", host_ip=host_ip, lidar_ip=lidar_ip) + return host_ip diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index 1723aafeaa..86cac95413 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -28,10 +28,8 @@ from __future__ import annotations -import ipaddress import os from pathlib import Path -import socket from typing import TYPE_CHECKING, Annotated from pydantic import Field @@ -41,6 +39,7 @@ from dimos.core.core import rpc from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import Out +from dimos.hardware.sensors.lidar.livox.net import resolve_host_ip from dimos.hardware.sensors.lidar.livox.ports import ( SDK_CMD_DATA_PORT, SDK_HOST_CMD_DATA_PORT, @@ -60,11 +59,8 @@ from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.navigation.nav_stack.frames import FRAME_ODOM from dimos.spec import perception -from dimos.utils.generic import get_local_ips -from dimos.utils.logging_config import setup_logger _CONFIG_DIR = Path(__file__).parent / "config" -_logger = setup_logger() class PointLioConfig(NativeModuleConfig): @@ -170,63 +166,9 @@ def _validate_network(self) -> None: "PointLio: lidar_ip not set — it's network-specific. Set it in the config " "or via the DIMOS_POINTLIO_LIDAR_IP env var." ) - local_ips = [ip for ip, _iface in get_local_ips()] - # host_ip optional: derive the local NIC on lidar_ip's /24 when unset or - # not one of our IPs. - configured = self.config.host_ip - if configured and configured in local_ips: - host_ip = configured - else: - try: - lidar_net = ipaddress.IPv4Network(f"{lidar_ip}/24", strict=False) - same_subnet = [ip for ip in local_ips if ipaddress.IPv4Address(ip) in lidar_net] - except (ValueError, TypeError): - same_subnet = [] - if not same_subnet: - subnet_prefix = ".".join(lidar_ip.split(".")[:3]) - msg = ( - f"PointLio: cannot resolve host_ip — no local IP on the lidar's subnet " - f"(lidar {lidar_ip}).\n" - f" Local IPs found: {', '.join(local_ips) or '(none)'}\n" - f" → Bring up the lidar NIC, or set host_ip explicitly.\n" - f" → Check: ip addr | grep {subnet_prefix}\n" - f" → Or assign: sudo ip addr add {subnet_prefix}.5/24 dev \n" - ) - _logger.error(msg) - raise RuntimeError(msg) - host_ip = same_subnet[0] - self.config.host_ip = host_ip - if configured: - _logger.warning( - f"PointLio: host_ip={configured!r} not local; using {host_ip!r} " - f"(on lidar {lidar_ip}'s subnet).", - ) - - _logger.info( - "PointLio network check", host_ip=host_ip, lidar_ip=lidar_ip, local_ips=local_ips - ) - - # Check if we can bind a UDP socket on host_ip (port 0 = ephemeral). - try: - with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: - sock.bind((host_ip, 0)) - except OSError as err: - _logger.error( - f"PointLio: Cannot bind UDP socket on host_ip={host_ip!r}: {err}\n" - f" Another process may be using the Livox SDK ports.\n" - f" → Check: ss -ulnp | grep {host_ip}" - ) - raise RuntimeError( - f"PointLio: Cannot bind UDP on {host_ip}: {err}. " - f"Check if another Livox/PointLio process is running." - ) from err - - _logger.info( - "PointLio network check passed", - host_ip=host_ip, - lidar_ip=lidar_ip, - ) + # not one of our IPs (shared with the Mid360 driver). + self.config.host_ip = resolve_host_ip(lidar_ip, self.config.host_ip, label="PointLio") # Verify protocol port compliance (mypy will flag missing ports) From e9f8096aa3bd1337bbacfa1a881f0d7d45ef4123 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 11:08:11 +0800 Subject: [PATCH 123/185] livox Mid360: host_ip defaults to None (machine-specific, auto-detected); DIMOS_MID360_HOST_IP overrides --- dimos/hardware/sensors/lidar/livox/module.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index dce5b9fb07..688f28a846 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -20,7 +20,7 @@ from dimos.core.coordination.module_coordinator import ModuleCoordinator ModuleCoordinator.build(autoconnect( - Mid360.blueprint(host_ip="192.168.1.5"), + Mid360.blueprint(), # host_ip auto-detected; set lidar_ip if not the factory default SomeConsumer.blueprint(), )).loop() """ @@ -59,7 +59,9 @@ class Mid360Config(NativeModuleConfig): cwd: str | None = "cpp" executable: str = "result/bin/mid360_native" build_command: str | None = "nix build .#mid360_native" - host_ip: str = "192.168.1.5" + # host_ip is machine-specific — left unset it's auto-derived in start() from a + # local NIC on lidar_ip's subnet. DIMOS_MID360_HOST_IP overrides. + host_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_HOST_IP")) # DIMOS_MID360_LIDAR_IP overrides; falls back to the Livox factory-default IP. lidar_ip: str = Field( default_factory=lambda: os.environ.get("DIMOS_MID360_LIDAR_IP", "192.168.1.155") From 027f214221dd128c712432e975596d89b0ab7401 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 11:32:32 +0800 Subject: [PATCH 124/185] clean --- dimos/hardware/sensors/lidar/livox/module.py | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index 688f28a846..021025677c 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -54,15 +54,10 @@ class Mid360Config(NativeModuleConfig): - """Config for the C++ Mid-360 native module.""" - cwd: str | None = "cpp" executable: str = "result/bin/mid360_native" build_command: str | None = "nix build .#mid360_native" - # host_ip is machine-specific — left unset it's auto-derived in start() from a - # local NIC on lidar_ip's subnet. DIMOS_MID360_HOST_IP overrides. host_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_HOST_IP")) - # DIMOS_MID360_LIDAR_IP overrides; falls back to the Livox factory-default IP. lidar_ip: str = Field( default_factory=lambda: os.environ.get("DIMOS_MID360_LIDAR_IP", "192.168.1.155") ) @@ -85,13 +80,6 @@ class Mid360Config(NativeModuleConfig): class Mid360(NativeModule, perception.Lidar, perception.IMU): - """Livox Mid-360 LiDAR module backed by a native C++ binary. - - Ports: - lidar (Out[PointCloud2]): Point cloud frames at configured frequency. - imu (Out[Imu]): IMU data at ~200 Hz (if enabled). - """ - config: Mid360Config lidar: Out[PointCloud2] From 28b01f80721c29704175c9b9a13b907c7979a8f1 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 11:39:46 +0800 Subject: [PATCH 125/185] fix pre-existing mypy 3.10 errors (Unpack import, numpy no-any-return) Unrelated to this PR but failing CI's 'mypy --python-version 3.10 dimos': - pgo.py / pgo_auto.py: typing.Unpack is 3.11+, import from typing_extensions - mujoco_shm.py / go2 cdr.py: type the numpy .copy() locals to avoid no-any-return --- dimos/mapping/loop_closure/pgo.py | 3 ++- dimos/mapping/loop_closure/pgo_auto.py | 3 ++- dimos/robot/unitree/go2/dds/cdr.py | 2 +- dimos/simulation/engines/mujoco_shm.py | 9 ++++++--- 4 files changed, 11 insertions(+), 6 deletions(-) diff --git a/dimos/mapping/loop_closure/pgo.py b/dimos/mapping/loop_closure/pgo.py index b6c2b595c7..7adbba262f 100644 --- a/dimos/mapping/loop_closure/pgo.py +++ b/dimos/mapping/loop_closure/pgo.py @@ -46,12 +46,13 @@ from collections.abc import Callable, Iterator from dataclasses import dataclass -from typing import TYPE_CHECKING, Any, Literal, TypedDict, TypeVar, Unpack, cast +from typing import TYPE_CHECKING, Any, Literal, TypedDict, TypeVar, cast import numpy as np import open3d as o3d # type: ignore[import-untyped] import open3d.core as o3c # type: ignore[import-untyped] from scipy.spatial.transform import Rotation, Slerp +from typing_extensions import Unpack # typing.Unpack is 3.11+ from dimos.memory2.transform import Transformer from dimos.memory2.type.observation import Observation diff --git a/dimos/mapping/loop_closure/pgo_auto.py b/dimos/mapping/loop_closure/pgo_auto.py index 37dbb7d36b..0c81fffcfe 100644 --- a/dimos/mapping/loop_closure/pgo_auto.py +++ b/dimos/mapping/loop_closure/pgo_auto.py @@ -56,12 +56,13 @@ from collections.abc import Callable, Iterable, Iterator from dataclasses import dataclass -from typing import TYPE_CHECKING, Any, TypedDict, TypeVar, Unpack +from typing import TYPE_CHECKING, Any, TypedDict, TypeVar import numpy as np import open3d as o3d # type: ignore[import-untyped] import open3d.core as o3c # type: ignore[import-untyped] from scipy.spatial.transform import Rotation +from typing_extensions import Unpack # typing.Unpack is 3.11+ from dimos.memory2.store.memory import MemoryStore from dimos.memory2.stream import Stream diff --git a/dimos/robot/unitree/go2/dds/cdr.py b/dimos/robot/unitree/go2/dds/cdr.py index 581086596c..19fdfbb4b3 100644 --- a/dimos/robot/unitree/go2/dds/cdr.py +++ b/dimos/robot/unitree/go2/dds/cdr.py @@ -94,7 +94,7 @@ def prim(self, code: str) -> Any: def prim_array(self, code: str, n: int) -> np.ndarray: _, sz, dt = _PRIM[code] self.align(sz) - a = np.frombuffer(self.b, dt, n, self.p).copy() + a: np.ndarray = np.frombuffer(self.b, dt, n, self.p).copy() self.p += sz * n return a diff --git a/dimos/simulation/engines/mujoco_shm.py b/dimos/simulation/engines/mujoco_shm.py index f424147d65..2783cf672d 100644 --- a/dimos/simulation/engines/mujoco_shm.py +++ b/dimos/simulation/engines/mujoco_shm.py @@ -283,7 +283,8 @@ def read_kp_command(self, num_joints: int) -> NDArray[np.float64] | None: return None self._last_kp_cmd_seq = seq arr = self._array(self.shm.kp_t, MAX_JOINTS, np.float64) - return arr[:num_joints].copy() + result: NDArray[np.float64] = arr[:num_joints].copy() + return result def read_kd_command(self, num_joints: int) -> NDArray[np.float64] | None: seq = self._get_seq(SEQ_KD_CMD) @@ -291,7 +292,8 @@ def read_kd_command(self, num_joints: int) -> NDArray[np.float64] | None: return None self._last_kd_cmd_seq = seq arr = self._array(self.shm.kd_t, MAX_JOINTS, np.float64) - return arr[:num_joints].copy() + result: NDArray[np.float64] = arr[:num_joints].copy() + return result def read_tau_command(self, num_joints: int) -> NDArray[np.float64] | None: """Per-joint feedforward torque if a new command landed since last call.""" @@ -300,7 +302,8 @@ def read_tau_command(self, num_joints: int) -> NDArray[np.float64] | None: return None self._last_tau_cmd_seq = seq arr = self._array(self.shm.tau_t, MAX_JOINTS, np.float64) - return arr[:num_joints].copy() + result: NDArray[np.float64] = arr[:num_joints].copy() + return result def signal_ready(self, num_joints: int) -> None: ctrl = self._control() From f31b1c8960172a260e7a2efaeaba7d832f7d4edd Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 12:48:12 +0800 Subject: [PATCH 126/185] feat(pointlio): pcap_to_db --config (pwd-relative) + --odom/lidar-stream-name - --config: resolve against pwd, forward outer->inner, pass as PointLio config= (config_path was derived/clobbered) so it reaches the binary - --odom-stream-name / --lidar-stream-name: configurable db table names for the recorded streams (default pointlio_odometry/pointlio_lidar) - fix inner re-exec module path after tools/ -> scripts/ move --- .../pointlio/{tools => scripts}/pcap_to_db.py | 125 +++++++++++------- 1 file changed, 75 insertions(+), 50 deletions(-) rename dimos/hardware/sensors/lidar/pointlio/{tools => scripts}/pcap_to_db.py (87%) diff --git a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py similarity index 87% rename from dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py rename to dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py index 952ac324b0..0879a21bec 100644 --- a/dimos/hardware/sensors/lidar/pointlio/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py @@ -12,42 +12,27 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Run Point-LIO over a .pcap (via the rust virtual_mid360 replay) → .db. - -Point-LIO has no in-process replay anymore; the only replay path is the -``virtual_mid360`` rust module, which replays a recorded Mid-360 pcap *over the -wire* so an unmodified live Point-LIO connects to it as real hardware. This tool -orchestrates that end to end and records Point-LIO's outputs into a memory2 -SQLite database: - -* ``pointlio_odometry`` — the IESKF pose at the native odom rate. -* ``pointlio_lidar`` — the sensor-frame point cloud at the native rate. - -By default both ends run in the host namespace with ``host_ip``/``lidar_ip`` -aliased on a dummy interface (``--alias-iface``); pass ``--lidar-ns``/``--drv-ns`` -to isolate them in separate netns + veth instead (needed to run two replays at -once). Either way it configures interfaces/routes, so **it needs CAP_NET_ADMIN** -and shells out to ``sudo`` for the setup. Live Point-LIO + the recorder run -together (one coordinator, so their LCM streams wire up normally). Replay is real -time (Point-LIO is not deterministic), so two runs over the same pcap differ. - -The ``--db`` is optional: with no existing db a fresh one is built from scratch -(defaults to ``.db`` next to the pcap). With an existing db the two streams -are appended and time-aligned onto its clock, so Point-LIO output can be compared -against whatever it already holds (e.g. a fastlio replay). If either stream -already exists the run aborts unless ``--force`` drops them first. - -Run it as your normal user from the dimos6 venv — it shells out to ``sudo`` -for the privileged netns/veth bits itself:: - - source .venv/bin/activate - PCAP=$(python -c "from dimos.utils.data import get_data; \ - print(get_data('mid360_shake_stairs/mid360_shake_stairs.pcap'))") - python -m dimos.hardware.sensors.lidar.pointlio.tools.pcap_to_db --pcap "$PCAP" - # -> writes mid360_shake_stairs.db next to the sample. - -Two simultaneous runs (e.g. alongside a fastlio replay) must use distinct -namespaces/IPs — see --drv-ns / --lidar-ns / --host-ip / --lidar-ip. +""" +Usage: + # snippet that normally diverges + PCAP_PATH="$(python -c "from dimos.utils.data import get_data; print(get_data('ruwik2_part3/ruwik2_part3.pcap'))")" + + # gen .db from pcap with your config + python -m dimos.hardware.sensors.lidar.pointlio.tools.pcap_to_db \ + --config your_pointlio_conf.yaml \ + --pcap "$PCAP_PATH" + + # add to existing .db + DB="mem2.db" + python -m dimos.hardware.sensors.lidar.pointlio.tools.pcap_to_db --db "$DB" --pcap "$PCAP_PATH" + + # generate map + dimos map summary "$DB" + dimos map pose-fill "$DB" \ + --target pointlio_lidar \ + --pose-source pointlio_odometry \ + --out "${DB%.db}_posed.db" + dimos map global "${DB%.db}_posed.db" --lidar pointlio_lidar """ from __future__ import annotations @@ -82,7 +67,7 @@ # to cover Point-LIO's IMU-init latency. _STARTUP_TIMEOUT_SEC = 60.0 # virtual_mid360 crate dir (its `nix build .#default` produces result/bin/virtual_mid360). -# .../sensors/lidar/pointlio/tools/pcap_to_db.py -> parents[2] == .../sensors/lidar +# .../sensors/lidar/pointlio/scripts/pcap_to_db.py -> parents[2] == .../sensors/lidar _VM_DIR = Path(__file__).resolve().parents[2] / "livox" / "virtual_mid360" @@ -90,6 +75,9 @@ class _RecConfig(ModuleConfig): """Configures the recorder with the target db and timing conversion.""" db_path: str = "" + # db stream/table names the Point-LIO outputs are recorded under. + odom_stream_name: str = "pointlio_odometry" + lidar_stream_name: str = "pointlio_lidar" # Earliest existing ts in the db, or -1.0 if the db has no timestamped rows. ref_start_ts: float = -1.0 # Explicit offset override; NaN means auto-derive from ref_start_ts. @@ -116,8 +104,8 @@ async def main(self) -> AsyncIterator[None]: from dimos.memory2.store.sqlite import SqliteStore self._store = SqliteStore(path=self.config.db_path) - self._os = self._store.stream("pointlio_odometry", Odometry) - self._ls = self._store.stream("pointlio_lidar", PointCloud2) + self._os = self._store.stream(self.config.odom_stream_name, Odometry) + self._ls = self._store.stream(self.config.lidar_stream_name, PointCloud2) yield self._store.stop() @@ -310,13 +298,20 @@ def _run_outer(args: argparse.Namespace) -> int: print(f"[pcap_to_db] missing pcap: {pcap_path}", file=sys.stderr) return 2 db_path = Path(args.db).expanduser().resolve() if args.db else pcap_path.with_suffix(".db") + # Resolve --config against the *invoking* cwd (pwd-relative) up front; the + # PointLio config field otherwise resolves a relative path against its own + # config/ dir, never the pwd. Absolute path passes through unchanged. + config_path = str(Path(args.config).expanduser().resolve()) if args.config else "" + if config_path and not Path(config_path).exists(): + print(f"[pcap_to_db] missing --config: {config_path}", file=sys.stderr) + return 2 db_existed = db_path.exists() db_path.parent.mkdir(parents=True, exist_ok=True) # Fail fast on stream conflicts before touching the network. Only open an # *existing* db here — a new db is created by the (root) inner so it owns it # outright; SQLite refuses WAL writes when the file owner != the process uid. - pointlio_streams = ("pointlio_odometry", "pointlio_lidar") + pointlio_streams = (args.odom_stream_name, args.lidar_stream_name) if db_existed: from dimos.memory2.store.sqlite import SqliteStore @@ -388,10 +383,16 @@ def _run_outer(args: argparse.Namespace) -> int: *inner_prefix, sys.executable, "-m", - "dimos.hardware.sensors.lidar.pointlio.tools.pcap_to_db", + "dimos.hardware.sensors.lidar.pointlio.scripts.pcap_to_db", "--inner", "--db", str(db_path), + "--config", + config_path, + "--odom-stream-name", + args.odom_stream_name, + "--lidar-stream-name", + args.lidar_stream_name, "--odom-freq", str(args.odom_freq), "--ref-start-ts", @@ -462,8 +463,8 @@ def _run_outer(args: argparse.Namespace) -> int: else: _teardown_aliases(args.alias_iface) - o_cnt, o_min, o_max = _table_stats(db_path, "pointlio_odometry") - l_cnt = _table_stats(db_path, "pointlio_lidar")[0] + o_cnt, o_min, o_max = _table_stats(db_path, args.odom_stream_name) + l_cnt = _table_stats(db_path, args.lidar_stream_name)[0] if o_cnt == 0: print("[pcap_to_db] no odometry recorded — check the run above", file=sys.stderr) return 1 @@ -486,13 +487,20 @@ def _run_inner(args: argparse.Namespace) -> int: db_path = Path(args.db) time_offset = float("nan") if args.time_offset == "nan" else float(args.time_offset) + # `config` (not `config_path`, which PointLioConfig derives itself); already + # an absolute path by the time it reaches the inner, so it bypasses the + # config-dir-relative resolution. Omit when empty to keep the default.yaml. + pointlio_kwargs: dict[str, object] = dict( + host_ip=args.host_ip, + lidar_ip=args.lidar_ip, + odom_freq=args.odom_freq, + debug=False, + ) + if args.config: + pointlio_kwargs["config"] = args.config + blueprint = autoconnect( - PointLio.blueprint( - host_ip=args.host_ip, - lidar_ip=args.lidar_ip, - odom_freq=args.odom_freq, - debug=False, - ).remappings( + PointLio.blueprint(**pointlio_kwargs).remappings( [ (PointLio, "odometry", "pointlio_odometry"), (PointLio, "lidar", "pointlio_lidar"), @@ -500,6 +508,8 @@ def _run_inner(args: argparse.Namespace) -> int: ), _Rec.blueprint( db_path=str(db_path), + odom_stream_name=args.odom_stream_name, + lidar_stream_name=args.lidar_stream_name, ref_start_ts=args.ref_start_ts, time_offset=time_offset, ), @@ -514,7 +524,7 @@ def _run_inner(args: argparse.Namespace) -> int: try: while True: time.sleep(_POLL_SEC) - cnt, min_ts, max_ts = _table_stats(db_path, "pointlio_odometry") + cnt, min_ts, max_ts = _table_stats(db_path, args.odom_stream_name) if cnt == 0: # Bound the no-output wait so a binary that never starts fails # cleanly instead of hanging (stagnation timeout only arms once @@ -580,6 +590,21 @@ def main(argv: list[str]) -> int: parser.add_argument( "--warmup-sec", type=float, default=4.0, help="wait before streaming starts" ) + parser.add_argument( + "--config", + default="", + help="Point-LIO YAML config (pwd-relative or absolute; default: module's default.yaml)", + ) + parser.add_argument( + "--odom-stream-name", + default="pointlio_odometry", + help="db stream/table name for the recorded odometry", + ) + parser.add_argument( + "--lidar-stream-name", + default="pointlio_lidar", + help="db stream/table name for the recorded point cloud", + ) # Addressing knobs (override to run two replays at once). parser.add_argument("--host-ip", default="192.168.1.5") parser.add_argument("--lidar-ip", default="192.168.1.155") From 9c4724f95e45598507daf47d28214937f9529174 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 14:41:50 +0800 Subject: [PATCH 127/185] cleanup --- .../lidar/livox/virtual_mid360/module.py | 75 --- .../sensors/lidar/pointlio/recorder.py | 156 +++++ .../lidar/pointlio/scripts/pcap_to_db.py | 594 ++++-------------- .../{livox => }/virtual_mid360/Cargo.lock | 0 .../{livox => }/virtual_mid360/Cargo.toml | 2 +- .../{livox => }/virtual_mid360/blueprints.py | 2 +- .../{livox => }/virtual_mid360/flake.lock | 26 +- .../{livox => }/virtual_mid360/flake.nix | 16 +- .../sensors/lidar/virtual_mid360/module.py | 180 ++++++ .../{livox => }/virtual_mid360/recorder.py | 0 .../{livox => }/virtual_mid360/src/main.rs | 62 +- dimos/robot/all_blueprints.py | 7 +- 12 files changed, 514 insertions(+), 606 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py create mode 100644 dimos/hardware/sensors/lidar/pointlio/recorder.py rename dimos/hardware/sensors/lidar/{livox => }/virtual_mid360/Cargo.lock (100%) rename dimos/hardware/sensors/lidar/{livox => }/virtual_mid360/Cargo.toml (83%) rename dimos/hardware/sensors/lidar/{livox => }/virtual_mid360/blueprints.py (93%) rename dimos/hardware/sensors/lidar/{livox => }/virtual_mid360/flake.lock (69%) rename dimos/hardware/sensors/lidar/{livox => }/virtual_mid360/flake.nix (67%) create mode 100644 dimos/hardware/sensors/lidar/virtual_mid360/module.py rename dimos/hardware/sensors/lidar/{livox => }/virtual_mid360/recorder.py (100%) rename dimos/hardware/sensors/lidar/{livox => }/virtual_mid360/src/main.rs (89%) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py deleted file mode 100644 index 275d010ff5..0000000000 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py +++ /dev/null @@ -1,75 +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. - -"""Python NativeModule wrapper for the virtual_mid360 Rust binary. - -virtual_mid360 replays a recorded Livox Mid-360 pcap onto a virtual network -interface, rewriting packet timestamps to current-time and synthesizing the -Livox SDK2 control protocol so an unmodified consumer (e.g. PointLio) connects -to it as if it were a real sensor. It carries no dimos streams; it speaks the -Livox wire protocol over UDP, so consumers reach it by host_ip/lidar_ip, not by -stream wiring. - -Usage:: - - from dimos.hardware.sensors.lidar.livox.virtual_mid360.module import VirtualMid360 - from dimos.hardware.sensors.lidar.pointlio.module import PointLio - from dimos.core.coordination.blueprints import autoconnect - - autoconnect( - VirtualMid360.blueprint(pcap="/path/to/ruwik2.pcap"), - PointLio.blueprint(), - ) -""" - -from __future__ import annotations - -import os -from typing import TYPE_CHECKING - -from pydantic import Field - -from dimos.core.native_module import NativeModule, NativeModuleConfig - - -class VirtualMid360Config(NativeModuleConfig): - cwd: str | None = "." - executable: str = "result/bin/virtual_mid360" - build_command: str | None = "nix build .#default" - - # pcap/lidar_ip/host_ip/lidar_netns default from DIMOS_MID360_* env vars so - # blueprints needn't restate them. pcap/lidar_ip/host_ip are required — empty - # makes the binary error. - pcap: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_PCAP", "")) - # Replay speed; 1.0 = original timing. - rate: float = 1.0 - # Seconds to wait before streaming begins. - delay: float = 0.0 - # IP the fake lidar sends from (on this netns's veth). - lidar_ip: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_LIDAR_IP", "")) - # Host IP the data is delivered to (where the SDK listens). - host_ip: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_HOST_IP", "")) - # Network namespace the fake lidar runs in. - lidar_netns: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_NETNS", "lidar")) - # Multicast group for point/IMU. 224.1.1.5 is the Livox default the SDK joins. - mcast_data: str = "224.1.1.5" - - -class VirtualMid360(NativeModule): - config: VirtualMid360Config - - -# Verify the module constructs (mirrors the pointlio wrapper's check). -if TYPE_CHECKING: - VirtualMid360() diff --git a/dimos/hardware/sensors/lidar/pointlio/recorder.py b/dimos/hardware/sensors/lidar/pointlio/recorder.py new file mode 100644 index 0000000000..3088667a77 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/recorder.py @@ -0,0 +1,156 @@ +# 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 Point-LIO odometry + lidar into a memory2 SQLite db. + +Subscribes to a PointLio's ``odometry`` / ``lidar`` outputs (auto-connected by +matching stream name + type — no remappings needed) and appends them to a +memory2 store. Timestamps are converted onto the db's existing clock so a run +can be appended to an existing db (e.g. a fastlio replay) and compared on one +timeline. Owns the db lifecycle: refuses to clobber existing streams unless +``force``, and derives the alignment reference from whatever the db already holds. +""" + +from __future__ import annotations + +from collections.abc import AsyncIterator +import math +from pathlib import Path +import sqlite3 +import time + +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +# Below this the db's existing timestamps are sensor-boot seconds, not unix time. +_SENSOR_CLOCK_MAX = 1e8 +# Strictly-increasing tie-breaker so two samples never collide on ts. +_EPS = 1e-9 + + +def _existing_min_ts(db_path: Path) -> float: + """Min ts across the db's existing streams, or -1.0 if none/absent.""" + if not db_path.exists(): + return -1.0 + con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) + try: + tables = [ + row[0] + for row in con.execute("SELECT name FROM sqlite_master WHERE type='table'").fetchall() + ] + best: float | None = None + for table in tables: + if table.startswith("_") or table.startswith("sqlite_"): + continue + try: + cols = [col[1] for col in con.execute(f"PRAGMA table_info('{table}')").fetchall()] + if "ts" not in cols: + continue + row = con.execute(f"SELECT MIN(ts) FROM '{table}'").fetchone() + except sqlite3.OperationalError: + continue + if row and row[0] is not None: + best = row[0] if best is None else min(best, row[0]) + return best if best is not None else -1.0 + finally: + con.close() + + +class PointlioRecorderConfig(ModuleConfig): + """Configures the recorder with the target db and timing conversion.""" + + db_path: str = "" + # db stream/table names the Point-LIO outputs are recorded under. + odom_stream_name: str = "pointlio_odometry" + lidar_stream_name: str = "pointlio_lidar" + # Explicit offset override; NaN means auto-derive from the db's earliest ts. + time_offset: float = float("nan") + # Drop pre-existing odom/lidar streams instead of refusing to overwrite. + force: bool = False + + +class PointlioRecorder(Module): + config: PointlioRecorderConfig + + lidar: In[PointCloud2] + odometry: In[Odometry] + _offset: float | None = None + _ref_start_ts: float = -1.0 + _last_odom_ts: float = 0.0 + _last_lidar_ts: float = 0.0 + _odom_count: int = 0 + _lidar_count: int = 0 + + async def main(self) -> AsyncIterator[None]: + # Deferred: the store is opened in the worker process that runs main(), + # not at module-scan/import time on the host. + from dimos.memory2.store.sqlite import SqliteStore + + cfg = self.config + self._store = SqliteStore(path=cfg.db_path) + names = (cfg.odom_stream_name, cfg.lidar_stream_name) + existing = sorted(set(self._store.list_streams()) & set(names)) + if existing and not cfg.force: + raise RuntimeError( + f"PointlioRecorder: {Path(cfg.db_path).name} already has {existing}; " + "set force=True to overwrite" + ) + for name in existing: + self._store.delete_stream(name) + # Reference is the db's earliest ts *after* dropping our own old streams, + # so only the data we're aligning against (e.g. a fastlio replay) counts. + self._ref_start_ts = _existing_min_ts(Path(cfg.db_path)) + self._os = self._store.stream(cfg.odom_stream_name, Odometry) + self._ls = self._store.stream(cfg.lidar_stream_name, PointCloud2) + yield + self._store.stop() + + def _resolve_offset(self, first_ts: float) -> float: + override = self.config.time_offset + if not math.isnan(override): + return override + ref = self._ref_start_ts + if ref < 0.0 or ref < _SENSOR_CLOCK_MAX: + # Empty db, or db already on the sensor clock -> exact alignment. + return 0.0 + # db on wall-clock time -> start-align Point-LIO onto the db's earliest ts. + return ref - first_ts + + def _aligned_ts(self, raw_ts: float, last_ts: float) -> float: + """Convert a sensor ts onto the db clock, kept strictly above last_ts.""" + if self._offset is None: + self._offset = self._resolve_offset(raw_ts) + return max(raw_ts + self._offset, last_ts + _EPS) + + async def handle_odometry(self, msg: Odometry) -> None: + # `is not None`, not `or`: a real sensor ts of 0.0 must not fall back to + # wall time (would misclassify the stream's clock in _resolve_offset). + raw_ts_raw = getattr(msg, "ts", None) + raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() + ts = self._aligned_ts(raw_ts, self._last_odom_ts) + self._last_odom_ts = ts + pose = getattr(msg, "pose", None) + pose_inner = getattr(pose, "pose", None) if pose is not None else None + self._os.append(msg, ts=ts, pose=pose_inner) + self._odom_count += 1 + + async def handle_lidar(self, msg: PointCloud2) -> None: + raw_ts_raw = getattr(msg, "ts", None) + raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() + ts = self._aligned_ts(raw_ts, self._last_lidar_ts) + self._last_lidar_ts = ts + self._ls.append(msg, ts=ts) + self._lidar_count += 1 diff --git a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py index 0879a21bec..4f9bad05b7 100644 --- a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py @@ -33,150 +33,39 @@ --pose-source pointlio_odometry \ --out "${DB%.db}_posed.db" dimos map global "${DB%.db}_posed.db" --lidar pointlio_lidar + +One coordinator runs three autoconnected modules: a ``VirtualMid360`` replays the +pcap over the Livox wire (and aliases the host/lidar IPs onto a dummy interface +itself — needs CAP_NET_ADMIN/sudo, Linux only), an unmodified live ``PointLio`` +consumes it as real hardware, and a ``PointlioRecorder`` appends PointLio's +odometry/lidar into the db. This script just wires them and stops once the pcap +has drained. Replay is real time (Point-LIO is not deterministic), so runs differ. """ from __future__ import annotations import argparse -from collections.abc import AsyncIterator -import json -import math -import os from pathlib import Path -import signal import sqlite3 -import subprocess import sys import time +from typing import TYPE_CHECKING -from dimos.core.module import Module, ModuleConfig -from dimos.core.stream import In -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +if TYPE_CHECKING: + from dimos.core.coordination.blueprints import Blueprint -# Below this the db's existing timestamps are sensor-boot seconds, not unix time. -_SENSOR_CLOCK_MAX = 1e8 -# Strictly-increasing tie-breaker so two samples never collide on ts. -_EPS = 1e-9 # Poll the db on this cadence while the replay drains the pcap. _POLL_SEC = 1.0 # Stop after the odom stream has been stagnant this long (pcap fully drained). _STAGNANT_SEC = 5.0 -# No odometry row within this long after start = binary failed to come up -# (missing artifact, bad pcap, SLAM-init crash); bounds the poll loop. Generous -# to cover Point-LIO's IMU-init latency. +# No odometry within this long after start = Point-LIO failed to come up (missing +# artifact, bad pcap, SLAM-init crash); bounds the poll loop. Generous to cover +# Point-LIO's IMU-init latency. _STARTUP_TIMEOUT_SEC = 60.0 -# virtual_mid360 crate dir (its `nix build .#default` produces result/bin/virtual_mid360). -# .../sensors/lidar/pointlio/scripts/pcap_to_db.py -> parents[2] == .../sensors/lidar -_VM_DIR = Path(__file__).resolve().parents[2] / "livox" / "virtual_mid360" - - -class _RecConfig(ModuleConfig): - """Configures the recorder with the target db and timing conversion.""" - - db_path: str = "" - # db stream/table names the Point-LIO outputs are recorded under. - odom_stream_name: str = "pointlio_odometry" - lidar_stream_name: str = "pointlio_lidar" - # Earliest existing ts in the db, or -1.0 if the db has no timestamped rows. - ref_start_ts: float = -1.0 - # Explicit offset override; NaN means auto-derive from ref_start_ts. - time_offset: float = float("nan") - - -class _Rec(Module): - """Append Point-LIO odometry + lidar into a SQLite db with ts conversion. - - Underscore-prefixed so the blueprint registry generator skips it — this is - an internal helper for the tool, not a public robot module. - """ - - config: _RecConfig - pointlio_odometry: In[Odometry] - pointlio_lidar: In[PointCloud2] - _offset: float | None = None - _last_odom_ts: float = 0.0 - _last_lidar_ts: float = 0.0 - _odom_count: int = 0 - _lidar_count: int = 0 - - async def main(self) -> AsyncIterator[None]: - from dimos.memory2.store.sqlite import SqliteStore - - self._store = SqliteStore(path=self.config.db_path) - self._os = self._store.stream(self.config.odom_stream_name, Odometry) - self._ls = self._store.stream(self.config.lidar_stream_name, PointCloud2) - yield - self._store.stop() - - def _resolve_offset(self, first_ts: float) -> float: - override = self.config.time_offset - if not math.isnan(override): - return override - ref = self.config.ref_start_ts - if ref < 0.0 or ref < _SENSOR_CLOCK_MAX: - # Empty db, or db already on the sensor clock -> exact alignment. - return 0.0 - # db on wall-clock time -> start-align Point-LIO onto the db's earliest ts. - return ref - first_ts - - def _aligned_ts(self, raw_ts: float, last_ts: float) -> float: - """Convert a sensor ts onto the db clock, kept strictly above last_ts.""" - if self._offset is None: - self._offset = self._resolve_offset(raw_ts) - return max(raw_ts + self._offset, last_ts + _EPS) - async def handle_pointlio_odometry(self, msg: Odometry) -> None: - # `is not None`, not `or`: a real sensor ts of 0.0 must not fall back to - # wall time (would misclassify the stream's clock in _resolve_offset). - raw_ts_raw = getattr(msg, "ts", None) - raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() - ts = self._aligned_ts(raw_ts, self._last_odom_ts) - self._last_odom_ts = ts - pose = getattr(msg, "pose", None) - pose_inner = getattr(pose, "pose", None) if pose is not None else None - self._os.append(msg, ts=ts, pose=pose_inner) - self._odom_count += 1 - async def handle_pointlio_lidar(self, msg: PointCloud2) -> None: - raw_ts_raw = getattr(msg, "ts", None) - raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() - ts = self._aligned_ts(raw_ts, self._last_lidar_ts) - self._last_lidar_ts = ts - self._ls.append(msg, ts=ts) - self._lidar_count += 1 - - -def _db_ref_start_ts(db_path: Path) -> float: - """Min ts across the db's existing streams, or -1.0 if none/absent.""" - if not db_path.exists(): - return -1.0 - con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) - try: - tables = [ - row[0] - for row in con.execute("SELECT name FROM sqlite_master WHERE type='table'").fetchall() - ] - best: float | None = None - for table in tables: - if table.startswith("_") or table.startswith("sqlite_"): - continue - try: - cols = [col[1] for col in con.execute(f"PRAGMA table_info('{table}')").fetchall()] - if "ts" not in cols: - continue - row = con.execute(f"SELECT MIN(ts) FROM '{table}'").fetchone() - except sqlite3.OperationalError: - continue - if row and row[0] is not None: - best = row[0] if best is None else min(best, row[0]) - return best if best is not None else -1.0 - finally: - con.close() - - -def _table_stats(db_path: Path, table: str) -> tuple[int, float, float]: - """(count, min_ts, max_ts) for a stream table; zeros if absent.""" +def _odom_stats(db_path: Path, table: str) -> tuple[int, float, float]: + """(count, min_ts, max_ts) for the odom table; zeros if absent.""" if not db_path.exists(): return 0, 0.0, 0.0 con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) @@ -185,119 +74,102 @@ def _table_stats(db_path: Path, table: str) -> tuple[int, float, float]: row = con.execute(f"SELECT COUNT(*), MIN(ts), MAX(ts) FROM '{table}'").fetchone() except sqlite3.OperationalError: return 0, 0.0, 0.0 - cnt = row[0] or 0 - return cnt, row[1] or 0.0, row[2] or 0.0 + return row[0] or 0, row[1] or 0.0, row[2] or 0.0 finally: con.close() -# Network-namespace orchestration (outer process; needs CAP_NET_ADMIN via sudo). - - -def _sudo() -> list[str]: - return ["sudo"] - +def _build_blueprint(args: argparse.Namespace, db_path: Path, config_path: str) -> Blueprint: + """autoconnect(VirtualMid360 + PointLio + PointlioRecorder). -def _ns(args: list[str], check: bool = True) -> subprocess.CompletedProcess[bytes]: - return subprocess.run(_sudo() + args, check=check, capture_output=True) - - -def _teardown(drv: str, lidar: str, veth: str) -> None: - for cmd in ( - ["ip", "netns", "del", drv], - ["ip", "netns", "del", lidar], - ["ip", "link", "del", veth], - ): - _ns(cmd, check=False) - - -def _setup_netns( - drv: str, lidar: str, veth_drv: str, veth_lidar: str, host_ip: str, lidar_ip: str -) -> None: - """Recreate the drv/lidar veth pair with the Livox multicast routing.""" - _teardown(drv, lidar, veth_drv) - steps = [ - ["ip", "netns", "add", drv], - ["ip", "netns", "add", lidar], - ["ip", "link", "add", veth_drv, "type", "veth", "peer", "name", veth_lidar], - ["ip", "link", "set", veth_drv, "netns", drv], - ["ip", "link", "set", veth_lidar, "netns", lidar], - ["ip", "netns", "exec", drv, "ip", "addr", "add", f"{host_ip}/24", "dev", veth_drv], - ["ip", "netns", "exec", lidar, "ip", "addr", "add", f"{lidar_ip}/24", "dev", veth_lidar], - ] - for ns in (drv, lidar): - steps += [ - ["ip", "netns", "exec", ns, "ip", "link", "set", "lo", "up"], - ["ip", "netns", "exec", ns, "ip", "link", "set", "lo", "multicast", "on"], - ["ip", "netns", "exec", ns, "ip", "route", "add", "224.0.0.0/4", "dev", "lo"], - ] - steps += [ - ["ip", "netns", "exec", drv, "ip", "link", "set", veth_drv, "up"], - ["ip", "netns", "exec", lidar, "ip", "link", "set", veth_lidar, "up"], - ["ip", "netns", "exec", drv, "ip", "link", "set", veth_drv, "multicast", "on"], - ["ip", "netns", "exec", lidar, "ip", "link", "set", veth_lidar, "multicast", "on"], - # Mid-360 multicasts point/IMU to 224.1.1.5; broadcast detection to 255.255.255.255. - [ - "ip", - "netns", - "exec", - lidar, - "ip", - "route", - "add", - "255.255.255.255/32", - "dev", - veth_lidar, - ], - ["ip", "netns", "exec", lidar, "ip", "route", "add", "224.1.1.5/32", "dev", veth_lidar], - ] - for cmd in steps: - _ns(cmd) - - -def _teardown_aliases(iface: str) -> None: - _ns(["ip", "link", "del", iface], check=False) + PointLio's ``odometry``/``lidar`` outputs auto-wire to the recorder's + same-named inputs. VirtualMid360 carries no dimos streams — it speaks the + Livox wire protocol, reached by host_ip/lidar_ip, and sets up the NIC itself. + """ + from dimos.core.coordination.blueprints import autoconnect + from dimos.hardware.sensors.lidar.pointlio.module import PointLio + from dimos.hardware.sensors.lidar.pointlio.recorder import PointlioRecorder + from dimos.hardware.sensors.lidar.virtual_mid360.module import VirtualMid360 + # `config` (not `config_path`, which PointLioConfig derives itself); already + # an absolute path so it bypasses the config-dir-relative resolution. Omit + # when empty to keep the default.yaml. + pointlio_kwargs: dict[str, object] = dict( + host_ip=args.host_ip, lidar_ip=args.lidar_ip, odom_freq=args.odom_freq, debug=False + ) + if config_path: + pointlio_kwargs["config"] = config_path + + return autoconnect( + VirtualMid360.blueprint( + pcap=str(args.pcap_path), + rate=args.rate, + delay=args.warmup_sec, # hold streaming until PointLio's SDK is up + host_ip=args.host_ip, + lidar_ip=args.lidar_ip, + alias_iface=args.alias_iface, + # When the NIC is provisioned by hand, skip the module's own sudo + # (it runs in a tty-less worker where a password prompt can't appear). + setup_network=not args.no_network_setup, + ), + PointLio.blueprint(**pointlio_kwargs), + PointlioRecorder.blueprint( + db_path=str(db_path), + odom_stream_name=args.odom_stream_name, + lidar_stream_name=args.lidar_stream_name, + time_offset=float("nan") if args.time_offset is None else args.time_offset, + force=args.force, + ), + ).global_config(n_workers=4, robot_model="mid360_pointlio_pcap_to_db") -def _setup_aliases(iface: str, host_ip: str, lidar_ip: str) -> None: - """No-netns setup: put host_ip + lidar_ip on a dummy interface in the same - /24 with the Livox multicast + discovery-broadcast routes. Both processes - then run in the host namespace; multicast loopback (on by default) delivers - the fake lidar's point/IMU to the local SDK. (Still iproute2/Linux — macOS - would need the ifconfig-alias equivalent.)""" - _teardown_aliases(iface) - steps = [ - ["ip", "link", "add", iface, "type", "dummy"], - ["ip", "addr", "add", f"{host_ip}/24", "dev", iface], - ["ip", "addr", "add", f"{lidar_ip}/24", "dev", iface], - ["ip", "link", "set", iface, "up"], - ["ip", "link", "set", iface, "multicast", "on"], - ["ip", "route", "add", "224.1.1.5/32", "dev", iface], - ["ip", "route", "add", "255.255.255.255/32", "dev", iface], - ] - for cmd in steps: - _ns(cmd) +def _poll_until_drained(db_path: Path, odom_stream: str, max_sensor_sec: float) -> bool: + """Block until the pcap drains (odom stream goes stagnant) or a cap is hit; + False if Point-LIO never produced odometry within the startup timeout.""" + last_max = 0.0 + first_max: float | None = None + stagnant_since: float | None = None + start_time = time.time() + while True: + time.sleep(_POLL_SEC) + cnt, min_ts, max_ts = _odom_stats(db_path, odom_stream) + if cnt == 0: + # Stagnation timeout only arms once the first row exists, so bound the + # no-output wait separately or a dead binary would hang forever. + if time.time() - start_time > _STARTUP_TIMEOUT_SEC: + print( + f"[pcap_to_db] no odometry after {_STARTUP_TIMEOUT_SEC:.0f}s — Point-LIO " + "failed to start (check the binary, pcap path, and interface setup).", + file=sys.stderr, + flush=True, + ) + return False + continue + if first_max is None: + first_max = min_ts + if max_sensor_sec > 0 and (max_ts - first_max) >= max_sensor_sec: + print(f"[pcap_to_db] reached --max-sensor-sec={max_sensor_sec:.1f}s", flush=True) + return True + if max_ts == last_max: + if stagnant_since is None: + stagnant_since = time.time() + elif time.time() - stagnant_since > _STAGNANT_SEC: + return True + else: + last_max = max_ts + stagnant_since = None -def _resolve_vm_binary() -> str: - """Path to the virtual_mid360 binary; build it via nix if not present.""" - env = os.environ.get("DIMOS_MID360_BIN") - if env: - return env - out = _VM_DIR / "result" / "bin" / "virtual_mid360" - if out.exists(): - return str(out) - print("[pcap_to_db] building virtual_mid360 (nix build .#default)...", flush=True) - subprocess.run(["nix", "build", ".#default"], cwd=_VM_DIR, check=True) - return str(out) +def _run(args: argparse.Namespace) -> int: + from dimos.core.coordination.module_coordinator import ModuleCoordinator -def _run_outer(args: argparse.Namespace) -> int: pcap_path = Path(args.pcap).expanduser().resolve() if not pcap_path.exists(): print(f"[pcap_to_db] missing pcap: {pcap_path}", file=sys.stderr) return 2 + args.pcap_path = pcap_path db_path = Path(args.db).expanduser().resolve() if args.db else pcap_path.with_suffix(".db") + db_path.parent.mkdir(parents=True, exist_ok=True) # Resolve --config against the *invoking* cwd (pwd-relative) up front; the # PointLio config field otherwise resolves a relative path against its own # config/ dir, never the pwd. Absolute path passes through unchanged. @@ -305,260 +177,33 @@ def _run_outer(args: argparse.Namespace) -> int: if config_path and not Path(config_path).exists(): print(f"[pcap_to_db] missing --config: {config_path}", file=sys.stderr) return 2 - db_existed = db_path.exists() - db_path.parent.mkdir(parents=True, exist_ok=True) - - # Fail fast on stream conflicts before touching the network. Only open an - # *existing* db here — a new db is created by the (root) inner so it owns it - # outright; SQLite refuses WAL writes when the file owner != the process uid. - pointlio_streams = (args.odom_stream_name, args.lidar_stream_name) - if db_existed: - from dimos.memory2.store.sqlite import SqliteStore - store = SqliteStore(path=str(db_path)) - try: - existing = sorted(set(store.list_streams()) & set(pointlio_streams)) - if existing and not args.force: - print( - f"[pcap_to_db] {db_path.name} already has {existing}; pass --force to overwrite", - file=sys.stderr, - ) - return 2 - for name in existing: - store.delete_stream(name) - if existing: - print(f"[pcap_to_db] --force: dropped existing {existing}", flush=True) - finally: - store.stop() - - ref_start_ts = _db_ref_start_ts(db_path) - vm_bin = _resolve_vm_binary() - net = ( - f"netns {args.drv_ns}/{args.lidar_ns}" - if args.lidar_ns - else f"aliases on {args.alias_iface}" - ) print( f"[pcap_to_db] pcap={pcap_path.name} db={db_path.name} " - f"({'append' if db_existed else 'new'}) rate={args.rate} " - f"{net} ips={args.host_ip}/{args.lidar_ip}", + f"({'append' if db_path.exists() else 'new'}) rate={args.rate} " + f"ips={args.host_ip}/{args.lidar_ip}", flush=True, ) - # netns is opt-in: with --lidar-ns the two ends get isolated namespaces (root - # inner); without it we alias both IPs on a dummy interface and run everything - # in the host ns as the normal user (so the db stays user-owned — no chown). - use_netns = bool(args.lidar_ns) - vm_proc: subprocess.Popen[bytes] | None = None - inner: subprocess.Popen[bytes] | None = None - # Setup lives inside the try so the finally always cleans up + (netns mode) - # restores db ownership, even if setup raises (e.g. missing CAP_NET_ADMIN). + coord = None try: - if use_netns: - # Root inner can't write a user-owned WAL db (SQLite refuses cross-uid - # writes), so hand it to root; restored in the finally. - if db_existed: - for suffix in ("", "-wal", "-shm"): - sidecar = Path(f"{db_path}{suffix}") - if sidecar.exists(): - _ns(["chown", "0:0", str(sidecar)], check=False) - _setup_netns( - args.drv_ns, - args.lidar_ns, - args.veth_drv, - args.veth_lidar, - args.host_ip, - args.lidar_ip, - ) - inner_prefix = [*_sudo(), "ip", "netns", "exec", args.drv_ns] - vm_prefix = [*_sudo(), "ip", "netns", "exec", args.lidar_ns] - else: - _setup_aliases(args.alias_iface, args.host_ip, args.lidar_ip) - inner_prefix = [] - vm_prefix = [] - - # Recorder + live Point-LIO run together (one coordinator). - inner = subprocess.Popen( - [ - *inner_prefix, - sys.executable, - "-m", - "dimos.hardware.sensors.lidar.pointlio.scripts.pcap_to_db", - "--inner", - "--db", - str(db_path), - "--config", - config_path, - "--odom-stream-name", - args.odom_stream_name, - "--lidar-stream-name", - args.lidar_stream_name, - "--odom-freq", - str(args.odom_freq), - "--ref-start-ts", - repr(ref_start_ts), - "--time-offset", - "nan" if args.time_offset is None else repr(args.time_offset), - "--max-sensor-sec", - str(args.max_sensor_sec), - "--host-ip", - args.host_ip, - "--lidar-ip", - args.lidar_ip, - ], - cwd=os.getcwd(), - ) - - # Give Point-LIO a moment to come up before the sensor starts streaming. - time.sleep(args.warmup_sec) - vm_cfg = json.dumps( - { - "topics": {}, - "config": { - "pcap": str(pcap_path), - "rate": args.rate, - "delay": 0.0, - "lidar_ip": args.lidar_ip, - "host_ip": args.host_ip, - "lidar_netns": args.lidar_ns, - }, - } - ).encode() - vm_proc = subprocess.Popen([*vm_prefix, vm_bin], stdin=subprocess.PIPE) - assert vm_proc.stdin is not None - vm_proc.stdin.write(vm_cfg) - vm_proc.stdin.close() - - # The inner exits itself once the odom stream goes stagnant (pcap drained). - inner.wait() + coord = ModuleCoordinator.build(_build_blueprint(args, db_path, config_path)) + drained = _poll_until_drained(db_path, args.odom_stream_name, args.max_sensor_sec) finally: - if use_netns: - # Kill ONLY this run's processes — those in its (uniquely named) - # namespaces — as root (the binaries run under sudo). `ip netns pids` - # scopes precisely, so a concurrent run elsewhere is untouched. - for ns in (args.lidar_ns, args.drv_ns): - pids = _ns(["ip", "netns", "pids", ns], check=False).stdout.decode().split() - if pids: - _ns(["kill", "-9", *pids], check=False) - else: - # Alias mode: our own user-owned children — signal them directly - # (pointlio_native dies with its parent via PR_SET_PDEATHSIG). - for proc in (vm_proc, inner): - if proc and proc.poll() is None: - proc.terminate() - for proc in (vm_proc, inner): - if proc and proc.poll() is None: - try: - proc.wait(timeout=5) - except subprocess.TimeoutExpired: - proc.kill() - if use_netns: - _teardown(args.drv_ns, args.lidar_ns, args.veth_drv) - # Root inner owned the db files → hand them back to the invoking user. - uid, gid = os.getuid(), os.getgid() - for suffix in ("", "-wal", "-shm"): - sidecar = Path(f"{db_path}{suffix}") - if sidecar.exists(): - _ns(["chown", f"{uid}:{gid}", str(sidecar)], check=False) - else: - _teardown_aliases(args.alias_iface) + if coord is not None: + coord.stop() - o_cnt, o_min, o_max = _table_stats(db_path, args.odom_stream_name) - l_cnt = _table_stats(db_path, args.lidar_stream_name)[0] - if o_cnt == 0: + o_cnt, o_min, o_max = _odom_stats(db_path, args.odom_stream_name) + if o_cnt == 0 or not drained: print("[pcap_to_db] no odometry recorded — check the run above", file=sys.stderr) return 1 print( - f"[pcap_to_db] done odom={o_cnt} lidar={l_cnt} " - f"ts=[{o_min:.3f}, {o_max:.3f}] span={o_max - o_min:.1f}s", + f"[pcap_to_db] done odom={o_cnt} ts=[{o_min:.3f}, {o_max:.3f}] span={o_max - o_min:.1f}s", flush=True, ) return 0 -# Inner process: live Point-LIO + recorder, already inside the drv netns. - - -def _run_inner(args: argparse.Namespace) -> int: - from dimos.core.coordination.blueprints import autoconnect - from dimos.core.coordination.module_coordinator import ModuleCoordinator - from dimos.hardware.sensors.lidar.pointlio.module import PointLio - - db_path = Path(args.db) - time_offset = float("nan") if args.time_offset == "nan" else float(args.time_offset) - - # `config` (not `config_path`, which PointLioConfig derives itself); already - # an absolute path by the time it reaches the inner, so it bypasses the - # config-dir-relative resolution. Omit when empty to keep the default.yaml. - pointlio_kwargs: dict[str, object] = dict( - host_ip=args.host_ip, - lidar_ip=args.lidar_ip, - odom_freq=args.odom_freq, - debug=False, - ) - if args.config: - pointlio_kwargs["config"] = args.config - - blueprint = autoconnect( - PointLio.blueprint(**pointlio_kwargs).remappings( - [ - (PointLio, "odometry", "pointlio_odometry"), - (PointLio, "lidar", "pointlio_lidar"), - ] - ), - _Rec.blueprint( - db_path=str(db_path), - odom_stream_name=args.odom_stream_name, - lidar_stream_name=args.lidar_stream_name, - ref_start_ts=args.ref_start_ts, - time_offset=time_offset, - ), - ).global_config(n_workers=4, robot_model="mid360_pointlio_pcap_to_db") - coord = ModuleCoordinator.build(blueprint) - - last_max = 0.0 - first_max: float | None = None - stagnant_since: float | None = None - start_time = time.time() - startup_failed = False - try: - while True: - time.sleep(_POLL_SEC) - cnt, min_ts, max_ts = _table_stats(db_path, args.odom_stream_name) - if cnt == 0: - # Bound the no-output wait so a binary that never starts fails - # cleanly instead of hanging (stagnation timeout only arms once - # the first row exists). - if time.time() - start_time > _STARTUP_TIMEOUT_SEC: - print( - f"[pcap_to_db] no odometry after {_STARTUP_TIMEOUT_SEC:.0f}s — Point-LIO " - "failed to start (check the binary, pcap path, and netns wiring).", - file=sys.stderr, - flush=True, - ) - startup_failed = True - break - continue - if first_max is None: - first_max = min_ts - if args.max_sensor_sec > 0 and (max_ts - first_max) >= args.max_sensor_sec: - print( - f"[pcap_to_db] reached --max-sensor-sec={args.max_sensor_sec:.1f}s", flush=True - ) - break - if max_ts == last_max: - if stagnant_since is None: - stagnant_since = time.time() - elif time.time() - stagnant_since > _STAGNANT_SEC: - break - else: - last_max = max_ts - stagnant_since = None - finally: - coord.stop() - return 1 if startup_failed else 0 - - def main(argv: list[str]) -> int: parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("--pcap", help="Livox Mid-360 pcap capture") @@ -588,7 +233,10 @@ def main(argv: list[str]) -> int: ) parser.add_argument("--force", action="store_true", help="overwrite existing pointlio streams") parser.add_argument( - "--warmup-sec", type=float, default=4.0, help="wait before streaming starts" + "--warmup-sec", + type=float, + default=4.0, + help="seconds the fake lidar waits before streaming (lets Point-LIO come up first)", ) parser.add_argument( "--config", @@ -608,28 +256,20 @@ def main(argv: list[str]) -> int: # Addressing knobs (override to run two replays at once). parser.add_argument("--host-ip", default="192.168.1.5") parser.add_argument("--lidar-ip", default="192.168.1.155") - # netns is opt-in. Default (empty --lidar-ns) aliases both IPs on a dummy - # interface and runs in the host namespace; pass --lidar-ns/--drv-ns to - # isolate the two ends in separate namespaces instead. - parser.add_argument("--drv-ns", default="") - parser.add_argument("--lidar-ns", default="") - parser.add_argument("--veth-drv", default="veth-drv") - parser.add_argument("--veth-lidar", default="veth-lidar") parser.add_argument( - "--alias-iface", default="dimos-mid360", help="dummy iface for no-netns mode" + "--alias-iface", default="dimos-mid360", help="dummy iface the host/lidar IPs live on" + ) + parser.add_argument( + "--no-network-setup", + action="store_true", + help="don't let the module alias the NIC via sudo — you've set up host/lidar IPs " + "+ multicast routes yourself (e.g. on macOS where worker-side sudo can't prompt)", ) - # Internal: re-exec inside the drv netns to run the coordinator. - parser.add_argument("--inner", action="store_true", help=argparse.SUPPRESS) - parser.add_argument("--ref-start-ts", type=float, default=-1.0, help=argparse.SUPPRESS) args = parser.parse_args(argv) - if args.inner: - return _run_inner(args) if not args.pcap: parser.error("--pcap is required") - # Ignore SIGINT in the parent so the finally-block teardown always runs. - signal.signal(signal.SIGINT, signal.SIG_IGN) - return _run_outer(args) + return _run(args) if __name__ == "__main__": diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.lock b/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.lock similarity index 100% rename from dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.lock rename to dimos/hardware/sensors/lidar/virtual_mid360/Cargo.lock diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml b/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.toml similarity index 83% rename from dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml rename to dimos/hardware/sensors/lidar/virtual_mid360/Cargo.toml index edbfe42b43..2322574c17 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml +++ b/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.toml @@ -8,7 +8,7 @@ name = "virtual_mid360" path = "src/main.rs" [dependencies] -dimos-module = { path = "../../../../../../native/rust/dimos-module" } +dimos-module = { path = "../../../../../native/rust/dimos-module" } tokio = { version = "1", features = ["rt-multi-thread", "macros", "time", "net", "sync"] } serde = { version = "1", features = ["derive"] } tracing = "0.1" diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py b/dimos/hardware/sensors/lidar/virtual_mid360/blueprints.py similarity index 93% rename from dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py rename to dimos/hardware/sensors/lidar/virtual_mid360/blueprints.py index cc5d04898b..d3635a3f63 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py +++ b/dimos/hardware/sensors/lidar/virtual_mid360/blueprints.py @@ -19,8 +19,8 @@ """ from dimos.core.coordination.blueprints import autoconnect -from dimos.hardware.sensors.lidar.livox.virtual_mid360.module import VirtualMid360 from dimos.hardware.sensors.lidar.pointlio.module import PointLio +from dimos.hardware.sensors.lidar.virtual_mid360.module import VirtualMid360 from dimos.visualization.vis_module import vis_module demo_virtual_mid360_pointlio = autoconnect( diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.lock b/dimos/hardware/sensors/lidar/virtual_mid360/flake.lock similarity index 69% rename from dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.lock rename to dimos/hardware/sensors/lidar/virtual_mid360/flake.lock index 63a40f41f0..45bea619b2 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.lock +++ b/dimos/hardware/sensors/lidar/virtual_mid360/flake.lock @@ -1,20 +1,16 @@ { "nodes": { - "dimos-repo": { + "dimos-rust": { "flake": false, "locked": { - "lastModified": 1781505942, - "narHash": "sha256-NAjf4/pEbZGbOVFfkbdZCBbZJMrpErcQuFmqSeSRbRM=", - "ref": "refs/heads/jeff/feat/pointlio_native", - "rev": "76743e05777bdd812a0271308c71d7bb5d412619", - "revCount": 860, - "type": "git", - "url": "file:../../../../../.." + "path": "../../../../../native/rust", + "type": "path" }, "original": { - "type": "git", - "url": "file:../../../../../.." - } + "path": "../../../../../native/rust", + "type": "path" + }, + "parent": [] }, "flake-utils": { "inputs": { @@ -36,11 +32,11 @@ }, "nixpkgs": { "locked": { - "lastModified": 1779560665, - "narHash": "sha256-tpyBcxPpcQb8ukyNF7DoCwfSY3VPsxHoYwj00Cayv5o=", + "lastModified": 1781577229, + "narHash": "sha256-lrp67w8AulE9Ks53n27I45ADSzbOCn4H+CNW1Ck8B+8=", "owner": "NixOS", "repo": "nixpkgs", - "rev": "64c08a7ca051951c8eae34e3e3cb1e202fe36786", + "rev": "567a49d1913ce81ac6e9582e3553dd90a955875f", "type": "github" }, "original": { @@ -52,7 +48,7 @@ }, "root": { "inputs": { - "dimos-repo": "dimos-repo", + "dimos-rust": "dimos-rust", "flake-utils": "flake-utils", "nixpkgs": "nixpkgs" } diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.nix b/dimos/hardware/sensors/lidar/virtual_mid360/flake.nix similarity index 67% rename from dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.nix rename to dimos/hardware/sensors/lidar/virtual_mid360/flake.nix index a74d6bb71b..15593fc049 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.nix +++ b/dimos/hardware/sensors/lidar/virtual_mid360/flake.nix @@ -4,16 +4,18 @@ inputs = { nixpkgs.url = "github:NixOS/nixpkgs/nixos-unstable"; flake-utils.url = "github:numtide/flake-utils"; - # Relative git+file: reaches the repo root for the local dimos-module path - # deps (same approach as dimos/mapping/ray_tracing/rust). - dimos-repo = { url = "git+file:../../../../../.."; flake = false; }; + # Path input to the in-repo rust crates (mirrors pointlio/cpp's path: inputs). + # A plain path: (not git+file:) hashes the directory contents, so it carries no + # git-tree NAR hash — which varies by nix version / clean-vs-dirty checkout and + # breaks cross-machine builds. + dimos-rust = { url = "path:../../../../../native/rust"; flake = false; }; }; - outputs = { self, nixpkgs, flake-utils, dimos-repo }: + outputs = { self, nixpkgs, flake-utils, dimos-rust }: flake-utils.lib.eachDefaultSystem (system: let pkgs = import nixpkgs { inherit system; }; - sub = "dimos/hardware/sensors/lidar/livox/virtual_mid360"; + sub = "dimos/hardware/sensors/lidar/virtual_mid360"; src = pkgs.runCommand "virtual-mid360-src" {} '' mkdir -p $out/${sub} @@ -22,8 +24,8 @@ cp ${./Cargo.lock} $out/${sub}/Cargo.lock mkdir -p $out/native/rust - cp -r ${dimos-repo}/native/rust/dimos-module $out/native/rust/dimos-module - cp -r ${dimos-repo}/native/rust/dimos-module-macros $out/native/rust/dimos-module-macros + cp -r ${dimos-rust}/dimos-module $out/native/rust/dimos-module + cp -r ${dimos-rust}/dimos-module-macros $out/native/rust/dimos-module-macros ''; in { packages.default = pkgs.rustPlatform.buildRustPackage { diff --git a/dimos/hardware/sensors/lidar/virtual_mid360/module.py b/dimos/hardware/sensors/lidar/virtual_mid360/module.py new file mode 100644 index 0000000000..b901798695 --- /dev/null +++ b/dimos/hardware/sensors/lidar/virtual_mid360/module.py @@ -0,0 +1,180 @@ +# 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. + +"""Python NativeModule wrapper for the virtual_mid360 Rust binary. + + +Usage:: + + from dimos.hardware.sensors.lidar.virtual_mid360.module import VirtualMid360 + from dimos.hardware.sensors.lidar.pointlio.module import PointLio + from dimos.core.coordination.blueprints import autoconnect + + autoconnect( + VirtualMid360.blueprint(pcap="/path/to/ruwik2.pcap"), + PointLio.blueprint(), + ) +""" + +from __future__ import annotations + +import os +import subprocess +import sys +from typing import TYPE_CHECKING, Any + +from pydantic import Field + +from dimos.core.core import rpc +from dimos.core.native_module import NativeModule, NativeModuleConfig +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +# Synthetic /24 the host_ip + lidar_ip share so they route to each other. +_ALIAS_PREFIX_LEN = 24 +_ALIAS_NETMASK = "255.255.255.0" +# Host-route prefix length for the point/IMU multicast + discovery broadcast. +_HOST_ROUTE_LEN = 32 +# Livox SDK's discovery hello; the fake lidar answers it. +_DISCOVERY_BROADCAST = "255.255.255.255" +# macOS has no dummy interfaces — the synthetic IPs are aliased onto loopback. +_MACOS_IFACE = "lo0" + + +class VirtualMid360Config(NativeModuleConfig): + cwd: str | None = "." + executable: str = "result/bin/virtual_mid360" + build_command: str | None = "nix build .#default" + # The rust binary reads its config as a JSON object on stdin (required). + stdin_config: bool = True + # Keep the Python-only NIC knobs out of the CLI args mirrored to the binary. + cli_exclude: frozenset[str] = frozenset({"setup_network", "alias_iface"}) + + # pcap/lidar_ip/host_ip default from DIMOS_MID360_* env vars so blueprints + # needn't restate them. pcap is required — empty makes the binary error. + pcap: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_PCAP", "")) + # Replay speed; 1.0 = original timing. + rate: float = 1.0 + # Seconds to wait before streaming begins. + delay: float = 0.0 + # IP the fake lidar sends from (on the dummy alias interface). + lidar_ip: str = Field( + default_factory=lambda: os.environ.get("DIMOS_MID360_LIDAR_IP", "192.168.1.155") + ) + # Host IP the data is delivered to (where the SDK listens). + host_ip: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_HOST_IP", "")) + lidar_netns: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_NETNS", "")) + # Multicast group for point/IMU. 224.1.1.5 is the Livox default the SDK joins. + mcast_data: str = "224.1.1.5" + + # Auto-configure the virtual NIC (host_ip + lidar_ip on a dummy interface, + # with the Livox multicast/discovery routes) on start, via sudo. Set False + # if the interface is provisioned externally or real hardware is present. + setup_network: bool = True + # Name of the dummy interface the synthetic IPs are aliased onto. + alias_iface: str = "dimos-mid360" + + def to_config_dict(self) -> dict[str, Any]: + return {k: v for k, v in super().to_config_dict().items() if k not in self.cli_exclude} + + +class VirtualMid360(NativeModule): + config: VirtualMid360Config + + def _sudo(self, args: list[str], *, check: bool = True) -> None: + """Run a privileged command via sudo, raising on failure (when check).""" + result = subprocess.run(["sudo", *args], capture_output=True) + if check and result.returncode != 0: + stderr = result.stderr.decode("utf-8", errors="replace").strip() + raise RuntimeError( + f"[{self._module_label}] `sudo {' '.join(args)}` failed " + f"(exit {result.returncode}): {stderr}" + ) + + def _teardown_virtual_nic(self) -> None: + # Idempotent: missing aliases/routes/interface are fine (check=False). + cfg = self.config + if sys.platform == "darwin": + for ip in (cfg.host_ip, cfg.lidar_ip): + self._sudo(["ifconfig", _MACOS_IFACE, "-alias", ip], check=False) + for dst in (cfg.mcast_data, _DISCOVERY_BROADCAST): + self._sudo( + ["route", "-n", "delete", "-host", dst, "-interface", _MACOS_IFACE], check=False + ) + else: + self._sudo(["ip", "link", "del", cfg.alias_iface], check=False) + + def _setup_virtual_nic(self) -> None: + self._teardown_virtual_nic() + if sys.platform == "darwin": + self._setup_macos() + elif sys.platform.startswith("linux"): + self._setup_linux() + else: + raise RuntimeError( + f"[{self._module_label}] setup_network supports Linux (iproute2) and macOS; " + f"got {sys.platform}. Provision the interface yourself and set setup_network=False." + ) + logger.info( + "Virtual Mid-360 NIC configured", + module=self._module_label, + platform=sys.platform, + host_ip=self.config.host_ip, + lidar_ip=self.config.lidar_ip, + ) + + def _setup_macos(self) -> None: + """Alias host_ip + lidar_ip onto loopback and route the Livox point/IMU + multicast (and the discovery broadcast) there, so the local SDK sees the + fake sensor over lo0. macOS has no dummy interfaces / netns.""" + cfg = self.config + for ip in (cfg.host_ip, cfg.lidar_ip): + self._sudo(["ifconfig", _MACOS_IFACE, "alias", ip, "netmask", _ALIAS_NETMASK]) + self._sudo(["route", "-n", "add", "-host", cfg.mcast_data, "-interface", _MACOS_IFACE]) + # Best-effort: the limited broadcast may already be deliverable on lo0. + self._sudo( + ["route", "-n", "add", "-host", _DISCOVERY_BROADCAST, "-interface", _MACOS_IFACE], + check=False, + ) + + def _setup_linux(self) -> None: + cfg = self.config + iface = cfg.alias_iface + self._sudo(["ip", "link", "add", iface, "type", "dummy"]) + self._sudo(["ip", "addr", "add", f"{cfg.host_ip}/{_ALIAS_PREFIX_LEN}", "dev", iface]) + self._sudo(["ip", "addr", "add", f"{cfg.lidar_ip}/{_ALIAS_PREFIX_LEN}", "dev", iface]) + self._sudo(["ip", "link", "set", iface, "up"]) + self._sudo(["ip", "link", "set", iface, "multicast", "on"]) + self._sudo(["ip", "route", "add", f"{cfg.mcast_data}/{_HOST_ROUTE_LEN}", "dev", iface]) + self._sudo( + ["ip", "route", "add", f"{_DISCOVERY_BROADCAST}/{_HOST_ROUTE_LEN}", "dev", iface] + ) + + @rpc + def build(self) -> None: + super().build() + if self.config.setup_network: + self._setup_virtual_nic() + + @rpc + def stop(self) -> None: + super().stop() + if self.config.setup_network: + self._teardown_virtual_nic() + + +# Verify the module constructs (mirrors the pointlio wrapper's check). +if TYPE_CHECKING: + VirtualMid360() diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py b/dimos/hardware/sensors/lidar/virtual_mid360/recorder.py similarity index 100% rename from dimos/hardware/sensors/lidar/livox/virtual_mid360/recorder.py rename to dimos/hardware/sensors/lidar/virtual_mid360/recorder.py diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs b/dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs similarity index 89% rename from dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs rename to dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs index 9a4a9835fc..3c862ca7b9 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs +++ b/dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs @@ -1,8 +1,8 @@ // Fake Livox Mid-360 — replays a recorded pcap over a virtual NIC and synthesizes // the Livox SDK2 control handshake so an unmodified, live-mode pointlio ingests it -// through the real Livox SDK as if from a live sensor. Runs in the "lidar" netns -// (peer "drv" runs pointlio); on a setup failure the error prints the exact -// netns/veth commands to run. +// through the real Livox SDK as if from a live sensor. Namespace-agnostic: it just +// binds lidar_ip and sends UDP, so it works wherever the host_ip/lidar_ip are +// reachable — IPs aliased on an interface (host ns, incl. macOS lo0) or a netns. use dimos_module::{run, LcmTransport, Module}; use serde::Deserialize; @@ -39,11 +39,17 @@ struct Config { #[serde(default)] #[validate(range(min = 0.0, max = 3600.0))] delay: f64, - /// IP the fake lidar sends from (on this netns's veth). Required. + /// IP the fake lidar sends from. Required. lidar_ip: String, /// Host IP the data is delivered to (where the SDK listens). Required. host_ip: String, - /// Network namespace the fake lidar runs in. Required. + /// Network namespace the fake lidar runs in. Optional — empty/omitted means + /// the host namespace (the IPs are aliased onto an interface instead, which + /// is the default and the only option on macOS). Accepted for wire-config + /// compatibility but not acted on: the process is *placed* in the netns by + /// the launcher (`ip netns exec`), so the binary itself stays agnostic. + #[serde(default)] + #[allow(dead_code)] lidar_netns: String, /// Multicast group for point/IMU. 224.1.1.5 is the Livox default the SDK /// joins; override only to match a differently-configured consumer. @@ -182,32 +188,34 @@ fn ensure_interface(cfg: &Config) -> Result { // (or we're in the wrong namespace). let probe = UdpSocket::bind(SocketAddrV4::new(lidar_ip, CMD_PORT)); if probe.is_err() { - let netns = &cfg.lidar_netns; let lidar_addr = &cfg.lidar_ip; let host_addr = &cfg.host_ip; let mcast_group = &cfg.mcast_data; + // The VirtualMid360 module sets the NIC up automatically (setup_network, + // via sudo); this fires only when that was skipped/failed. Show the + // by-hand recipe for the current platform. + let how = if cfg!(target_os = "macos") { + format!( + "macOS — alias the IPs onto loopback and route the Livox multicast there:\n \ + sudo ifconfig lo0 alias {host_addr} netmask 255.255.255.0\n \ + sudo ifconfig lo0 alias {lidar_addr} netmask 255.255.255.0\n \ + sudo route -n add -host {mcast_group} -interface lo0\n \ + sudo route -n add -host 255.255.255.255 -interface lo0" + ) + } else { + format!( + "Linux — alias the IPs onto a dummy interface (no netns needed):\n \ + sudo ip link add dimos-mid360 type dummy\n \ + sudo ip addr add {host_addr}/24 dev dimos-mid360\n \ + sudo ip addr add {lidar_addr}/24 dev dimos-mid360\n \ + sudo ip link set dimos-mid360 up\n \ + sudo ip link set dimos-mid360 multicast on\n \ + sudo ip route add {mcast_group}/32 dev dimos-mid360\n \ + sudo ip route add 255.255.255.255/32 dev dimos-mid360" + ) + }; return Err(format!( - "cannot bind {lidar_addr}:{CMD_PORT} — the virtual network interface isn't set up \ - (or this process isn't in the '{netns}' netns).\n\ - Run this once (creates the lidar/drv veth pair), then re-run the module:\n\ - \n sudo ip netns add drv\n sudo ip netns add {netns}\n \ - sudo ip link add veth-drv type veth peer name veth-lidar\n \ - sudo ip link set veth-drv netns drv\n \ - sudo ip link set veth-lidar netns {netns}\n \ - sudo ip netns exec drv ip addr add {host_addr}/24 dev veth-drv\n \ - sudo ip netns exec {netns} ip addr add {lidar_addr}/24 dev veth-lidar\n \ - sudo ip netns exec drv ip link set veth-drv up\n \ - sudo ip netns exec {netns} ip link set veth-lidar up\n \ - sudo ip netns exec drv ip link set lo up\n \ - sudo ip netns exec {netns} ip link set lo up\n \ - sudo ip netns exec drv ip link set veth-drv multicast on\n \ - sudo ip netns exec {netns} ip link set veth-lidar multicast on\n \ - sudo ip netns exec {netns} ip route add 255.255.255.255/32 dev veth-lidar\n \ - sudo ip netns exec {netns} ip route add {mcast_group}/32 dev veth-lidar # point/IMU multicast\n \ - sudo ip netns exec drv ip route add 224.0.0.0/4 dev lo # LCM (dimos transport)\n \ - sudo ip netns exec {netns} ip route add 224.0.0.0/4 dev lo # LCM (dimos transport)\n\ - \nThen launch this module inside the lidar netns:\n \ - sudo ip netns exec {netns} " + "cannot bind {lidar_addr}:{CMD_PORT} — the virtual NIC isn't set up.\n{how}" )); } Ok(lidar_ip) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 7effefb997..291af8540e 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -54,7 +54,7 @@ "demo-object-scene-registration": "dimos.perception.demo_object_scene_registration:demo_object_scene_registration", "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", - "demo-virtual-mid360-pointlio": "dimos.hardware.sensors.lidar.livox.virtual_mid360.blueprints:demo_virtual_mid360_pointlio", + "demo-virtual-mid360-pointlio": "dimos.hardware.sensors.lidar.virtual_mid360.blueprints:demo_virtual_mid360_pointlio", "desk-marker-tf": "dimos.perception.fiducial.blueprints.desk_marker_tf:desk_marker_tf", "drone-agentic": "dimos.robot.drone.blueprints.agentic.drone_agentic:drone_agentic", "drone-basic": "dimos.robot.drone.blueprints.basic.drone_basic:drone_basic", @@ -188,7 +188,7 @@ "mcp-client": "dimos.agents.mcp.mcp_client.McpClient", "mcp-server": "dimos.agents.mcp.mcp_server.McpServer", "memory-module": "dimos.memory2.module.MemoryModule", - "mid360-pcap-recorder": "dimos.hardware.sensors.lidar.livox.virtual_mid360.recorder.Mid360PcapRecorder", + "mid360-pcap-recorder": "dimos.hardware.sensors.lidar.virtual_mid360.recorder.Mid360PcapRecorder", "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", @@ -212,6 +212,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-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", "real-sense-camera": "dimos.hardware.sensors.camera.realsense.camera.RealSenseCamera", @@ -238,7 +239,7 @@ "unitree-skill-container": "dimos.robot.unitree.unitree_skill_container.UnitreeSkillContainer", "unity-bridge-module": "dimos.simulation.unity.module.UnityBridgeModule", "video-arm-teleop-module": "dimos.teleop.quest.quest_extensions.VideoArmTeleopModule", - "virtual-mid360": "dimos.hardware.sensors.lidar.livox.virtual_mid360.module.VirtualMid360", + "virtual-mid360": "dimos.hardware.sensors.lidar.virtual_mid360.module.VirtualMid360", "vlm-agent": "dimos.agents.vlm_agent.VLMAgent", "vlm-stream-tester": "dimos.agents.vlm_stream_tester.VlmStreamTester", "voxel-grid-mapper": "dimos.mapping.voxels.VoxelGridMapper", From a5256e9f969e50dc1ff5d4be28caf30e26a7df4d Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 15:19:53 +0800 Subject: [PATCH 128/185] macos fixes --- .../sensors/lidar/virtual_mid360/Cargo.lock | 1 + .../sensors/lidar/virtual_mid360/Cargo.toml | 1 + .../sensors/lidar/virtual_mid360/src/main.rs | 67 ++++++++++++------- 3 files changed, 45 insertions(+), 24 deletions(-) diff --git a/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.lock b/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.lock index f9b68b4d57..53993a985c 100644 --- a/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.lock +++ b/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.lock @@ -711,6 +711,7 @@ version = "0.1.0" dependencies = [ "dimos-module", "serde", + "socket2 0.5.10", "tokio", "tracing", "validator", diff --git a/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.toml b/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.toml index 2322574c17..c3d9519cc3 100644 --- a/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.toml +++ b/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.toml @@ -11,5 +11,6 @@ path = "src/main.rs" dimos-module = { path = "../../../../../native/rust/dimos-module" } tokio = { version = "1", features = ["rt-multi-thread", "macros", "time", "net", "sync"] } serde = { version = "1", features = ["derive"] } +socket2 = "0.5" tracing = "0.1" validator = { version = "0.20", features = ["derive"] } diff --git a/dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs b/dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs index 3c862ca7b9..206e23c5f8 100644 --- a/dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs +++ b/dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs @@ -6,6 +6,7 @@ use dimos_module::{run, LcmTransport, Module}; use serde::Deserialize; +use socket2::{Domain, Protocol, Socket, Type}; use std::net::{Ipv4Addr, SocketAddrV4, UdpSocket}; use std::sync::atomic::{AtomicBool, Ordering}; use std::sync::Arc; @@ -269,8 +270,8 @@ impl VirtualMid360 { let rate = cfg.rate; let delay = cfg.delay; - // discovery responder (:56000) — answers the 0x0000 broadcast - spawn_discovery(lidar_ip, stop.clone()); + // discovery responder (:56000) — proactively announces + answers 0x0000 + spawn_discovery(lidar_ip, host_ip, stop.clone()); // control responder (:56100) — per-cmd ACKs; arms streaming on 0x0100 spawn_control(lidar_ip, armed.clone(), stop.clone()); // data streamer — point/IMU/status paced at `rate`, timestamps shifted to now @@ -281,39 +282,57 @@ impl VirtualMid360 { } } -fn spawn_discovery(lidar_ip: Ipv4Addr, stop: Arc) { +/// UDP socket bound with SO_REUSEADDR so it can share a port with the consumer +/// SDK's own sockets when both run in one network namespace — macOS (and Linux +/// alias mode) have no netns to separate the two endpoints. +fn reuse_bind(addr: SocketAddrV4) -> std::io::Result { + let socket = Socket::new(Domain::IPV4, Type::DGRAM, Some(Protocol::UDP))?; + socket.set_reuse_address(true)?; + let bind_addr: std::net::SocketAddr = addr.into(); + socket.bind(&bind_addr.into())?; + Ok(socket.into()) +} + +fn spawn_discovery(lidar_ip: Ipv4Addr, host_ip: Ipv4Addr, stop: Arc) { std::thread::spawn(move || { - let socket = match UdpSocket::bind(SocketAddrV4::new(Ipv4Addr::UNSPECIFIED, DISCOVERY_PORT)) - { + // Bind the lidar's detection port (not INADDR_ANY): SO_REUSEADDR + a + // specific source IP lets this coexist with the consumer SDK's own + // :56000 sockets in a shared namespace, and makes our packets arrive + // *from* lidar_ip:56000 (which is how the SDK identifies the device). + let socket = match reuse_bind(SocketAddrV4::new(lidar_ip, DISCOVERY_PORT)) { Ok(socket) => socket, Err(err) => { - tracing::error!("discovery bind :{DISCOVERY_PORT} failed: {err}"); + tracing::error!("discovery bind {lidar_ip}:{DISCOVERY_PORT} failed: {err}"); return; } }; - let _ = socket.set_broadcast(true); socket - .set_read_timeout(Some(Duration::from_millis(500))) + .set_read_timeout(Some(Duration::from_millis(200))) .ok(); - let broadcast_addr = SocketAddrV4::new(Ipv4Addr::BROADCAST, DISCOVERY_PORT); + // The SDK solicits lidars by broadcasting to 255.255.255.255, which macOS + // refuses to send — so it can never reach us. Instead we *proactively* + // unicast the search-ACK to the host's detection port; the SDK accepts an + // unsolicited detection response (it matches no request seq — none is + // required for cmd 0x0000) and registers the device. Harmless on Linux, + // where the broadcast path also works. + let host_detect = SocketAddrV4::new(host_ip, DISCOVERY_PORT); + let announce = build_ack(0x0000, 0, &discovery_ack_payload(lidar_ip)); let mut buffer = [0u8; 2048]; while !stop.load(Ordering::Relaxed) { - let len = match socket.recv_from(&mut buffer) { - Ok((len, _)) => len, - Err(_) => continue, - }; - if len < WRAPPER || buffer[0] != SOF { - continue; + let _ = socket.send_to(&announce, host_detect); + // Also answer a real broadcast solicitation if one arrives, echoing + // its seq (the original live/netns path). + if let Ok((len, _)) = socket.recv_from(&mut buffer) { + if len >= WRAPPER + && buffer[0] == SOF + && u16::from_le_bytes([buffer[8], buffer[9]]) == 0x0000 + && buffer[10] == 0 + { + let seq = u32::from_le_bytes([buffer[4], buffer[5], buffer[6], buffer[7]]); + let ack = build_ack(0x0000, seq, &discovery_ack_payload(lidar_ip)); + let _ = socket.send_to(&ack, host_detect); + } } - let cmd_id = u16::from_le_bytes([buffer[8], buffer[9]]); - let cmd_type = buffer[10]; - if cmd_id != 0x0000 || cmd_type != 0 { - continue; - } - let seq = u32::from_le_bytes([buffer[4], buffer[5], buffer[6], buffer[7]]); - // ACK describes the device (dev_type, serial, lidar_ip, cmd port). - let ack = build_ack(0x0000, seq, &discovery_ack_payload(lidar_ip)); - let _ = socket.send_to(&ack, broadcast_addr); } }); } From b4109a13999dec68cac859a6256ece0a695387df Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 16:18:19 +0800 Subject: [PATCH 129/185] macos fix --- .../lidar/livox/cpp/livox-sdk2-darwin.patch | 23 ++++++++++++++++++ .../pointlio/cpp/fastlio-resize-darwin.patch | 24 +++++++++++++++++++ .../sensors/lidar/pointlio/cpp/flake.nix | 12 +++++++++- .../sensors/lidar/virtual_mid360/src/main.rs | 5 ++++ 4 files changed, 63 insertions(+), 1 deletion(-) create mode 100644 dimos/hardware/sensors/lidar/pointlio/cpp/fastlio-resize-darwin.patch diff --git a/dimos/hardware/sensors/lidar/livox/cpp/livox-sdk2-darwin.patch b/dimos/hardware/sensors/lidar/livox/cpp/livox-sdk2-darwin.patch index 6a326dee27..2e0495d4d6 100644 --- a/dimos/hardware/sensors/lidar/livox/cpp/livox-sdk2-darwin.patch +++ b/dimos/hardware/sensors/lidar/livox/cpp/livox-sdk2-darwin.patch @@ -22,3 +22,26 @@ servaddr.sin_addr.s_addr = INADDR_ANY; } else { if(!multicast_ip.empty()){ +--- a/sdk_core/device_manager.cpp ++++ b/sdk_core/device_manager.cpp +@@ -259,7 +259,10 @@ + } + + bool DeviceManager::CreateDetectionChannel() { +-#ifdef WIN32 ++#if defined(WIN32) || defined(__APPLE__) ++ // macOS: no working broadcast on loopback; the fake-lidar consumer reaches us ++ // by unicast to the host detection socket. Skipping this INADDR_ANY socket ++ // also frees :56000 so the in-process replayer can bind lidar_ip:56000. + #else + detection_broadcast_socket_ = util::CreateSocket(kDetectionPort, true, true, true, "255.255.255.255", ""); + if (detection_broadcast_socket_ < 0) { +@@ -324,7 +327,7 @@ + } + } + +-#ifdef WIN32 ++#if defined(WIN32) || defined(__APPLE__) + #else + if (dev_type == kLivoxLidarTypeMid360) { + socket_t broadcast_socket = util::CreateSocket(host_net_info.push_msg_port, true, true, true, "255.255.255.255", ""); diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/fastlio-resize-darwin.patch b/dimos/hardware/sensors/lidar/pointlio/cpp/fastlio-resize-darwin.patch new file mode 100644 index 0000000000..8a3a803023 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/fastlio-resize-darwin.patch @@ -0,0 +1,24 @@ +Point-LIO laserMapping: resize (not reserve) crossmat_list / pbody_list. + +Written via operator[] in run_once and read as [idx+j+1] in Estimator.cpp, so +they need size, not just capacity. reserve()+operator[] is out-of-bounds UB: +benign on libstdc++ (Linux) but a SIGTRAP under macOS libc++ hardened +operator[]. Upstream-able to hku-mars. + +--- a/src/laserMapping.hpp ++++ b/src/laserMapping.hpp +@@ -642,8 +642,12 @@ + t2 = omp_get_wtime(); + + /*** iterated state estimation ***/ +- crossmat_list.reserve(feats_down_size); +- pbody_list.reserve(feats_down_size); ++ // resize (not reserve): both are indexed by [i] below and read as ++ // [idx+j+1] in Estimator.cpp, so they need size, not just capacity. ++ // reserve+operator[] is out-of-bounds UB — silently tolerated by ++ // libstdc++ but trapped by macOS libc++'s hardened operator[]. ++ crossmat_list.resize(feats_down_size); ++ pbody_list.resize(feats_down_size); + // pbody_ext_list.reserve(feats_down_size); + + for (size_t i = 0; i < feats_down_body->size(); i++) { diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix index 0d174f6e24..303a5aa093 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix @@ -66,6 +66,16 @@ livox-common = ../../common; + # Patch the Point-LIO fork in place: resize (not reserve) the per-point + # vectors in run_once, whose reserve+operator[] is out-of-bounds UB that + # macOS libc++'s hardened operator[] turns into a SIGTRAP. applyPatches + # gives a writable, patched copy of the read-only flake input. + fast-lio-patched = pkgs.applyPatches { + name = "fast-lio-pointlio-patched"; + src = fast-lio; + patches = [ ./fastlio-resize-darwin.patch ]; + }; + pointlio_native = pkgs.stdenv.mkDerivation { pname = "pointlio_native"; version = "0.2.0"; @@ -88,7 +98,7 @@ cmakeFlags = [ "-DCMAKE_POLICY_VERSION_MINIMUM=3.5" "-DFETCHCONTENT_SOURCE_DIR_DIMOS_LCM=${dimos-lcm}" - "-DFASTLIO_DIR=${fast-lio}" + "-DFASTLIO_DIR=${fast-lio-patched}" "-DLIVOX_COMMON_DIR=${livox-common}" ]; }; diff --git a/dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs b/dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs index 206e23c5f8..171c8d6ddc 100644 --- a/dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs +++ b/dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs @@ -288,6 +288,11 @@ impl VirtualMid360 { fn reuse_bind(addr: SocketAddrV4) -> std::io::Result { let socket = Socket::new(Domain::IPV4, Type::DGRAM, Some(Protocol::UDP))?; socket.set_reuse_address(true)?; + // SO_REUSEPORT too: the consumer SDK opens its own :56000 sockets (one on + // INADDR_ANY), and on macOS a wildcard bind can't be added over an existing + // specific bind with SO_REUSEADDR alone — so without this the two race and + // whichever loses fails to bind. REUSEPORT makes the binds order-independent. + socket.set_reuse_port(true)?; let bind_addr: std::net::SocketAddr = addr.into(); socket.bind(&bind_addr.into())?; Ok(socket.into()) From 4d3f1c16ce5bad4779ab5093f34d306873009ead Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 02:05:42 -0700 Subject: [PATCH 130/185] fix(pointlio): pcap_to_db doc paths + self-terminate on lidar-stream drain MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - docstring: tools.->scripts.pcap_to_db (moved), get_data arg -> mid360_shake_stairs/...pcap (was a double-.tar.gz name), real config path, Linux dummy-iface / macOS lo0 note (was 'Linux only') - _poll_until_drained: detect drain on the lidar stream's latest ts going flat (input-driven) instead of odometry's — Point-LIO dead-reckons at odom_freq after input stops, so the odom stream never goes stagnant and the run hung forever --- .../lidar/pointlio/scripts/pcap_to_db.py | 52 ++++++++++++------- 1 file changed, 32 insertions(+), 20 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py index 4f9bad05b7..f03476e7e7 100644 --- a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py @@ -14,17 +14,18 @@ """ Usage: - # snippet that normally diverges - PCAP_PATH="$(python -c "from dimos.utils.data import get_data; print(get_data('ruwik2_part3/ruwik2_part3.pcap'))")" + # fetch a sample Mid-360 capture (the get_data arg is the dir/file inside the + # LFS archive, NOT the archive name) + PCAP_PATH="$(python -c "from dimos.utils.data import get_data; print(get_data('mid360_shake_stairs/mid360_shake_stairs.pcap'))")" - # gen .db from pcap with your config - python -m dimos.hardware.sensors.lidar.pointlio.tools.pcap_to_db \ - --config your_pointlio_conf.yaml \ + # gen .db from pcap with your config (defaults to .db next to the pcap) + python -m dimos.hardware.sensors.lidar.pointlio.scripts.pcap_to_db \ + --config dimos/hardware/sensors/lidar/pointlio/config/default.yaml \ --pcap "$PCAP_PATH" # add to existing .db DB="mem2.db" - python -m dimos.hardware.sensors.lidar.pointlio.tools.pcap_to_db --db "$DB" --pcap "$PCAP_PATH" + python -m dimos.hardware.sensors.lidar.pointlio.scripts.pcap_to_db --db "$DB" --pcap "$PCAP_PATH" # generate map dimos map summary "$DB" @@ -35,8 +36,8 @@ dimos map global "${DB%.db}_posed.db" --lidar pointlio_lidar One coordinator runs three autoconnected modules: a ``VirtualMid360`` replays the -pcap over the Livox wire (and aliases the host/lidar IPs onto a dummy interface -itself — needs CAP_NET_ADMIN/sudo, Linux only), an unmodified live ``PointLio`` +pcap over the Livox wire (aliasing the host/lidar IPs onto a dummy interface on +Linux, or lo0 on macOS — needs CAP_NET_ADMIN/sudo), an unmodified live ``PointLio`` consumes it as real hardware, and a ``PointlioRecorder`` appends PointLio's odometry/lidar into the db. This script just wires them and stops once the pcap has drained. Replay is real time (Point-LIO is not deterministic), so runs differ. @@ -123,18 +124,26 @@ def _build_blueprint(args: argparse.Namespace, db_path: Path, config_path: str) ).global_config(n_workers=4, robot_model="mid360_pointlio_pcap_to_db") -def _poll_until_drained(db_path: Path, odom_stream: str, max_sensor_sec: float) -> bool: - """Block until the pcap drains (odom stream goes stagnant) or a cap is hit; - False if Point-LIO never produced odometry within the startup timeout.""" - last_max = 0.0 +def _poll_until_drained( + db_path: Path, odom_stream: str, lidar_stream: str, max_sensor_sec: float +) -> bool: + """Block until the pcap drains or a cap is hit; False if Point-LIO never + produced odometry within the startup timeout. + + Drain is detected on the *lidar* stream's latest timestamp going flat: lidar + is input-driven, so it stops advancing the moment the pcap is exhausted. The + odometry stream can't be used for this — Point-LIO keeps publishing odometry + (dead-reckoning) at odom_freq after input stops, with ever-advancing + timestamps, so its stream never looks stagnant and the run would hang.""" + last_lidar_max = 0.0 first_max: float | None = None stagnant_since: float | None = None start_time = time.time() while True: time.sleep(_POLL_SEC) - cnt, min_ts, max_ts = _odom_stats(db_path, odom_stream) - if cnt == 0: - # Stagnation timeout only arms once the first row exists, so bound the + odom_cnt, odom_min, odom_max = _odom_stats(db_path, odom_stream) + if odom_cnt == 0: + # Stagnation timeout only arms once odometry exists, so bound the # no-output wait separately or a dead binary would hang forever. if time.time() - start_time > _STARTUP_TIMEOUT_SEC: print( @@ -146,17 +155,18 @@ def _poll_until_drained(db_path: Path, odom_stream: str, max_sensor_sec: float) return False continue if first_max is None: - first_max = min_ts - if max_sensor_sec > 0 and (max_ts - first_max) >= max_sensor_sec: + first_max = odom_min + if max_sensor_sec > 0 and (odom_max - first_max) >= max_sensor_sec: print(f"[pcap_to_db] reached --max-sensor-sec={max_sensor_sec:.1f}s", flush=True) return True - if max_ts == last_max: + _, _, lidar_max = _odom_stats(db_path, lidar_stream) + if lidar_max == last_lidar_max: if stagnant_since is None: stagnant_since = time.time() elif time.time() - stagnant_since > _STAGNANT_SEC: return True else: - last_max = max_ts + last_lidar_max = lidar_max stagnant_since = None @@ -188,7 +198,9 @@ def _run(args: argparse.Namespace) -> int: coord = None try: coord = ModuleCoordinator.build(_build_blueprint(args, db_path, config_path)) - drained = _poll_until_drained(db_path, args.odom_stream_name, args.max_sensor_sec) + drained = _poll_until_drained( + db_path, args.odom_stream_name, args.lidar_stream_name, args.max_sensor_sec + ) finally: if coord is not None: coord.stop() From 723d6fa821d563f4c741a0bb1f0886d3899c531e Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 02:14:11 -0700 Subject: [PATCH 131/185] fix(virtual_mid360): derive Serialize on Config for NativeConfig bound main's dimos-module ModuleConfig->NativeConfig now requires Serialize (+ DeserializeOwned + Debug + Validate). The PR-merged-with-main clippy build failed on Config (Deserialize-only). Adding Serialize satisfies it; harmless against the branch's older dimos-module. --- dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs b/dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs index 171c8d6ddc..0463ffdee2 100644 --- a/dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs +++ b/dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs @@ -5,7 +5,7 @@ // reachable — IPs aliased on an interface (host ns, incl. macOS lo0) or a netns. use dimos_module::{run, LcmTransport, Module}; -use serde::Deserialize; +use serde::{Deserialize, Serialize}; use socket2::{Domain, Protocol, Socket, Type}; use std::net::{Ipv4Addr, SocketAddrV4, UdpSocket}; use std::sync::atomic::{AtomicBool, Ordering}; @@ -28,7 +28,7 @@ const DST_STATUS: u16 = 56201; // cmd_id whose ACK means the host finished configuring -> start streaming const CMD_WORKMODE: u16 = 0x0100; -#[derive(Debug, Deserialize, Validate)] +#[derive(Debug, Deserialize, Serialize, Validate)] #[serde(deny_unknown_fields)] struct Config { /// Recorded Mid-360 pcap (point/IMU/status UDP). Read fully into RAM. From 095350651095ea45fd8435add4f52166a4e76fb4 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 02:37:06 -0700 Subject: [PATCH 132/185] fix(pointlio): serialize Point-LIO EKF access across IMU + main threads MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The Livox SDK delivers IMU on its own callback thread (on_imu_data -> feed_imu) while the main loop runs feed_lidar/process/get_* on the same Point-LIO estimator. g_pc_mutex only guarded the point accumulator, not the EKF, so the two threads raced on estimator state — under load this stochastically produced a corrupt 2nd odometry trajectory interleaved with the real one (seen ~once in 5 full-length pcap replays). Add a dedicated g_lio_mutex held around feed_imu and around the main loop's EKF section; kept separate from g_pc_mutex (taken sequentially, never nested) so point packets still accumulate while the EKF processes. --- dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index f40cfba931..72e7a25094 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -65,6 +65,13 @@ static float g_frequency = 10.0f; // Frame accumulator (Livox SDK raw → CustomMsg) static std::mutex g_pc_mutex; +// Serializes all Point-LIO EKF access. The SDK delivers IMU on its own callback +// thread (on_imu_data → feed_imu) while the main loop runs feed_lidar/process/ +// get_* — Point-LIO's estimator is not thread-safe, so without this the two +// threads race on the EKF state and occasionally emit a corrupt 2nd trajectory. +// Distinct from g_pc_mutex (which only guards the point accumulator) so incoming +// point packets can still accumulate while the EKF is processing. +static std::mutex g_lio_mutex; static std::vector g_accumulated_points; static uint64_t g_frame_start_ns = 0; static bool g_frame_has_timestamp = false; @@ -245,6 +252,9 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, L auto* imu_pts = reinterpret_cast(data->data); uint16_t dot_num = data->dot_num; + // Serialize EKF access against the main loop (run_main_iter). Held across the + // whole packet so its samples feed atomically. + std::lock_guard lio_lock(g_lio_mutex); for (uint16_t point_idx = 0; point_idx < dot_num; ++point_idx) { auto imu_msg = boost::make_shared(); imu_msg->header.stamp = custom_messages::Time().fromSec(ts); @@ -413,6 +423,9 @@ int main(int argc, char** argv) { last_emit = now; } } + // Serialize EKF access against the SDK IMU callback (on_imu_data) for the + // rest of the iteration — feed_lidar/process/get_* all touch the estimator. + std::lock_guard lio_lock(g_lio_mutex); if (!points.empty()) { const size_t num_points = points.size(); auto lidar_msg = boost::make_shared(); From 4d3c79d85fe43dedd05398e5531a4e1ff458db70 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 02:56:19 -0700 Subject: [PATCH 133/185] fix(virtual_mid360): use #[native_config] for Config (main dimos-module API) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit main's dimos-module makes NativeConfig macro-only (no blanket impl), so the manual derive(Deserialize,Serialize,Validate)+deny_unknown_fields no longer satisfies it — the merged-with-main clippy build failed. Switch Config to the #[native_config] attribute (injects those derives + deny_unknown_fields + impl NativeConfig). Drop the serde(default)s/default_mcast_data it forbids — the Python VirtualMid360Config already sends every field unconditionally. --- .../sensors/lidar/virtual_mid360/src/main.rs | 26 +++++++------------ 1 file changed, 9 insertions(+), 17 deletions(-) diff --git a/dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs b/dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs index 0463ffdee2..17303b70d7 100644 --- a/dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs +++ b/dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs @@ -4,14 +4,12 @@ // binds lidar_ip and sends UDP, so it works wherever the host_ip/lidar_ip are // reachable — IPs aliased on an interface (host ns, incl. macOS lo0) or a netns. -use dimos_module::{run, LcmTransport, Module}; -use serde::{Deserialize, Serialize}; +use dimos_module::{native_config, run, LcmTransport, Module}; use socket2::{Domain, Protocol, Socket, Type}; use std::net::{Ipv4Addr, SocketAddrV4, UdpSocket}; use std::sync::atomic::{AtomicBool, Ordering}; use std::sync::Arc; use std::time::{Duration, Instant, SystemTime, UNIX_EPOCH}; -use validator::Validate; // ---- Livox SDK2 control wire format (SdkPacket) ---- const SOF: u8 = 0xAA; @@ -28,8 +26,11 @@ const DST_STATUS: u16 = 56201; // cmd_id whose ACK means the host finished configuring -> start streaming const CMD_WORKMODE: u16 = 0x0100; -#[derive(Debug, Deserialize, Serialize, Validate)] -#[serde(deny_unknown_fields)] +// native_config: every field required + supplied by the Python wrapper over +// stdin (no Rust-side serde defaults / Option). VirtualMid360Config sends all of +// these, so each is unconditionally present. Injects the +// Deserialize/Serialize/Validate derives + deny_unknown_fields + impl NativeConfig. +#[native_config] struct Config { /// Recorded Mid-360 pcap (point/IMU/status UDP). Read fully into RAM. pcap: String, @@ -37,31 +38,22 @@ struct Config { #[validate(range(min = 0.01, max = 1000.0))] rate: f64, /// Seconds to wait before streaming begins. - #[serde(default)] #[validate(range(min = 0.0, max = 3600.0))] delay: f64, - /// IP the fake lidar sends from. Required. + /// IP the fake lidar sends from. lidar_ip: String, - /// Host IP the data is delivered to (where the SDK listens). Required. + /// Host IP the data is delivered to (where the SDK listens). host_ip: String, - /// Network namespace the fake lidar runs in. Optional — empty/omitted means - /// the host namespace (the IPs are aliased onto an interface instead, which - /// is the default and the only option on macOS). Accepted for wire-config + /// Network namespace the fake lidar runs in. Accepted for wire-config /// compatibility but not acted on: the process is *placed* in the netns by /// the launcher (`ip netns exec`), so the binary itself stays agnostic. - #[serde(default)] #[allow(dead_code)] lidar_netns: String, /// Multicast group for point/IMU. 224.1.1.5 is the Livox default the SDK /// joins; override only to match a differently-configured consumer. - #[serde(default = "default_mcast_data")] mcast_data: String, } -fn default_mcast_data() -> String { - "224.1.1.5".into() -} - #[derive(Module)] #[module(setup = start)] struct VirtualMid360 { From ec5594d3bbc1b35d6246c350c0e43332711f9cf0 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 05:27:36 -0700 Subject: [PATCH 134/185] feat(pointlio): pcap_to_db writes a .rrd quick-look After recording, aggregate pointlio_lidar registered into the world frame (each body-frame cloud transformed by the nearest pointlio_odometry pose; the voxel rebuild assumes world-frame clouds and won't transform them) plus the pose path, and save them to .rrd via rerun. Voxel-deduped and height-colored (hot pink low -> dark purple high). --no-rrd to skip, --voxel to tune; best-effort so a viz failure never fails the recording. --- .../lidar/pointlio/scripts/pcap_to_db.py | 121 ++++++++++++++++-- 1 file changed, 113 insertions(+), 8 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py index f03476e7e7..57c9c914d1 100644 --- a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py @@ -27,13 +27,9 @@ DB="mem2.db" python -m dimos.hardware.sensors.lidar.pointlio.scripts.pcap_to_db --db "$DB" --pcap "$PCAP_PATH" - # generate map - dimos map summary "$DB" - dimos map pose-fill "$DB" \ - --target pointlio_lidar \ - --pose-source pointlio_odometry \ - --out "${DB%.db}_posed.db" - dimos map global "${DB%.db}_posed.db" --lidar pointlio_lidar + # A quick-look .rrd (aggregated world lidar + pose path) is written next + # to the db automatically. View it with: + rerun "${DB%.db}.rrd" One coordinator runs three autoconnected modules: a ``VirtualMid360`` replays the pcap over the Livox wire (aliasing the host/lidar IPs onto a dummy interface on @@ -50,7 +46,7 @@ import sqlite3 import sys import time -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, Any if TYPE_CHECKING: from dimos.core.coordination.blueprints import Blueprint @@ -63,6 +59,8 @@ # artifact, bad pcap, SLAM-init crash); bounds the poll loop. Generous to cover # Point-LIO's IMU-init latency. _STARTUP_TIMEOUT_SEC = 60.0 +# Max |Δts| to match a lidar frame to an odometry pose when aggregating the .rrd. +_POSE_MATCH_TOL = 0.1 def _odom_stats(db_path: Path, table: str) -> tuple[int, float, float]: @@ -80,6 +78,98 @@ def _odom_stats(db_path: Path, table: str) -> tuple[int, float, float]: con.close() +def _quat_to_rot(qx: float, qy: float, qz: float, qw: float) -> Any: + import numpy as np + + return np.array( + [ + [1 - 2 * (qy * qy + qz * qz), 2 * (qx * qy - qz * qw), 2 * (qx * qz + qy * qw)], + [2 * (qx * qy + qz * qw), 1 - 2 * (qx * qx + qz * qz), 2 * (qy * qz - qx * qw)], + [2 * (qx * qz - qy * qw), 2 * (qy * qz + qx * qw), 1 - 2 * (qx * qx + qy * qy)], + ] + ) + + +def _write_rrd(db_path: Path, odom_stream: str, lidar_stream: str, voxel: float) -> Path | None: + """Aggregate the recorded lidar (registered into world via the nearest odometry + pose) plus the pose path into a ``.rrd`` next to the db, for a quick look. + + Point-LIO publishes its cloud in the sensor/body frame, so each frame is + transformed to world by its pose here, then voxel-deduped. Best-effort: any + failure is non-fatal to the recording. Returns the .rrd path, or None.""" + import numpy as np + import rerun as rr + + from dimos.memory2.store.sqlite import SqliteStore + from dimos.msgs.nav_msgs.Odometry import Odometry + from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + from dimos.visualization.rerun.init import rerun_init + + store = SqliteStore(path=str(db_path), must_exist=True) + try: + odom = list(store.stream(odom_stream, Odometry).order_by("ts")) + if not odom: + return None + ots = np.array([o.ts for o in odom]) + opos = np.array( + [ + [ + o.data.pose.pose.position.x, + o.data.pose.pose.position.y, + o.data.pose.pose.position.z, + ] + for o in odom + ] + ) + oquat = np.array( + [ + [ + o.data.pose.pose.orientation.x, + o.data.pose.pose.orientation.y, + o.data.pose.pose.orientation.z, + o.data.pose.pose.orientation.w, + ] + for o in odom + ] + ) + chunks = [] + for lid in store.stream(lidar_stream, PointCloud2).order_by("ts"): + j = int(np.argmin(np.abs(ots - lid.ts))) + if abs(ots[j] - lid.ts) > _POSE_MATCH_TOL: + continue + pts = np.asarray(lid.data.as_numpy()[0])[:, :3].astype(np.float64) + if pts.shape[0] == 0: + continue + world = pts @ _quat_to_rot(*oquat[j]).T + opos[j] + # Per-frame voxel-dedup to bound memory before the global merge. + _, idx = np.unique(np.floor(world / voxel).astype(np.int64), axis=0, return_index=True) + chunks.append(world[idx]) + if not chunks: + return None + allpts = np.concatenate(chunks) + _, idx = np.unique(np.floor(allpts / voxel).astype(np.int64), axis=0, return_index=True) + agg = allpts[idx].astype(np.float32) + + # Height gradient: hot pink (low) -> dark purple (high). + z = agg[:, 2] + zn = (z - z.min()) / (np.ptp(z) + 1e-9) + low = np.array([255, 20, 147], dtype=np.float64) + high = np.array([60, 0, 80], dtype=np.float64) + colors = (low * (1 - zn)[:, None] + high * zn[:, None]).astype(np.uint8) + + rrd = db_path.with_suffix(".rrd") + rerun_init("pcap_to_db") + rr.save(str(rrd)) + rr.log("world/map", rr.Points3D(positions=agg, colors=colors, radii=[voxel / 8])) + rr.log( + "world/path", + rr.LineStrips3D(strips=[opos.astype(np.float32)], colors=[[231, 76, 60]], radii=[0.05]), + ) + return rrd + finally: + store.stop() + + def _build_blueprint(args: argparse.Namespace, db_path: Path, config_path: str) -> Blueprint: """autoconnect(VirtualMid360 + PointLio + PointlioRecorder). @@ -213,6 +303,13 @@ def _run(args: argparse.Namespace) -> int: f"[pcap_to_db] done odom={o_cnt} ts=[{o_min:.3f}, {o_max:.3f}] span={o_max - o_min:.1f}s", flush=True, ) + if not args.no_rrd: + try: + rrd = _write_rrd(db_path, args.odom_stream_name, args.lidar_stream_name, args.voxel) + if rrd is not None: + print(f"[pcap_to_db] wrote {rrd.name} (aggregated lidar + pose path)", flush=True) + except Exception as exc: # viz is a non-fatal bonus + print(f"[pcap_to_db] .rrd generation skipped ({exc})", file=sys.stderr, flush=True) return 0 @@ -244,6 +341,14 @@ def main(argv: list[str]) -> int: help="seconds added to every output ts (auto if omitted)", ) parser.add_argument("--force", action="store_true", help="overwrite existing pointlio streams") + parser.add_argument( + "--no-rrd", + action="store_true", + help="skip writing the .rrd quick-look (aggregated world lidar + pose path)", + ) + parser.add_argument( + "--voxel", type=float, default=0.2, help="voxel size (m) for the .rrd aggregated map" + ) parser.add_argument( "--warmup-sec", type=float, From 35a8ad9b7686e55b8b2beca682982dddab17e0f8 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 05:34:49 -0700 Subject: [PATCH 135/185] feat(map): map global registers clouds by per-frame pose; --go2 for old behavior MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The voxel rebuild assumed clouds were already world-frame and only applied the PGO correction, so body-frame clouds (e.g. Point-LIO's pointlio_lidar) piled onto the origin. Now each frame's pose registers its cloud into the world by default (graph composes the PGO correction on top: correction ∘ pose). --go2 restores the old assume-world-frame path for already-registered clouds (fastlio). NOTE: flips the default — world-frame recordings (fastlio/go2) now need --go2. --- dimos/mapping/utils/cli/map.py | 42 +++++++++++++++++++++++++++++++--- 1 file changed, 39 insertions(+), 3 deletions(-) diff --git a/dimos/mapping/utils/cli/map.py b/dimos/mapping/utils/cli/map.py index a9af81ea08..c9e23fbf7f 100644 --- a/dimos/mapping/utils/cli/map.py +++ b/dimos/mapping/utils/cli/map.py @@ -97,14 +97,33 @@ def _accumulate( block_count: int, device: str, graph: PoseGraph | None = None, + world_frame: bool = False, progress_cb: Callable[[Observation[Any]], None] | None = None, ) -> PointCloud2 | None: """Accumulate a voxel map from `obs_iter`, optionally PGO-correcting each frame. + By default each frame's per-frame pose is applied to register the (sensor/body + frame) cloud into the world. Set ``world_frame=True`` (the ``--go2`` path) when + the clouds are already world-registered (e.g. fastlio) — then only the PGO + correction is applied, if any. + Returns the final ``PointCloud2`` (or ``None`` if the input was empty). Disposal of the underlying ``VoxelGrid`` is handled by ``VoxelMapTransformer``. """ from dimos.mapping.voxels import VoxelMapTransformer + from dimos.msgs.geometry_msgs.Quaternion import Quaternion + from dimos.msgs.geometry_msgs.Transform import Transform + from dimos.msgs.geometry_msgs.Vector3 import Vector3 + + def _pose_tf(obs: Observation[Any]) -> Transform: + pose = obs.pose + assert pose is not None + return Transform( + translation=Vector3(pose.position.x, pose.position.y, pose.position.z), + rotation=Quaternion( + pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w + ), + ) def prepared() -> Iterable[Observation[PointCloud2]]: for obs in obs_iter: @@ -112,12 +131,20 @@ def prepared() -> Iterable[Observation[PointCloud2]]: progress_cb(obs) if len(obs.data) == 0: continue + # body->world via the per-frame pose, unless the clouds are already + # world-registered (--go2). graph adds the PGO correction on top + # (correction ∘ pose), applied after the pose. + tf: Transform | None = None + if not world_frame: + if obs.pose is None: + continue + tf = _pose_tf(obs) if graph is not None: if obs.pose_tuple is None: continue - yield obs.derive(data=obs.data.transform(graph.correction_at(obs.ts))) - else: - yield obs + correction = graph.correction_at(obs.ts) + tf = correction if tf is None else correction + tf + yield obs if tf is None else obs.derive(data=obs.data.transform(tf)) vmt = VoxelMapTransformer( emit_every=0, # batch mode: emit once on exhaustion @@ -299,6 +326,12 @@ def main( None, "--out", help="Output .rrd path (default: ./.rrd)" ), no_gui: bool = typer.Option(False, "--no-gui", help="Write the .rrd but don't launch rerun"), + go2: bool = typer.Option( + False, + "--go2", + help="Clouds are already world-registered (e.g. fastlio); skip applying the " + "per-frame pose. Default registers each (body-frame) cloud by its pose.", + ), markers: bool = typer.Option( False, "--markers", @@ -428,6 +461,7 @@ def main( block_count=block_count, device=device, graph=graph, + world_frame=go2, progress_cb=progress(n_kept, "pgo pass 2 (rebuilding)"), ) @@ -440,6 +474,7 @@ def main( block_count=block_count, device=device, graph=graph, + world_frame=go2, progress_cb=progress(total, "full pgo (rebuilding)"), ) @@ -449,6 +484,7 @@ def main( voxel=voxel, block_count=block_count, device=device, + world_frame=go2, progress_cb=progress(n_kept, "reconstructing global map"), ) From 55099fcbc21fe20525502b8f404e39e2e5fa03de Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 05:50:24 -0700 Subject: [PATCH 136/185] fastlio2: mirror pointlio's virtual_mid360 evolution (self-provision NIC, no netns) Port all pointlio virtual_mid360 changes onto fastlio. Pointlio dropped the netns/veth orchestrator: VirtualMid360 now self-provisions a dummy NIC (Linux) or lo0 alias (macOS) and the consumer + recorder autoconnect in one coordinator. - Move livox/virtual_mid360 -> virtual_mid360 (dir up one level, mirrors pointlio). - Copy pointlio's consumer-agnostic VM files verbatim: src/main.rs (namespace- agnostic, macOS, socket2 SO_REUSEADDR/REUSEPORT, #[native_config]), module.py (self-provisions NIC), Cargo, flake (path: input), recorder.py (Mid360PcapRecorder, tcpdump raw-livox recorder; replaces livox/pcap_recorder.py). - livox/net.py: shared resolve_host_ip() host_ip auto-detect; livox/module.py (Mid360 driver) and fastlio2/module.py now use it. - fastlio2/recorder.py: self-contained FastLio2Recorder (force, internal min-ts, configurable stream names; In odometry/lidar -> db fastlio_odometry/fastlio_lidar). - fastlio2/tools/pcap_to_db.py: autoconnect(VirtualMid360 + FastLio2 + FastLio2Recorder), setup_network flag, drain on lidar-stream stagnation. - all_blueprints: virtual_mid360 paths drop livox.; livox-pcap-recorder -> mid360-pcap-recorder (Mid360PcapRecorder). Tested on ruwik mid360_20260529_225346 (live-SDK via dummy NIC): acc_cov=0.1 diverges (~202 km), acc_cov=1.0 default bounded (15 m). --- .../hardware/sensors/lidar/fastlio2/module.py | 74 +-- .../sensors/lidar/fastlio2/recorder.py | 121 ++-- .../lidar/fastlio2/tools/pcap_to_db.py | 531 +++++++----------- dimos/hardware/sensors/lidar/livox/module.py | 26 +- dimos/hardware/sensors/lidar/livox/net.py | 85 +++ .../sensors/lidar/livox/pcap_recorder.py | 304 ---------- .../lidar/livox/virtual_mid360/module.py | 75 --- .../{livox => }/virtual_mid360/Cargo.lock | 1 + .../{livox => }/virtual_mid360/Cargo.toml | 3 +- .../{livox => }/virtual_mid360/blueprints.py | 16 +- .../{livox => }/virtual_mid360/flake.lock | 26 +- .../{livox => }/virtual_mid360/flake.nix | 16 +- .../sensors/lidar/virtual_mid360/module.py | 180 ++++++ .../sensors/lidar/virtual_mid360/recorder.py | 393 +++++++++++++ .../{livox => }/virtual_mid360/src/main.rs | 157 +++--- dimos/robot/all_blueprints.py | 6 +- 16 files changed, 1083 insertions(+), 931 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/livox/net.py delete mode 100644 dimos/hardware/sensors/lidar/livox/pcap_recorder.py delete mode 100644 dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py rename dimos/hardware/sensors/lidar/{livox => }/virtual_mid360/Cargo.lock (99%) rename dimos/hardware/sensors/lidar/{livox => }/virtual_mid360/Cargo.toml (80%) rename dimos/hardware/sensors/lidar/{livox => }/virtual_mid360/blueprints.py (56%) rename dimos/hardware/sensors/lidar/{livox => }/virtual_mid360/flake.lock (70%) rename dimos/hardware/sensors/lidar/{livox => }/virtual_mid360/flake.nix (67%) create mode 100644 dimos/hardware/sensors/lidar/virtual_mid360/module.py create mode 100644 dimos/hardware/sensors/lidar/virtual_mid360/recorder.py rename dimos/hardware/sensors/lidar/{livox => }/virtual_mid360/src/main.rs (75%) diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 104bad8d46..895fdde8b1 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -20,9 +20,7 @@ from __future__ import annotations -import ipaddress from pathlib import Path -import socket import time from typing import TYPE_CHECKING, Annotated @@ -32,6 +30,7 @@ from dimos.core.core import rpc from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import Out +from dimos.hardware.sensors.lidar.livox.net import resolve_host_ip from dimos.hardware.sensors.lidar.livox.ports import ( SDK_CMD_DATA_PORT, SDK_HOST_CMD_DATA_PORT, @@ -51,11 +50,8 @@ from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.navigation.nav_stack.frames import FRAME_BODY, FRAME_ODOM from dimos.spec import perception -from dimos.utils.generic import get_local_ips -from dimos.utils.logging_config import setup_logger _CONFIG_DIR = Path(__file__).parent / "config" -_logger = setup_logger() class FastLio2Config(NativeModuleConfig): @@ -153,70 +149,10 @@ def stop(self) -> None: super().stop() def _validate_network(self) -> None: - host_ip = self.config.host_ip - lidar_ip = self.config.lidar_ip - local_ips = [ip for ip, _iface in get_local_ips()] - - _logger.info( - "FastLio2 network check", - host_ip=host_ip, - lidar_ip=lidar_ip, - local_ips=local_ips, - ) - - if host_ip not in local_ips: - try: - lidar_net = ipaddress.IPv4Network(f"{lidar_ip}/24", strict=False) - same_subnet = [ip for ip in local_ips if ipaddress.IPv4Address(ip) in lidar_net] - except (ValueError, TypeError): - same_subnet = [] - - if same_subnet: - picked = same_subnet[0] - _logger.warning( - f"FastLio2: host_ip={host_ip!r} not found locally. " - f"Auto-correcting to {picked!r} (same subnet as lidar {lidar_ip}).", - configured_ip=host_ip, - corrected_ip=picked, - lidar_ip=lidar_ip, - local_ips=local_ips, - ) - self.config.host_ip = picked - host_ip = picked - else: - subnet_prefix = ".".join(lidar_ip.split(".")[:3]) - msg = ( - f"FastLio2: host_ip={host_ip!r} is not assigned to any local interface.\n" - f" Lidar IP: {lidar_ip}\n" - f" Local IPs found: {', '.join(local_ips) or '(none)'}\n" - f" No local IP found on the same subnet as lidar ({lidar_ip}).\n" - f" The lidar network interface may be down or unconfigured.\n" - f" → Check: ip addr | grep {subnet_prefix}\n" - f" → Or assign an IP: " - f"sudo ip addr add {subnet_prefix}.5/24 dev \n" - ) - _logger.error(msg) - raise RuntimeError(msg) - - # Check if we can bind a UDP socket on host_ip (port 0 = ephemeral). - try: - with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: - sock.bind((host_ip, 0)) - except OSError as e: - _logger.error( - f"FastLio2: Cannot bind UDP socket on host_ip={host_ip!r}: {e}\n" - f" Another process may be using the Livox SDK ports.\n" - f" → Check: ss -ulnp | grep {host_ip}" - ) - raise RuntimeError( - f"FastLio2: Cannot bind UDP on {host_ip}: {e}. " - f"Check if another Livox/FastLio2 process is running." - ) from e - - _logger.info( - "FastLio2 network check passed", - host_ip=host_ip, - lidar_ip=lidar_ip, + # Auto-derive host_ip from a local NIC on the lidar's subnet (shared with + # the Mid360 driver / Point-LIO) when the configured value isn't local. + self.config.host_ip = resolve_host_ip( + self.config.lidar_ip, self.config.host_ip, label="FastLio2" ) diff --git a/dimos/hardware/sensors/lidar/fastlio2/recorder.py b/dimos/hardware/sensors/lidar/fastlio2/recorder.py index 9d9140d71c..b530a7b744 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/recorder.py +++ b/dimos/hardware/sensors/lidar/fastlio2/recorder.py @@ -12,14 +12,22 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Record FAST-LIO's odometry + lidar into a memory2 SQLite db, rewriting only -those streams' timestamps onto the db clock so offline pcap replay lines up with -a live recording. Used by tools/pcap_to_db.py.""" +"""Record FAST-LIO odometry + lidar into a memory2 SQLite db. + +Subscribes to a FastLio2's ``odometry`` / ``lidar`` outputs (auto-connected by +matching stream name + type — no remappings needed) and appends them to a +memory2 store. Timestamps are converted onto the db's existing clock so a run +can be appended to an existing db and compared on one timeline. Owns the db +lifecycle: refuses to clobber existing streams unless ``force``, and derives the +alignment reference from whatever the db already holds. +""" from __future__ import annotations from collections.abc import AsyncIterator import math +from pathlib import Path +import sqlite3 import time from dimos.core.module import Module, ModuleConfig @@ -27,29 +35,60 @@ from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -# Below this an absolute timestamp is sensor-boot seconds, not unix wall time. +# Below this the db's existing timestamps are sensor-boot seconds, not unix time. _SENSOR_CLOCK_MAX = 1e8 # Strictly-increasing tie-breaker so two samples never collide on ts. _EPS = 1e-9 +def _existing_min_ts(db_path: Path) -> float: + """Min ts across the db's existing streams, or -1.0 if none/absent.""" + if not db_path.exists(): + return -1.0 + con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) + try: + tables = [ + row[0] + for row in con.execute("SELECT name FROM sqlite_master WHERE type='table'").fetchall() + ] + best: float | None = None + for table in tables: + if table.startswith("_") or table.startswith("sqlite_"): + continue + try: + cols = [col[1] for col in con.execute(f"PRAGMA table_info('{table}')").fetchall()] + if "ts" not in cols: + continue + row = con.execute(f"SELECT MIN(ts) FROM '{table}'").fetchone() + except sqlite3.OperationalError: + continue + if row and row[0] is not None: + best = row[0] if best is None else min(best, row[0]) + return best if best is not None else -1.0 + finally: + con.close() + + class FastLio2RecorderConfig(ModuleConfig): - """Target db and the timestamp-conversion policy for fastlio streams.""" + """Configures the recorder with the target db and timing conversion.""" db_path: str = "" - # Earliest existing ts in the db, or -1.0 if the db has no timestamped rows. - ref_start_ts: float = -1.0 - # Explicit offset override; NaN means auto-derive from ref_start_ts. + # db stream/table names the FastLio2 outputs are recorded under. + odom_stream_name: str = "fastlio_odometry" + lidar_stream_name: str = "fastlio_lidar" + # Explicit offset override; NaN means auto-derive from the db's earliest ts. time_offset: float = float("nan") + # Drop pre-existing odom/lidar streams instead of refusing to overwrite. + force: bool = False class FastLio2Recorder(Module): - """Offset auto-derives: same clock family -> 0; cross-clock -> start-align.""" - config: FastLio2RecorderConfig - fastlio_odometry: In[Odometry] - fastlio_lidar: In[PointCloud2] + + lidar: In[PointCloud2] + odometry: In[Odometry] _offset: float | None = None + _ref_start_ts: float = -1.0 _last_odom_ts: float = 0.0 _last_lidar_ts: float = 0.0 _last_pose: object = None @@ -57,11 +96,26 @@ class FastLio2Recorder(Module): _lidar_count: int = 0 async def main(self) -> AsyncIterator[None]: + # Deferred: the store is opened in the worker process that runs main(), + # not at module-scan/import time on the host. from dimos.memory2.store.sqlite import SqliteStore - self._store = SqliteStore(path=self.config.db_path) - self._os = self._store.stream("fastlio_odometry", Odometry) - self._ls = self._store.stream("fastlio_lidar", PointCloud2) + cfg = self.config + self._store = SqliteStore(path=cfg.db_path) + names = (cfg.odom_stream_name, cfg.lidar_stream_name) + existing = sorted(set(self._store.list_streams()) & set(names)) + if existing and not cfg.force: + raise RuntimeError( + f"FastLio2Recorder: {Path(cfg.db_path).name} already has {existing}; " + "set force=True to overwrite" + ) + for name in existing: + self._store.delete_stream(name) + # Reference is the db's earliest ts *after* dropping our own old streams, + # so only the data we're aligning against counts. + self._ref_start_ts = _existing_min_ts(Path(cfg.db_path)) + self._os = self._store.stream(cfg.odom_stream_name, Odometry) + self._ls = self._store.stream(cfg.lidar_stream_name, PointCloud2) yield self._store.stop() @@ -69,36 +123,35 @@ def _resolve_offset(self, first_ts: float) -> float: override = self.config.time_offset if not math.isnan(override): return override - ref = self.config.ref_start_ts - if ref < 0.0: - return 0.0 - # Same clock family -> aligned; cross-clock -> start-align onto db's first. - if (first_ts > _SENSOR_CLOCK_MAX) == (ref > _SENSOR_CLOCK_MAX): + ref = self._ref_start_ts + if ref < 0.0 or ref < _SENSOR_CLOCK_MAX: + # Empty db, or db already on the sensor clock -> exact alignment. return 0.0 + # db on wall-clock time -> start-align FastLio2 onto the db's earliest ts. return ref - first_ts def _aligned_ts(self, raw_ts: float, last_ts: float) -> float: - """Convert a replay ts onto the db clock, kept strictly above last_ts.""" + """Convert a sensor ts onto the db clock, kept strictly above last_ts.""" if self._offset is None: self._offset = self._resolve_offset(raw_ts) return max(raw_ts + self._offset, last_ts + _EPS) - @staticmethod - def _raw_ts(v: object) -> float: - # Guard on None: a genuine 0.0 ts is falsy, so `ts or time.time()` would drop it. - ts = getattr(v, "ts", None) - return ts if ts is not None else time.time() - - async def handle_fastlio_odometry(self, v: Odometry) -> None: - ts = self._aligned_ts(self._raw_ts(v), self._last_odom_ts) + async def handle_odometry(self, msg: Odometry) -> None: + # `is not None`, not `or`: a real sensor ts of 0.0 must not fall back to + # wall time (would misclassify the stream's clock in _resolve_offset). + raw_ts_raw = getattr(msg, "ts", None) + raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() + ts = self._aligned_ts(raw_ts, self._last_odom_ts) self._last_odom_ts = ts - pose = getattr(v, "pose", None) + pose = getattr(msg, "pose", None) self._last_pose = getattr(pose, "pose", None) if pose is not None else None - self._os.append(v, ts=ts, pose=self._last_pose) + self._os.append(msg, ts=ts, pose=self._last_pose) self._odom_count += 1 - async def handle_fastlio_lidar(self, v: PointCloud2) -> None: - ts = self._aligned_ts(self._raw_ts(v), self._last_lidar_ts) + async def handle_lidar(self, msg: PointCloud2) -> None: + raw_ts_raw = getattr(msg, "ts", None) + raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() + ts = self._aligned_ts(raw_ts, self._last_lidar_ts) self._last_lidar_ts = ts - self._ls.append(v, ts=ts, pose=self._last_pose) + self._ls.append(msg, ts=ts, pose=self._last_pose) self._lidar_count += 1 diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index 9530e1ab57..8a3cc6b5f2 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -12,89 +12,61 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Replay a Livox Mid-360 pcap through FAST-LIO and record its output to a .db. - -``virtual_mid360`` replays the pcap over the wire (a fake Mid-360 on a virtual -NIC) and FastLio2 connects in live SDK mode, exactly as to real hardware: - -* ``pcap_to_db --pcap capture.pcap`` -> writes ``capture.db`` -* ``pcap_to_db --pcap capture.pcap --db mem2.db`` -> appends into an existing db - -Records ``fastlio_odometry`` + ``fastlio_lidar``, time-aligned onto the db clock -(``--time-offset`` overrides; ``--force`` overwrites pre-existing fastlio streams). -Stands up two netns joined by a veth, so it needs root — set ``$SUDO`` to a -passwordless escalation (default ``sudo``); netns/veth names override via -``$DRV_NS``/``$LIDAR_NS``/``$VETH_*``. - -Build the virtual_mid360 binary once:: - - (cd dimos/hardware/sensors/lidar/livox/virtual_mid360 && nix build .#default) +""" +Usage: + # fetch a sample Mid-360 capture (the get_data arg is the dir/file inside the + # LFS archive, NOT the archive name) + PCAP_PATH="$(python -c "from dimos.utils.data import get_data; print(get_data('mid360_shake_stairs/mid360_shake_stairs.pcap'))")" + + # gen .db from pcap with your config (defaults to .db next to the pcap) + python -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db \ + --config dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml \ + --pcap "$PCAP_PATH" + + # add to existing .db + DB="mem2.db" + python -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db --db "$DB" --pcap "$PCAP_PATH" + + # generate map + dimos map summary "$DB" + dimos map pose-fill "$DB" \ + --target fastlio_lidar \ + --pose-source fastlio_odometry \ + --out "${DB%.db}_posed.db" + dimos map global "${DB%.db}_posed.db" --lidar fastlio_lidar + +One coordinator runs three autoconnected modules: a ``VirtualMid360`` replays the +pcap over the Livox wire (aliasing the host/lidar IPs onto a dummy interface on +Linux, or lo0 on macOS — needs CAP_NET_ADMIN/sudo), an unmodified live ``FastLio2`` +consumes it as real hardware, and a ``FastLio2Recorder`` appends FastLio2's +odometry/lidar into the db. This script just wires them and stops once the pcap +has drained. Replay is real time (FAST-LIO is not deterministic), so runs differ. """ from __future__ import annotations import argparse -import json -import os from pathlib import Path import sqlite3 -import subprocess import sys -import tempfile import time +from typing import TYPE_CHECKING -from dimos.hardware.sensors.lidar.fastlio2.recorder import FastLio2Recorder +if TYPE_CHECKING: + from dimos.core.coordination.blueprints import Blueprint -# Poll cadence while recording. +# Poll the db on this cadence while the replay drains the pcap. _POLL_SEC = 1.0 -# Let FastLio2 drain its scan backlog after the pcap finishes before stopping. -_DRAIN_SEC = 10.0 - -# lidar ns runs virtual_mid360; drv ns runs the FastLio2 consumer. Defaults are -# distinct from pointlio's harness so both can run concurrently. -_SUDO = os.environ.get("SUDO", "sudo") -_DRV_NS = os.environ.get("DRV_NS", "fl_drv") -_LIDAR_NS = os.environ.get("LIDAR_NS", "fl_lidar") -_VETH_DRV = os.environ.get("VETH_DRV", "veth-fl-drv") -_VETH_LIDAR = os.environ.get("VETH_LIDAR", "veth-fl-lidar") -_HOST_IP = "192.168.1.5" -_LIDAR_IP = "192.168.1.155" -_REPO = Path(__file__).resolve().parents[6] -_VM_BIN = _REPO / "dimos/hardware/sensors/lidar/livox/virtual_mid360/result/bin/virtual_mid360" +# Stop after the odom stream has been stagnant this long (pcap fully drained). +_STAGNANT_SEC = 5.0 +# No odometry within this long after start = FAST-LIO failed to come up (missing +# artifact, bad pcap, SLAM-init crash); bounds the poll loop. Generous to cover +# FAST-LIO's IMU-init latency. +_STARTUP_TIMEOUT_SEC = 60.0 -def _db_ref_start_ts(db_path: Path) -> float: - """Min ts across the db's existing streams, or -1.0 if none/absent.""" - if not db_path.exists(): - return -1.0 - con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) - try: - tables = [ - row[0] - for row in con.execute("SELECT name FROM sqlite_master WHERE type='table'").fetchall() - ] - best: float | None = None - for table in tables: - if table.startswith("_") or table.startswith("sqlite_"): - continue - try: - # vec0/rtree virtual tables raise "no such module" when the - # extension isn't loaded -- skip them. - cols = [c[1] for c in con.execute(f"PRAGMA table_info('{table}')").fetchall()] - if "ts" not in cols: - continue - row = con.execute(f"SELECT MIN(ts) FROM '{table}'").fetchone() - except sqlite3.OperationalError: - continue - if row and row[0] is not None: - best = row[0] if best is None else min(best, row[0]) - return best if best is not None else -1.0 - finally: - con.close() - - -def _table_stats(db_path: Path, table: str) -> tuple[int, float, float]: - """(count, min_ts, max_ts) for a stream table; zeros if absent.""" +def _odom_stats(db_path: Path, table: str) -> tuple[int, float, float]: + """(count, min_ts, max_ts) for the odom table; zeros if absent.""" if not db_path.exists(): return 0, 0.0, 0.0 con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) @@ -103,262 +75,141 @@ def _table_stats(db_path: Path, table: str) -> tuple[int, float, float]: row = con.execute(f"SELECT COUNT(*), MIN(ts), MAX(ts) FROM '{table}'").fetchone() except sqlite3.OperationalError: return 0, 0.0, 0.0 - cnt = row[0] or 0 - return cnt, row[1] or 0.0, row[2] or 0.0 + return row[0] or 0, row[1] or 0.0, row[2] or 0.0 finally: con.close() -# Orchestrator: set up the netns + fake sensor, drive the consumer, tear down. - - -def _sudo(*args: str, check: bool = True) -> subprocess.CompletedProcess[bytes]: - return subprocess.run([_SUDO, *args], check=check) - - -def _teardown_netns() -> None: - _sudo("pkill", "-9", "-f", "result/bin/virtual_mid360", check=False) - _sudo("ip", "netns", "del", _DRV_NS, check=False) - _sudo("ip", "netns", "del", _LIDAR_NS, check=False) - _sudo("ip", "link", "del", _VETH_DRV, check=False) +def _build_blueprint(args: argparse.Namespace, db_path: Path, config_path: str) -> Blueprint: + """autoconnect(VirtualMid360 + FastLio2 + FastLio2Recorder). + FastLio2's ``odometry``/``lidar`` outputs auto-wire to the recorder's + same-named inputs. VirtualMid360 carries no dimos streams — it speaks the + Livox wire protocol, reached by host_ip/lidar_ip, and sets up the NIC itself. + """ + from dimos.core.coordination.blueprints import autoconnect + from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 + from dimos.hardware.sensors.lidar.fastlio2.recorder import FastLio2Recorder + from dimos.hardware.sensors.lidar.virtual_mid360.module import VirtualMid360 -def _setup_netns() -> None: - _teardown_netns() - _sudo("ip", "netns", "add", _DRV_NS) - _sudo("ip", "netns", "add", _LIDAR_NS) - _sudo("ip", "link", "add", _VETH_DRV, "type", "veth", "peer", "name", _VETH_LIDAR) - _sudo("ip", "link", "set", _VETH_DRV, "netns", _DRV_NS) - _sudo("ip", "link", "set", _VETH_LIDAR, "netns", _LIDAR_NS) - _sudo("ip", "netns", "exec", _DRV_NS, "ip", "addr", "add", f"{_HOST_IP}/24", "dev", _VETH_DRV) - _sudo( - "ip", "netns", "exec", _LIDAR_NS, "ip", "addr", "add", f"{_LIDAR_IP}/24", "dev", _VETH_LIDAR - ) - for ns in (_DRV_NS, _LIDAR_NS): - _sudo("ip", "netns", "exec", ns, "ip", "link", "set", "lo", "up") - _sudo("ip", "netns", "exec", ns, "ip", "link", "set", "lo", "multicast", "on") - _sudo("ip", "netns", "exec", ns, "ip", "route", "add", "224.0.0.0/4", "dev", "lo") - _sudo("ip", "netns", "exec", _DRV_NS, "ip", "link", "set", _VETH_DRV, "up") - _sudo("ip", "netns", "exec", _LIDAR_NS, "ip", "link", "set", _VETH_LIDAR, "up") - _sudo("ip", "netns", "exec", _DRV_NS, "ip", "link", "set", _VETH_DRV, "multicast", "on") - _sudo("ip", "netns", "exec", _LIDAR_NS, "ip", "link", "set", _VETH_LIDAR, "multicast", "on") - _sudo( - "ip", - "netns", - "exec", - _LIDAR_NS, - "ip", - "route", - "add", - "255.255.255.255/32", - "dev", - _VETH_LIDAR, - ) - # Mid-360 multicasts point/IMU to 224.1.1.5 — egress the virtual NIC. - _sudo( - "ip", "netns", "exec", _LIDAR_NS, "ip", "route", "add", "224.1.1.5/32", "dev", _VETH_LIDAR + # `config` is already an absolute path so it bypasses the config-dir-relative + # resolution. Omit when empty to keep the default mid360.yaml. + fastlio_kwargs: dict[str, object] = dict( + host_ip=args.host_ip, lidar_ip=args.lidar_ip, odom_freq=args.odom_freq, debug=False ) + if config_path: + fastlio_kwargs["config"] = config_path + + return autoconnect( + VirtualMid360.blueprint( + pcap=str(args.pcap_path), + rate=args.rate, + delay=args.warmup_sec, # hold streaming until FastLio2's SDK is up + host_ip=args.host_ip, + lidar_ip=args.lidar_ip, + alias_iface=args.alias_iface, + # When the NIC is provisioned by hand, skip the module's own sudo + # (it runs in a tty-less worker where a password prompt can't appear). + setup_network=not args.no_network_setup, + ), + FastLio2.blueprint(**fastlio_kwargs), + FastLio2Recorder.blueprint( + db_path=str(db_path), + odom_stream_name=args.odom_stream_name, + lidar_stream_name=args.lidar_stream_name, + time_offset=float("nan") if args.time_offset is None else args.time_offset, + force=args.force, + ), + ).global_config(n_workers=4, robot_model="mid360_fastlio_pcap_to_db") + + +def _poll_until_drained( + db_path: Path, odom_stream: str, lidar_stream: str, max_sensor_sec: float +) -> bool: + """Block until the pcap drains or a cap is hit; False if FAST-LIO never + produced odometry within the startup timeout. + + Drain is detected on the *lidar* stream's latest timestamp going flat: lidar + is input-driven, so it stops advancing the moment the pcap is exhausted. The + odometry stream can't be used for this — FAST-LIO keeps publishing odometry + (dead-reckoning) at odom_freq after input stops, with ever-advancing + timestamps, so its stream never looks stagnant and the run would hang.""" + last_lidar_max = 0.0 + first_max: float | None = None + stagnant_since: float | None = None + start_time = time.time() + while True: + time.sleep(_POLL_SEC) + odom_cnt, odom_min, odom_max = _odom_stats(db_path, odom_stream) + if odom_cnt == 0: + # Stagnation timeout only arms once odometry exists, so bound the + # no-output wait separately or a dead binary would hang forever. + if time.time() - start_time > _STARTUP_TIMEOUT_SEC: + print( + f"[pcap_to_db] no odometry after {_STARTUP_TIMEOUT_SEC:.0f}s — FAST-LIO " + "failed to start (check the binary, pcap path, and interface setup).", + file=sys.stderr, + flush=True, + ) + return False + continue + if first_max is None: + first_max = odom_min + if max_sensor_sec > 0 and (odom_max - first_max) >= max_sensor_sec: + print(f"[pcap_to_db] reached --max-sensor-sec={max_sensor_sec:.1f}s", flush=True) + return True + _, _, lidar_max = _odom_stats(db_path, lidar_stream) + if lidar_max == last_lidar_max: + if stagnant_since is None: + stagnant_since = time.time() + elif time.time() - stagnant_since > _STAGNANT_SEC: + return True + else: + last_lidar_max = lidar_max + stagnant_since = None -def _orchestrate(args: argparse.Namespace) -> int: - pcap = Path(args.pcap).expanduser().resolve() - if not pcap.exists(): - print(f"[pcap_to_db] missing pcap: {pcap}", file=sys.stderr) +def _run(args: argparse.Namespace) -> int: + from dimos.core.coordination.module_coordinator import ModuleCoordinator + + pcap_path = Path(args.pcap).expanduser().resolve() + if not pcap_path.exists(): + print(f"[pcap_to_db] missing pcap: {pcap_path}", file=sys.stderr) return 2 - if not _VM_BIN.exists(): - print( - f"[pcap_to_db] missing virtual_mid360 binary at {_VM_BIN}\n" - f" build it: (cd {_VM_BIN.parents[1]} && nix build .#default)", - file=sys.stderr, - ) + args.pcap_path = pcap_path + db_path = Path(args.db).expanduser().resolve() if args.db else pcap_path.with_suffix(".db") + db_path.parent.mkdir(parents=True, exist_ok=True) + # Resolve --config against the *invoking* cwd (pwd-relative) up front; the + # FastLio2 config field otherwise resolves a relative path against its own + # config/ dir, never the pwd. Absolute path passes through unchanged. + config_path = str(Path(args.config).expanduser().resolve()) if args.config else "" + if config_path and not Path(config_path).exists(): + print(f"[pcap_to_db] missing --config: {config_path}", file=sys.stderr) return 2 - db = Path(args.db).expanduser().resolve() if args.db else pcap.with_suffix(".db") - db.parent.mkdir(parents=True, exist_ok=True) + print( - f"[pcap_to_db] {pcap.name} -> {db.name} " - f"({'append' if db.exists() else 'new'}) via virtual_mid360 (live SDK)", + f"[pcap_to_db] pcap={pcap_path.name} db={db_path.name} " + f"({'append' if db_path.exists() else 'new'}) rate={args.rate} " + f"ips={args.host_ip}/{args.lidar_ip}", flush=True, ) - consumer: subprocess.Popen[bytes] | None = None - tmp = Path(tempfile.gettempdir()) - stopfile = tmp / f"pcap_to_db_stop.{os.getpid()}" - vmlog = tmp / f"pcap_to_db_vm.{os.getpid()}.log" - stopfile.unlink(missing_ok=True) - _setup_netns() + coord = None try: - # FastLio2 consumer in the drv netns (re-exec self as the recorder). - cmd = [ - _SUDO, - "ip", - "netns", - "exec", - _DRV_NS, - "env", - f"PYTHONPATH={_REPO}", - sys.executable, - "-m", - "dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db", - "--_consume", - "--_stopfile", - str(stopfile), - "--db", - str(db), - "--duration", - str(args.duration), - "--odom-freq", - str(args.odom_freq), - ] - if args.config: - cmd += ["--config", args.config] - if args.force: - cmd += ["--force"] - if args.time_offset is not None: - cmd += ["--time-offset", str(args.time_offset)] - # SQLite won't let root (the in-netns recorder) write a user-owned db, so - # take ownership for the run; the chown back at the end restores it. - for suffix in ("", "-wal", "-shm"): - q = Path(str(db) + suffix) - if q.exists(): - _sudo("chown", "0:0", str(q), check=False) - consumer = subprocess.Popen(cmd) - time.sleep(5) # let the coordinator boot + open the SDK sockets - - # Fake lidar in the lidar netns, replaying the pcap over the wire. - vm_cfg = json.dumps( - { - "topics": {}, - "config": { - "pcap": str(pcap), - "rate": 1.0, - "delay": 2.0, - "lidar_ip": _LIDAR_IP, - "host_ip": _HOST_IP, - "lidar_netns": _LIDAR_NS, - }, - } + coord = ModuleCoordinator.build(_build_blueprint(args, db_path, config_path)) + drained = _poll_until_drained( + db_path, args.odom_stream_name, args.lidar_stream_name, args.max_sensor_sec ) - with open(vmlog, "wb") as vmerr: - vm = subprocess.Popen( - [_SUDO, "ip", "netns", "exec", _LIDAR_NS, str(_VM_BIN)], - stdin=subprocess.PIPE, - stderr=vmerr, - ) - assert vm.stdin is not None - vm.stdin.write((vm_cfg + "\n").encode()) - vm.stdin.close() - - # virtual_mid360 logs "data stream finished" after one pcap pass; wait - # for that (capped by --duration), then drain + stop. (Watching db - # stagnation is unreliable — a diverging FastLio2 emits after quiet.) - deadline = time.time() + args.duration - while time.time() < deadline: - if vm.poll() is not None: - break - try: - if b"data stream finished" in vmlog.read_bytes(): - break - except OSError: - pass - time.sleep(1.0) - time.sleep(_DRAIN_SEC) - stopfile.touch() - try: - consumer.wait(timeout=60) - except subprocess.TimeoutExpired: - consumer.terminate() - rc = consumer.returncode or 0 finally: - if consumer is not None and consumer.poll() is None: - consumer.terminate() - try: - consumer.wait(timeout=10) - except subprocess.TimeoutExpired: - consumer.kill() - _teardown_netns() - stopfile.unlink(missing_ok=True) - vmlog.unlink(missing_ok=True) - - # The consumer ran as root inside the netns, so the db is root-owned — - # hand it back to the invoking user. - for suffix in ("", "-wal", "-shm"): - p = Path(str(db) + suffix) - if p.exists(): - _sudo("chown", f"{os.getuid()}:{os.getgid()}", str(p), check=False) - return rc - - -# Consumer: FastLio2 live SDK + recorder. Runs inside the drv netns. + if coord is not None: + coord.stop() - -def _consume(args: argparse.Namespace) -> int: - db_path = Path(args.db).expanduser().resolve() - db_path.parent.mkdir(parents=True, exist_ok=True) - - from dimos.core.coordination.blueprints import autoconnect - from dimos.core.coordination.module_coordinator import ModuleCoordinator - from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 - from dimos.memory2.store.sqlite import SqliteStore - - fastlio_streams = ("fastlio_odometry", "fastlio_lidar") - store = SqliteStore(path=str(db_path)) - try: - existing = sorted(set(store.list_streams()) & set(fastlio_streams)) - if existing and not args.force: - print( - f"[pcap_to_db] {db_path.name} already has {existing}; pass --force to overwrite", - file=sys.stderr, - ) - return 2 - for name in existing: - store.delete_stream(name) - if existing: - print(f"[pcap_to_db] --force: dropped existing {existing}", flush=True) - finally: - store.stop() - - ref_start_ts = _db_ref_start_ts(db_path) - time_offset = float("nan") if args.time_offset is None else args.time_offset - - fastlio_kwargs: dict[str, object] = dict( - frame_id="world", odom_freq=args.odom_freq, debug=False - ) - if args.config: - fastlio_kwargs["config"] = Path(args.config) - fastlio = FastLio2.blueprint(**fastlio_kwargs).remappings( - [ - (FastLio2, "odometry", "fastlio_odometry"), - (FastLio2, "lidar", "fastlio_lidar"), - ] - ) - blueprint = autoconnect( - fastlio, - FastLio2Recorder.blueprint( - db_path=str(db_path), ref_start_ts=ref_start_ts, time_offset=time_offset - ), - ).global_config(n_workers=4, robot_model="mid360_fastlio2_pcap_to_db") - coord = ModuleCoordinator.build(blueprint) - - # The orchestrator signals stop via the stop-file; --duration is a safety cap. - stopfile = Path(args._stopfile) if args._stopfile else None - t0 = time.time() - try: - while time.time() - t0 < args.duration: - time.sleep(_POLL_SEC) - if stopfile is not None and stopfile.exists(): - print("[pcap_to_db] stop signalled", flush=True) - break - else: - print(f"[pcap_to_db] reached --duration cap {args.duration:.0f}s", flush=True) - finally: - coord.stop() - - o_cnt, o_min, o_max = _table_stats(db_path, "fastlio_odometry") - l_cnt = _table_stats(db_path, "fastlio_lidar")[0] + o_cnt, o_min, o_max = _odom_stats(db_path, args.odom_stream_name) + if o_cnt == 0 or not drained: + print("[pcap_to_db] no odometry recorded — check the run above", file=sys.stderr) + return 1 print( - f"[pcap_to_db] done odom={o_cnt} lidar={l_cnt} " - f"ts=[{o_min:.3f}, {o_max:.3f}] span={o_max - o_min:.1f}s " - f"wall={time.time() - t0:.1f}s", + f"[pcap_to_db] done odom={o_cnt} ts=[{o_min:.3f}, {o_max:.3f}] span={o_max - o_min:.1f}s", flush=True, ) return 0 @@ -366,56 +217,70 @@ def _consume(args: argparse.Namespace) -> int: def main(argv: list[str]) -> int: parser = argparse.ArgumentParser(description=__doc__) - parser.add_argument("--pcap", default=None, help="Livox Mid-360 pcap capture to replay") + parser.add_argument("--pcap", help="Livox Mid-360 pcap capture") parser.add_argument( "--db", default=None, - help="target memory2 SQLite db; defaults to .db. Existing fastlio " - "streams are time-aligned onto its clock (use --force to overwrite them).", + help="target memory2 SQLite db. Existing -> append/align; missing -> built from " + "scratch. Omit to default to .db next to the pcap.", ) parser.add_argument( - "--config", - type=str, - default=None, - help="FAST-LIO yaml (relative to config/ or absolute); omit for the module default", + "--rate", type=float, default=1.0, help="replay-speed multiplier (default 1.0)" ) parser.add_argument( - "--odom-freq", + "--odom-freq", type=float, default=30.0, help="FAST-LIO odometry rate Hz (default 30)" + ) + parser.add_argument( + "--max-sensor-sec", type=float, - default=30.0, - help="FAST-LIO odometry publish rate in Hz (default 30)", + default=0.0, + help="stop after N sensor seconds (0 = whole pcap)", ) parser.add_argument( "--time-offset", type=float, default=None, - help="seconds added to every output ts; omit to auto-derive from the db clock", + help="seconds added to every output ts (auto if omitted)", ) + parser.add_argument("--force", action="store_true", help="overwrite existing fastlio streams") parser.add_argument( - "--duration", + "--warmup-sec", type=float, - default=3600.0, - help="safety cap in seconds; replay normally stops when the pcap is drained", + default=4.0, + help="seconds the fake lidar waits before streaming (lets FAST-LIO come up first)", + ) + parser.add_argument( + "--config", + default="", + help="FAST-LIO YAML config (pwd-relative or absolute; default: module's mid360.yaml)", ) parser.add_argument( - "--force", + "--odom-stream-name", + default="fastlio_odometry", + help="db stream/table name for the recorded odometry", + ) + parser.add_argument( + "--lidar-stream-name", + default="fastlio_lidar", + help="db stream/table name for the recorded point cloud", + ) + # Addressing knobs (override to run two replays at once). + parser.add_argument("--host-ip", default="192.168.1.5") + parser.add_argument("--lidar-ip", default="192.168.1.155") + parser.add_argument( + "--alias-iface", default="dimos-mid360", help="dummy iface the host/lidar IPs live on" + ) + parser.add_argument( + "--no-network-setup", action="store_true", - help="overwrite existing fastlio_odometry/fastlio_lidar streams in the db", + help="don't let the module alias the NIC via sudo — you've set up host/lidar IPs " + "+ multicast routes yourself (e.g. on macOS where worker-side sudo can't prompt)", ) - # Internal: the in-netns recorder half, spawned by the orchestrator. - parser.add_argument("--_consume", action="store_true", help=argparse.SUPPRESS) - parser.add_argument("--_stopfile", default=None, help=argparse.SUPPRESS) - args = parser.parse_args(argv) - if args._consume: - if not args.db: - print("[pcap_to_db] --_consume requires --db", file=sys.stderr) - return 2 - return _consume(args) + args = parser.parse_args(argv) if not args.pcap: - print("[pcap_to_db] --pcap is required", file=sys.stderr) - return 2 - return _orchestrate(args) + parser.error("--pcap is required") + return _run(args) if __name__ == "__main__": diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index e7507913c3..021025677c 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -20,18 +20,22 @@ from dimos.core.coordination.module_coordinator import ModuleCoordinator ModuleCoordinator.build(autoconnect( - Mid360.blueprint(host_ip="192.168.1.5"), + Mid360.blueprint(), # host_ip auto-detected; set lidar_ip if not the factory default SomeConsumer.blueprint(), )).loop() """ from __future__ import annotations +import os from typing import TYPE_CHECKING +from pydantic import Field + from dimos.core.core import rpc from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import Out +from dimos.hardware.sensors.lidar.livox.net import resolve_host_ip from dimos.hardware.sensors.lidar.livox.ports import ( SDK_CMD_DATA_PORT, SDK_HOST_CMD_DATA_PORT, @@ -50,13 +54,13 @@ class Mid360Config(NativeModuleConfig): - """Config for the C++ Mid-360 native module.""" - cwd: str | None = "cpp" executable: str = "result/bin/mid360_native" build_command: str | None = "nix build .#mid360_native" - host_ip: str = "192.168.1.5" - lidar_ip: str = "192.168.1.155" + host_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_HOST_IP")) + lidar_ip: str = Field( + default_factory=lambda: os.environ.get("DIMOS_MID360_LIDAR_IP", "192.168.1.155") + ) frequency: float = 10.0 enable_imu: bool = True frame_id: str = "lidar_link" @@ -76,13 +80,6 @@ class Mid360Config(NativeModuleConfig): class Mid360(NativeModule, perception.Lidar, perception.IMU): - """Livox Mid-360 LiDAR module backed by a native C++ binary. - - Ports: - lidar (Out[PointCloud2]): Point cloud frames at configured frequency. - imu (Out[Imu]): IMU data at ~200 Hz (if enabled). - """ - config: Mid360Config lidar: Out[PointCloud2] @@ -90,6 +87,11 @@ class Mid360(NativeModule, perception.Lidar, perception.IMU): @rpc def start(self) -> None: + # Auto-derive host_ip from a local NIC on the lidar's subnet (shared with + # Point-LIO) when the configured value isn't one of our IPs. + self.config.host_ip = resolve_host_ip( + self.config.lidar_ip, self.config.host_ip, label="Mid360" + ) super().start() @rpc diff --git a/dimos/hardware/sensors/lidar/livox/net.py b/dimos/hardware/sensors/lidar/livox/net.py new file mode 100644 index 0000000000..d7f98d8814 --- /dev/null +++ b/dimos/hardware/sensors/lidar/livox/net.py @@ -0,0 +1,85 @@ +# 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. + +"""Shared host_ip auto-detection for Livox SDK modules (Mid360 driver, Point-LIO).""" + +from __future__ import annotations + +import ipaddress +import socket + +from dimos.utils.generic import get_local_ips +from dimos.utils.logging_config import setup_logger + +_logger = setup_logger() + + +def resolve_host_ip(lidar_ip: str, configured: str | None, *, label: str) -> str: + """Resolve the local host IP the Livox SDK should bind to. + + Uses ``configured`` when it's one of our local IPs; otherwise auto-derives + the local NIC on the lidar's /24 subnet. The chosen IP is UDP-bind-tested + before returning. Raises ``RuntimeError`` with an actionable message when no + local IP is on the lidar's subnet or the bind fails. ``label`` prefixes log + and error messages (e.g. ``"PointLio"``, ``"Mid360"``). + """ + local_ips = [ip for ip, _iface in get_local_ips()] + + if configured and configured in local_ips: + host_ip = configured + else: + try: + lidar_net = ipaddress.IPv4Network(f"{lidar_ip}/24", strict=False) + same_subnet = [ip for ip in local_ips if ipaddress.IPv4Address(ip) in lidar_net] + except (ValueError, TypeError): + same_subnet = [] + if not same_subnet: + subnet_prefix = ".".join(lidar_ip.split(".")[:3]) + msg = ( + f"{label}: cannot resolve host_ip — no local IP on the lidar's subnet " + f"(lidar {lidar_ip}).\n" + f" Local IPs found: {', '.join(local_ips) or '(none)'}\n" + f" → Bring up the lidar NIC, or set host_ip explicitly.\n" + f" → Check: ip addr | grep {subnet_prefix}\n" + f" → Or assign: sudo ip addr add {subnet_prefix}.5/24 dev \n" + ) + _logger.error(msg) + raise RuntimeError(msg) + host_ip = same_subnet[0] + if configured: + _logger.warning( + f"{label}: host_ip={configured!r} not local; using {host_ip!r} " + f"(on lidar {lidar_ip}'s subnet).", + ) + + _logger.info(f"{label} network check", host_ip=host_ip, lidar_ip=lidar_ip, local_ips=local_ips) + + # Bind a UDP socket on host_ip (port 0 = ephemeral) to catch a host already + # holding the Livox SDK ports before the native binary starts. + try: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.bind((host_ip, 0)) + except OSError as err: + _logger.error( + f"{label}: Cannot bind UDP socket on host_ip={host_ip!r}: {err}\n" + f" Another process may be using the Livox SDK ports.\n" + f" → Check: ss -ulnp | grep {host_ip}" + ) + raise RuntimeError( + f"{label}: Cannot bind UDP on {host_ip}: {err}. " + f"Check if another Livox/PointLio process is running." + ) from err + + _logger.info(f"{label} network check passed", host_ip=host_ip, lidar_ip=lidar_ip) + return host_ip diff --git a/dimos/hardware/sensors/lidar/livox/pcap_recorder.py b/dimos/hardware/sensors/lidar/livox/pcap_recorder.py deleted file mode 100644 index 31f27e801f..0000000000 --- a/dimos/hardware/sensors/lidar/livox/pcap_recorder.py +++ /dev/null @@ -1,304 +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. - -"""Standalone Livox pcap recorder. - -Captures the raw Livox Mid-360 UDP packets to a libpcap file via tcpdump — the -ground-truth sensor input FastLio2 can be replayed against (see -fastlio2/tools/pcap_to_db.py). Kept separate from the SLAM module on purpose. - -tcpdump needs capture capability once per host: - sudo setcap cap_net_raw,cap_net_admin=eip $(which tcpdump) -""" - -from __future__ import annotations - -import asyncio -from collections.abc import AsyncIterator -import os -from pathlib import Path -import re -import shlex -import shutil -import signal -import subprocess -import textwrap -import time - -from pydantic import Field - -from dimos.core.module import Module, ModuleConfig -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -def _stop_when_parent_dies(cmd: list[str], grace_sec: float) -> list[str]: - """Reap cmd if the recorder dies, including via SIGKILL (which it can't - intercept) — otherwise tcpdump's own session would outlive it.""" - parent_pid = os.getpid() - quoted = " ".join(shlex.quote(arg) for arg in cmd) - # Foreground waits on tcpdump so a startup failure propagates its exit code. - script = textwrap.dedent(f""" - {quoted} & - child=$! - ( - while kill -0 {parent_pid} 2>/dev/null; do - sleep 0.5 - done - kill -INT "$child" 2>/dev/null - sleep {grace_sec} - kill -KILL "$child" 2>/dev/null - ) & - watcher=$! - wait "$child" - code=$? - kill "$watcher" 2>/dev/null - exit $code - """).strip() - return ["bash", "-c", script] - - -class LivoxPcapRecorderConfig(ModuleConfig): - """Where and how to capture the raw Livox UDP stream.""" - - pcap_path: str | Path = "raw_mid360.pcap" - # Machine-specific, so defaults from DIMOS_PCAP_IFACE env (fallback enp2s0). - record_pcap_iface: str = Field( - default_factory=lambda: os.environ.get("DIMOS_PCAP_IFACE", "enp2s0") - ) - record_pcap_snaplen: int = 2048 - lidar_ip: str = "192.168.1.107" - # Grace period for each stop signal (SIGINT→SIGTERM→SIGKILL) when tearing - # down the tcpdump capture. - pcap_stop_timeout: float = 5.0 - - -class LivoxPcapRecorder(Module): - """Owns a tcpdump process capturing raw Mid-360 UDP packets to a pcap.""" - - config: LivoxPcapRecorderConfig - - # tcpdump fails fast (EPERM, bad iface) within a few ms; pause briefly so poll() catches that. - _TCPDUMP_STARTUP_PROBE_SEC: float = 0.3 - # How long to let tcpdump run before declaring the capture dead if nothing landed. - _PCAP_WATCHDOG_SEC: float = 5.0 - # A libpcap file with zero packets is just its 24-byte global header. - _PCAP_GLOBAL_HEADER_BYTES: int = 24 - # How long the diagnostic sniff listens for *any* UDP source on the iface. - _PCAP_DIAGNOSTIC_SNIFF_SEC: float = 3.0 - - _pcap_proc: subprocess.Popen[bytes] | None = None - - async def main(self) -> AsyncIterator[None]: - self._start_pcap() - if self._pcap_proc is not None: - watchdog = asyncio.create_task(self._pcap_watchdog()) - else: - watchdog = None - yield - if watchdog is not None: - watchdog.cancel() - self._stop_pcap() - - def _start_pcap(self) -> None: - cfg = self.config - path = Path(cfg.pcap_path).expanduser() - path.parent.mkdir(parents=True, exist_ok=True) - - # Capture every UDP packet originating from the lidar. - packet_filter_expression = f"src host {cfg.lidar_ip} and udp" - tcpdump = shutil.which("tcpdump") or "tcpdump" - cmd = [ - tcpdump, - "-i", - cfg.record_pcap_iface, - "-w", - str(path), - "-s", - str(cfg.record_pcap_snaplen), - "-U", - "-n", - packet_filter_expression, - ] - - # Own session/group so _stop_pcap can signal the wrapper + tcpdump - # without touching the recorder, and Ctrl-C doesn't race shutdown. - proc = subprocess.Popen( - _stop_when_parent_dies(cmd, cfg.pcap_stop_timeout), - stdout=subprocess.DEVNULL, - stderr=subprocess.PIPE, - start_new_session=True, - ) - # tcpdump exits within a few ms on EPERM; wait briefly so we can detect that. - time.sleep(self._TCPDUMP_STARTUP_PROBE_SEC) - if proc.poll() is not None: - stderr = proc.stderr.read().decode(errors="replace") if proc.stderr else "" - self._pcap_proc = None - logger.error( - f"LivoxPcapRecorder failed to start — tcpdump exited" - f" rc={proc.returncode} stderr={stderr.strip()}" - ) - print( - "[livox_pcap] pcap recording is enabled but tcpdump cannot capture.\n" - " Grant capture capability once with:\n" - f" sudo setcap cap_net_raw,cap_net_admin=eip {tcpdump}\n" - " then restart. (tcpdump stderr above.)", - flush=True, - ) - return - - logger.info( - f"LivoxPcapRecorder capturing path={path} " - f"iface={cfg.record_pcap_iface} filter={packet_filter_expression!r}" - ) - self._pcap_proc = proc - - async def _pcap_watchdog(self) -> None: - """If tcpdump captured nothing after a few seconds, dump everything we - know about why — almost always a wrong lidar_ip or interface.""" - await asyncio.sleep(self._PCAP_WATCHDOG_SEC) - proc = self._pcap_proc - if proc is None: - return - path = Path(self.config.pcap_path).expanduser() - try: - size = path.stat().st_size - except OSError: - size = -1 - if size > self._PCAP_GLOBAL_HEADER_BYTES: - logger.info( - f"LivoxPcapRecorder pcap healthy — {size} bytes captured in " - f"{self._PCAP_WATCHDOG_SEC:.0f}s path={path}" - ) - return - report = await asyncio.to_thread(self._build_empty_pcap_report, size, proc) - logger.error(report) - print(report, flush=True) - - def _build_empty_pcap_report(self, size: int, proc: subprocess.Popen[bytes]) -> str: - cfg = self.config - packet_filter_expression = f"src host {cfg.lidar_ip} and udp" - proc_alive = proc.poll() is None - stderr_text = "" - if not proc_alive and proc.stderr is not None: - try: - stderr_text = proc.stderr.read().decode(errors="replace").strip() - except (OSError, ValueError): - stderr_text = "" - - observed = self._observed_udp_sources() - if observed: - listing = "\n".join( - f" {source} ({count} pkts)" - for source, count in sorted(observed.items(), key=lambda kv: kv[1], reverse=True) - ) - diagnosis = ( - f" UDP traffic IS flowing on {cfg.record_pcap_iface}, but from other source(s):\n" - f"{listing}\n" - f" None matched 'src host {cfg.lidar_ip}'. The lidar_ip is almost certainly\n" - f" wrong — set lidar_ip to whichever address above is the lidar and restart." - ) - else: - diagnosis = ( - f" NO UDP traffic at all was seen on {cfg.record_pcap_iface} during a " - f"{self._PCAP_DIAGNOSTIC_SNIFF_SEC:.0f}s probe.\n" - f" Wrong interface, unplugged cable, or the lidar is powered off." - ) - - neigh = self._run_quiet(["ip", "neigh", "show", cfg.lidar_ip]).strip() - return textwrap.dedent(f""" - ============================================================================ - [livox_pcap] PCAP WATCHDOG: 0 packets captured after {self._PCAP_WATCHDOG_SEC:.0f}s - ============================================================================ - Recording is enabled but tcpdump wrote an EMPTY pcap (size={size} bytes; an - empty libpcap file is {self._PCAP_GLOBAL_HEADER_BYTES} bytes of global header). - - Capture config: - interface : {cfg.record_pcap_iface} - lidar_ip : {cfg.lidar_ip} - filter : {packet_filter_expression!r} - pcap_path : {cfg.pcap_path} - tcpdump : alive={proc_alive} pid={proc.pid}{f" stderr={stderr_text!r}" if stderr_text else ""} - - Diagnosis: - {diagnosis} - - arp/neigh for {cfg.lidar_ip}: {neigh or ""} - ============================================================================ - """).strip() - - def _observed_udp_sources(self) -> dict[str, int]: - """Sniff the interface briefly and tally which source IPs are sending UDP.""" - tcpdump = shutil.which("tcpdump") or "tcpdump" - cmd = [tcpdump, "-i", self.config.record_pcap_iface, "-nn", "-c", "60", "udp"] - try: - result = subprocess.run( - cmd, - capture_output=True, - text=True, - timeout=self._PCAP_DIAGNOSTIC_SNIFF_SEC, - ) - output = result.stdout - except subprocess.TimeoutExpired as expired: - stdout = expired.stdout - output = ( - stdout.decode(errors="replace") if isinstance(stdout, bytes) else (stdout or "") - ) - except OSError: - return {} - counts: dict[str, int] = {} - for line in output.splitlines(): - match = re.search(r"\bIP6?\s+(\S+?)\.\d+\s+>", line) - if match: - source = match.group(1) - counts[source] = counts.get(source, 0) + 1 - return counts - - @staticmethod - def _run_quiet(cmd: list[str]) -> str: - try: - return subprocess.run(cmd, capture_output=True, text=True, timeout=2.0).stdout - except (OSError, subprocess.TimeoutExpired): - return "" - - def _stop_pcap(self) -> None: - proc = self._pcap_proc - if proc is None: - return - self._pcap_proc = None - if proc.poll() is not None: - return - # Signal the group so tcpdump gets it directly. SIGINT is its - # clean-stop signal (flushes the pcap); escalate if it hangs. - try: - pgid = os.getpgid(proc.pid) - except ProcessLookupError: - return - timeout = self.config.pcap_stop_timeout - for sig in (signal.SIGINT, signal.SIGTERM, signal.SIGKILL): - try: - os.killpg(pgid, sig) - except ProcessLookupError: - break - try: - proc.wait(timeout=timeout) - break - except subprocess.TimeoutExpired: - logger.warning( - f"tcpdump did not exit on {sig.name}; escalating path={self.config.pcap_path}" - ) - else: - proc.wait() - logger.info(f"LivoxPcapRecorder stopped path={self.config.pcap_path}") diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py b/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py deleted file mode 100644 index 275d010ff5..0000000000 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/module.py +++ /dev/null @@ -1,75 +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. - -"""Python NativeModule wrapper for the virtual_mid360 Rust binary. - -virtual_mid360 replays a recorded Livox Mid-360 pcap onto a virtual network -interface, rewriting packet timestamps to current-time and synthesizing the -Livox SDK2 control protocol so an unmodified consumer (e.g. PointLio) connects -to it as if it were a real sensor. It carries no dimos streams; it speaks the -Livox wire protocol over UDP, so consumers reach it by host_ip/lidar_ip, not by -stream wiring. - -Usage:: - - from dimos.hardware.sensors.lidar.livox.virtual_mid360.module import VirtualMid360 - from dimos.hardware.sensors.lidar.pointlio.module import PointLio - from dimos.core.coordination.blueprints import autoconnect - - autoconnect( - VirtualMid360.blueprint(pcap="/path/to/ruwik2.pcap"), - PointLio.blueprint(), - ) -""" - -from __future__ import annotations - -import os -from typing import TYPE_CHECKING - -from pydantic import Field - -from dimos.core.native_module import NativeModule, NativeModuleConfig - - -class VirtualMid360Config(NativeModuleConfig): - cwd: str | None = "." - executable: str = "result/bin/virtual_mid360" - build_command: str | None = "nix build .#default" - - # pcap/lidar_ip/host_ip/lidar_netns default from DIMOS_MID360_* env vars so - # blueprints needn't restate them. pcap/lidar_ip/host_ip are required — empty - # makes the binary error. - pcap: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_PCAP", "")) - # Replay speed; 1.0 = original timing. - rate: float = 1.0 - # Seconds to wait before streaming begins. - delay: float = 0.0 - # IP the fake lidar sends from (on this netns's veth). - lidar_ip: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_LIDAR_IP", "")) - # Host IP the data is delivered to (where the SDK listens). - host_ip: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_HOST_IP", "")) - # Network namespace the fake lidar runs in. - lidar_netns: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_NETNS", "lidar")) - # Multicast group for point/IMU. 224.1.1.5 is the Livox default the SDK joins. - mcast_data: str = "224.1.1.5" - - -class VirtualMid360(NativeModule): - config: VirtualMid360Config - - -# Verify the module constructs (mirrors the pointlio wrapper's check). -if TYPE_CHECKING: - VirtualMid360() diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.lock b/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.lock similarity index 99% rename from dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.lock rename to dimos/hardware/sensors/lidar/virtual_mid360/Cargo.lock index f9b68b4d57..53993a985c 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.lock +++ b/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.lock @@ -711,6 +711,7 @@ version = "0.1.0" dependencies = [ "dimos-module", "serde", + "socket2 0.5.10", "tokio", "tracing", "validator", diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml b/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.toml similarity index 80% rename from dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml rename to dimos/hardware/sensors/lidar/virtual_mid360/Cargo.toml index edbfe42b43..c3d9519cc3 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/Cargo.toml +++ b/dimos/hardware/sensors/lidar/virtual_mid360/Cargo.toml @@ -8,8 +8,9 @@ name = "virtual_mid360" path = "src/main.rs" [dependencies] -dimos-module = { path = "../../../../../../native/rust/dimos-module" } +dimos-module = { path = "../../../../../native/rust/dimos-module" } tokio = { version = "1", features = ["rt-multi-thread", "macros", "time", "net", "sync"] } serde = { version = "1", features = ["derive"] } +socket2 = "0.5" tracing = "0.1" validator = { version = "0.20", features = ["derive"] } diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py b/dimos/hardware/sensors/lidar/virtual_mid360/blueprints.py similarity index 56% rename from dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py rename to dimos/hardware/sensors/lidar/virtual_mid360/blueprints.py index 03822a5919..6e7ec19e46 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/blueprints.py +++ b/dimos/hardware/sensors/lidar/virtual_mid360/blueprints.py @@ -12,25 +12,19 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Blueprint: FastLio2 fed by a VirtualMid360 replaying a recorded pcap. +"""FastLio2 fed by a VirtualMid360 replaying a recorded pcap (live SDK path). -VirtualMid360 replays the pcap over the Livox wire protocol on a virtual NIC; -FastLio2 connects in live SDK mode, unaware the sensor is synthetic. They talk -over UDP on lidar_ip/host_ip, so the harness puts them in separate netns joined -by a veth — see fastlio2/tools/pcap_to_db.py. +Each module reads its own config from env vars (DIMOS_MID360_* for the sensor); +set the lidar/host IPs so the two ends agree. """ from dimos.core.coordination.blueprints import autoconnect from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 -from dimos.hardware.sensors.lidar.livox.virtual_mid360.module import VirtualMid360 +from dimos.hardware.sensors.lidar.virtual_mid360.module import VirtualMid360 from dimos.visualization.vis_module import vis_module -# Set pcap to a recorded Mid-360 capture before running, e.g.: -# dimos run virtual-mid360-fastlio --VirtualMid360.pcap /path/to/capture.pcap demo_virtual_mid360_fastlio = autoconnect( - VirtualMid360.blueprint( - pcap="", lidar_ip="192.168.1.155", host_ip="192.168.1.5", lidar_netns="fl_lidar" - ), + VirtualMid360.blueprint(), FastLio2.blueprint(), vis_module("rerun"), ).global_config(n_workers=3, robot_model="virtual_mid360_fastlio") diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.lock b/dimos/hardware/sensors/lidar/virtual_mid360/flake.lock similarity index 70% rename from dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.lock rename to dimos/hardware/sensors/lidar/virtual_mid360/flake.lock index a548660557..45bea619b2 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.lock +++ b/dimos/hardware/sensors/lidar/virtual_mid360/flake.lock @@ -1,20 +1,16 @@ { "nodes": { - "dimos-repo": { + "dimos-rust": { "flake": false, "locked": { - "lastModified": 1779865691, - "narHash": "sha256-2CVWcov7DiC1qX/B/zFKDJiSYsnbrZ3FNT/viprFWTQ=", - "ref": "refs/heads/jeff/feat/g1_raycast", - "rev": "51666bcd298c1d08bdee179f176f45c0a7dd417d", - "revCount": 744, - "type": "git", - "url": "file:../../../.." + "path": "../../../../../native/rust", + "type": "path" }, "original": { - "type": "git", - "url": "file:../../../.." - } + "path": "../../../../../native/rust", + "type": "path" + }, + "parent": [] }, "flake-utils": { "inputs": { @@ -36,11 +32,11 @@ }, "nixpkgs": { "locked": { - "lastModified": 1779560665, - "narHash": "sha256-tpyBcxPpcQb8ukyNF7DoCwfSY3VPsxHoYwj00Cayv5o=", + "lastModified": 1781577229, + "narHash": "sha256-lrp67w8AulE9Ks53n27I45ADSzbOCn4H+CNW1Ck8B+8=", "owner": "NixOS", "repo": "nixpkgs", - "rev": "64c08a7ca051951c8eae34e3e3cb1e202fe36786", + "rev": "567a49d1913ce81ac6e9582e3553dd90a955875f", "type": "github" }, "original": { @@ -52,7 +48,7 @@ }, "root": { "inputs": { - "dimos-repo": "dimos-repo", + "dimos-rust": "dimos-rust", "flake-utils": "flake-utils", "nixpkgs": "nixpkgs" } diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.nix b/dimos/hardware/sensors/lidar/virtual_mid360/flake.nix similarity index 67% rename from dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.nix rename to dimos/hardware/sensors/lidar/virtual_mid360/flake.nix index a74d6bb71b..15593fc049 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/flake.nix +++ b/dimos/hardware/sensors/lidar/virtual_mid360/flake.nix @@ -4,16 +4,18 @@ inputs = { nixpkgs.url = "github:NixOS/nixpkgs/nixos-unstable"; flake-utils.url = "github:numtide/flake-utils"; - # Relative git+file: reaches the repo root for the local dimos-module path - # deps (same approach as dimos/mapping/ray_tracing/rust). - dimos-repo = { url = "git+file:../../../../../.."; flake = false; }; + # Path input to the in-repo rust crates (mirrors pointlio/cpp's path: inputs). + # A plain path: (not git+file:) hashes the directory contents, so it carries no + # git-tree NAR hash — which varies by nix version / clean-vs-dirty checkout and + # breaks cross-machine builds. + dimos-rust = { url = "path:../../../../../native/rust"; flake = false; }; }; - outputs = { self, nixpkgs, flake-utils, dimos-repo }: + outputs = { self, nixpkgs, flake-utils, dimos-rust }: flake-utils.lib.eachDefaultSystem (system: let pkgs = import nixpkgs { inherit system; }; - sub = "dimos/hardware/sensors/lidar/livox/virtual_mid360"; + sub = "dimos/hardware/sensors/lidar/virtual_mid360"; src = pkgs.runCommand "virtual-mid360-src" {} '' mkdir -p $out/${sub} @@ -22,8 +24,8 @@ cp ${./Cargo.lock} $out/${sub}/Cargo.lock mkdir -p $out/native/rust - cp -r ${dimos-repo}/native/rust/dimos-module $out/native/rust/dimos-module - cp -r ${dimos-repo}/native/rust/dimos-module-macros $out/native/rust/dimos-module-macros + cp -r ${dimos-rust}/dimos-module $out/native/rust/dimos-module + cp -r ${dimos-rust}/dimos-module-macros $out/native/rust/dimos-module-macros ''; in { packages.default = pkgs.rustPlatform.buildRustPackage { diff --git a/dimos/hardware/sensors/lidar/virtual_mid360/module.py b/dimos/hardware/sensors/lidar/virtual_mid360/module.py new file mode 100644 index 0000000000..b901798695 --- /dev/null +++ b/dimos/hardware/sensors/lidar/virtual_mid360/module.py @@ -0,0 +1,180 @@ +# 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. + +"""Python NativeModule wrapper for the virtual_mid360 Rust binary. + + +Usage:: + + from dimos.hardware.sensors.lidar.virtual_mid360.module import VirtualMid360 + from dimos.hardware.sensors.lidar.pointlio.module import PointLio + from dimos.core.coordination.blueprints import autoconnect + + autoconnect( + VirtualMid360.blueprint(pcap="/path/to/ruwik2.pcap"), + PointLio.blueprint(), + ) +""" + +from __future__ import annotations + +import os +import subprocess +import sys +from typing import TYPE_CHECKING, Any + +from pydantic import Field + +from dimos.core.core import rpc +from dimos.core.native_module import NativeModule, NativeModuleConfig +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +# Synthetic /24 the host_ip + lidar_ip share so they route to each other. +_ALIAS_PREFIX_LEN = 24 +_ALIAS_NETMASK = "255.255.255.0" +# Host-route prefix length for the point/IMU multicast + discovery broadcast. +_HOST_ROUTE_LEN = 32 +# Livox SDK's discovery hello; the fake lidar answers it. +_DISCOVERY_BROADCAST = "255.255.255.255" +# macOS has no dummy interfaces — the synthetic IPs are aliased onto loopback. +_MACOS_IFACE = "lo0" + + +class VirtualMid360Config(NativeModuleConfig): + cwd: str | None = "." + executable: str = "result/bin/virtual_mid360" + build_command: str | None = "nix build .#default" + # The rust binary reads its config as a JSON object on stdin (required). + stdin_config: bool = True + # Keep the Python-only NIC knobs out of the CLI args mirrored to the binary. + cli_exclude: frozenset[str] = frozenset({"setup_network", "alias_iface"}) + + # pcap/lidar_ip/host_ip default from DIMOS_MID360_* env vars so blueprints + # needn't restate them. pcap is required — empty makes the binary error. + pcap: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_PCAP", "")) + # Replay speed; 1.0 = original timing. + rate: float = 1.0 + # Seconds to wait before streaming begins. + delay: float = 0.0 + # IP the fake lidar sends from (on the dummy alias interface). + lidar_ip: str = Field( + default_factory=lambda: os.environ.get("DIMOS_MID360_LIDAR_IP", "192.168.1.155") + ) + # Host IP the data is delivered to (where the SDK listens). + host_ip: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_HOST_IP", "")) + lidar_netns: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_NETNS", "")) + # Multicast group for point/IMU. 224.1.1.5 is the Livox default the SDK joins. + mcast_data: str = "224.1.1.5" + + # Auto-configure the virtual NIC (host_ip + lidar_ip on a dummy interface, + # with the Livox multicast/discovery routes) on start, via sudo. Set False + # if the interface is provisioned externally or real hardware is present. + setup_network: bool = True + # Name of the dummy interface the synthetic IPs are aliased onto. + alias_iface: str = "dimos-mid360" + + def to_config_dict(self) -> dict[str, Any]: + return {k: v for k, v in super().to_config_dict().items() if k not in self.cli_exclude} + + +class VirtualMid360(NativeModule): + config: VirtualMid360Config + + def _sudo(self, args: list[str], *, check: bool = True) -> None: + """Run a privileged command via sudo, raising on failure (when check).""" + result = subprocess.run(["sudo", *args], capture_output=True) + if check and result.returncode != 0: + stderr = result.stderr.decode("utf-8", errors="replace").strip() + raise RuntimeError( + f"[{self._module_label}] `sudo {' '.join(args)}` failed " + f"(exit {result.returncode}): {stderr}" + ) + + def _teardown_virtual_nic(self) -> None: + # Idempotent: missing aliases/routes/interface are fine (check=False). + cfg = self.config + if sys.platform == "darwin": + for ip in (cfg.host_ip, cfg.lidar_ip): + self._sudo(["ifconfig", _MACOS_IFACE, "-alias", ip], check=False) + for dst in (cfg.mcast_data, _DISCOVERY_BROADCAST): + self._sudo( + ["route", "-n", "delete", "-host", dst, "-interface", _MACOS_IFACE], check=False + ) + else: + self._sudo(["ip", "link", "del", cfg.alias_iface], check=False) + + def _setup_virtual_nic(self) -> None: + self._teardown_virtual_nic() + if sys.platform == "darwin": + self._setup_macos() + elif sys.platform.startswith("linux"): + self._setup_linux() + else: + raise RuntimeError( + f"[{self._module_label}] setup_network supports Linux (iproute2) and macOS; " + f"got {sys.platform}. Provision the interface yourself and set setup_network=False." + ) + logger.info( + "Virtual Mid-360 NIC configured", + module=self._module_label, + platform=sys.platform, + host_ip=self.config.host_ip, + lidar_ip=self.config.lidar_ip, + ) + + def _setup_macos(self) -> None: + """Alias host_ip + lidar_ip onto loopback and route the Livox point/IMU + multicast (and the discovery broadcast) there, so the local SDK sees the + fake sensor over lo0. macOS has no dummy interfaces / netns.""" + cfg = self.config + for ip in (cfg.host_ip, cfg.lidar_ip): + self._sudo(["ifconfig", _MACOS_IFACE, "alias", ip, "netmask", _ALIAS_NETMASK]) + self._sudo(["route", "-n", "add", "-host", cfg.mcast_data, "-interface", _MACOS_IFACE]) + # Best-effort: the limited broadcast may already be deliverable on lo0. + self._sudo( + ["route", "-n", "add", "-host", _DISCOVERY_BROADCAST, "-interface", _MACOS_IFACE], + check=False, + ) + + def _setup_linux(self) -> None: + cfg = self.config + iface = cfg.alias_iface + self._sudo(["ip", "link", "add", iface, "type", "dummy"]) + self._sudo(["ip", "addr", "add", f"{cfg.host_ip}/{_ALIAS_PREFIX_LEN}", "dev", iface]) + self._sudo(["ip", "addr", "add", f"{cfg.lidar_ip}/{_ALIAS_PREFIX_LEN}", "dev", iface]) + self._sudo(["ip", "link", "set", iface, "up"]) + self._sudo(["ip", "link", "set", iface, "multicast", "on"]) + self._sudo(["ip", "route", "add", f"{cfg.mcast_data}/{_HOST_ROUTE_LEN}", "dev", iface]) + self._sudo( + ["ip", "route", "add", f"{_DISCOVERY_BROADCAST}/{_HOST_ROUTE_LEN}", "dev", iface] + ) + + @rpc + def build(self) -> None: + super().build() + if self.config.setup_network: + self._setup_virtual_nic() + + @rpc + def stop(self) -> None: + super().stop() + if self.config.setup_network: + self._teardown_virtual_nic() + + +# Verify the module constructs (mirrors the pointlio wrapper's check). +if TYPE_CHECKING: + VirtualMid360() diff --git a/dimos/hardware/sensors/lidar/virtual_mid360/recorder.py b/dimos/hardware/sensors/lidar/virtual_mid360/recorder.py new file mode 100644 index 0000000000..7710601bad --- /dev/null +++ b/dimos/hardware/sensors/lidar/virtual_mid360/recorder.py @@ -0,0 +1,393 @@ +# 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. + +"""tcpdump-based recorder for raw Livox Mid-360""" + +from __future__ import annotations + +import asyncio +from datetime import datetime +import getpass +import os +from pathlib import Path +import re +import shlex +import shutil +import signal +import subprocess +import textwrap +import time + +from pydantic import Field + +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +def _stamp() -> str: + now = datetime.now() + return now.strftime("%Y-%m-%d_%I-%M%p").lower() + + +def _default_pcap_path() -> Path: + return Path("recordings") / f"mid360_{_stamp()}.pcap" + + +def _stop_when_parent_dies(cmd: list[str], grace_sec: float) -> list[str]: + """complicated because of AppArmor label. Must kill with `sudo -n aa-exec -p unconfined`""" + parent_pid = os.getpid() + quoted = " ".join(shlex.quote(arg) for arg in cmd) + # Resolved here so the failure echo can show real paths + the long-term fix. + aa = shutil.which("aa-exec") or "/usr/sbin/aa-exec" + kill = shutil.which("kill") or "/usr/bin/kill" + user = getpass.getuser() + sudoers = "/etc/sudoers.d/dimos-mid360-pcap-kill" + # Narrow rule: passwordless for ONLY the unconfined kill, not all sudo. + rule = f"{user} ALL=(root) NOPASSWD: {aa} -p unconfined -- {kill} *" + long_term_fix = ( + f"Long-term fix (passwordless for ONLY this kill, not all sudo): " + f"echo '{rule}' | sudo tee {sudoers} && sudo chmod 440 {sudoers}" + ) + + def _kill(sig: str) -> str: + return ( + f'kill -{sig} "$child" 2>/dev/null' + f' || sudo -n {aa} -p unconfined -- {kill} -{sig} "$child" 2>/dev/null' + f' || echo "[mid360_record] FAILED to {sig} tcpdump pid $child' + f" (AppArmor blocked it + sudo -n could not escalate) — it is ORPHANED." + f" Kill it now: sudo {aa} -p unconfined -- {kill} -9 $child." + f' {long_term_fix}" >&2' + ) + + # Foreground waits on tcpdump so a startup failure propagates its exit code. + script = textwrap.dedent(f""" + {quoted} & + child=$! + ( + while kill -0 {parent_pid} 2>/dev/null; do + sleep 0.5 + done + {_kill("INT")} + sleep {grace_sec} + {_kill("KILL")} + ) & + watcher=$! + wait "$child" + code=$? + kill "$watcher" 2>/dev/null + exit $code + """).strip() + return ["bash", "-c", script] + + +class Mid360PcapRecorderConfig(ModuleConfig): + pcap_path: Path = Field(default_factory=_default_pcap_path) + iface: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_PCAP_IFACE", "")) + lidar_ip: str = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_LIDAR_IP", "")) + snaplen: int = 2048 + stop_timeout: float = 5.0 + + +class Mid360PcapRecorder(Module): + config: Mid360PcapRecorderConfig + + _TCPDUMP_STARTUP_PROBE_SEC: float = 0.3 + # Declare the capture dead if nothing landed after this long. + _PCAP_WATCHDOG_SEC: float = 5.0 + _PCAP_GLOBAL_HEADER_BYTES: int = 24 + _PCAP_DIAGNOSTIC_SNIFF_SEC: float = 3.0 + + _pcap_proc: subprocess.Popen[bytes] | None = None + + @rpc + def start(self) -> None: + self._start_pcap() + super().start() + if self._pcap_proc is not None: + self.spawn(self._pcap_watchdog()) + + @rpc + def stop(self) -> None: + try: + super().stop() + finally: + self._stop_pcap() + + def _filter(self) -> str: + return f"src host {self.config.lidar_ip} and udp" + + def _start_pcap(self) -> None: + cfg = self.config + if not cfg.lidar_ip: + raise ValueError( + "Mid360PcapRecorder requires lidar_ip — pass lidar_ip=... or set " + "DIMOS_MID360_LIDAR_IP. It's the real Mid-360's IP, used to filter the capture." + ) + path = Path(cfg.pcap_path).expanduser() + path.parent.mkdir(parents=True, exist_ok=True) + + tcpdump = shutil.which("tcpdump") or "tcpdump" + cmd = [ + tcpdump, + "-i", + cfg.iface, + "-w", + str(path), + "-s", + str(cfg.snaplen), + "-U", # packet-buffered: flush each packet so a kill loses nothing + "-n", + self._filter(), + ] + # Own session so _stop_pcap can signal the wrapper + tcpdump without + # touching the recorder, and Ctrl-C doesn't race shutdown. + proc = subprocess.Popen( + _stop_when_parent_dies(cmd, cfg.stop_timeout), + stdout=subprocess.DEVNULL, + stderr=subprocess.PIPE, + start_new_session=True, + ) + time.sleep(self._TCPDUMP_STARTUP_PROBE_SEC) + if proc.poll() is not None: + stderr = proc.stderr.read().decode(errors="replace") if proc.stderr else "" + self._pcap_proc = None + logger.error( + f"Mid360PcapRecorder: tcpdump exited rc={proc.returncode} stderr={stderr.strip()}" + ) + print( + "[mid360_record] tcpdump cannot capture. Grant capture capability once with:\n" + f" sudo setcap cap_net_raw,cap_net_admin=eip {tcpdump}\n" + " then restart. (tcpdump stderr above.)", + flush=True, + ) + return + + logger.info( + f"Mid360PcapRecorder capturing path={path} iface={cfg.iface} " + f"filter={self._filter()!r}" + ) + self._pcap_proc = proc + + async def _pcap_watchdog(self) -> None: + """If tcpdump captured nothing after a few seconds, report why — almost + always a wrong lidar_ip or interface.""" + await asyncio.sleep(self._PCAP_WATCHDOG_SEC) + proc = self._pcap_proc + if proc is None: + return + path = Path(self.config.pcap_path).expanduser() + try: + size = path.stat().st_size + except OSError: + size = -1 + if size > self._PCAP_GLOBAL_HEADER_BYTES: + logger.info( + f"Mid360PcapRecorder healthy — {size} bytes captured in " + f"{self._PCAP_WATCHDOG_SEC:.0f}s path={path}" + ) + return + report = await asyncio.to_thread(self._build_empty_pcap_report, size, proc) + logger.error(report) + print(report, flush=True) + + def _build_empty_pcap_report(self, size: int, proc: subprocess.Popen[bytes]) -> str: + cfg = self.config + proc_alive = proc.poll() is None + stderr_text = "" + if not proc_alive and proc.stderr is not None: + try: + stderr_text = proc.stderr.read().decode(errors="replace").strip() + except (OSError, ValueError): + stderr_text = "" + + observed = self._observed_udp_sources() + if observed: + listing = "\n".join( + f" {source} ({count} pkts)" + for source, count in sorted(observed.items(), key=lambda kv: kv[1], reverse=True) + ) + diagnosis = ( + f" UDP traffic IS flowing on {cfg.iface}, but from other source(s):\n" + f"{listing}\n" + f" None matched 'src host {cfg.lidar_ip}'. The lidar_ip is almost\n" + f" certainly wrong — set it to whichever address above is the lidar." + ) + else: + diagnosis = ( + f" NO UDP traffic at all was seen on {cfg.iface} during a " + f"{self._PCAP_DIAGNOSTIC_SNIFF_SEC:.0f}s probe.\n" + f" Wrong interface, unplugged cable, or the lidar is powered off." + ) + + neigh = self._run_quiet(["ip", "neigh", "show", cfg.lidar_ip]).strip() + return textwrap.dedent(f""" + ============================================================================ + [mid360_record] PCAP WATCHDOG: 0 packets captured after {self._PCAP_WATCHDOG_SEC:.0f}s + ============================================================================ + tcpdump wrote an EMPTY pcap (size={size} bytes; an empty libpcap file is + {self._PCAP_GLOBAL_HEADER_BYTES} bytes of global header). + + Capture config: + interface : {cfg.iface} + lidar_ip : {cfg.lidar_ip} + filter : {self._filter()!r} + pcap_path : {cfg.pcap_path} + tcpdump : alive={proc_alive} pid={proc.pid}{f" stderr={stderr_text!r}" if stderr_text else ""} + + Diagnosis: + {diagnosis} + + arp/neigh for {cfg.lidar_ip}: {neigh or ""} + ============================================================================ + """).strip() + + def _observed_udp_sources(self) -> dict[str, int]: + """Sniff the interface briefly and tally which source IPs send UDP.""" + tcpdump = shutil.which("tcpdump") or "tcpdump" + cmd = [tcpdump, "-i", self.config.iface, "-nn", "-c", "60", "udp"] + try: + result = subprocess.run( + cmd, capture_output=True, text=True, timeout=self._PCAP_DIAGNOSTIC_SNIFF_SEC + ) + output = result.stdout + except subprocess.TimeoutExpired as expired: + stdout = expired.stdout + output = ( + stdout.decode(errors="replace") if isinstance(stdout, bytes) else (stdout or "") + ) + except OSError: + return {} + counts: dict[str, int] = {} + for line in output.splitlines(): + match = re.search(r"\bIP6?\s+(\S+?)\.\d+\s+>", line) + if match: + counts[match.group(1)] = counts.get(match.group(1), 0) + 1 + return counts + + @staticmethod + def _run_quiet(cmd: list[str]) -> str: + try: + return subprocess.run(cmd, capture_output=True, text=True, timeout=2.0).stdout + except (OSError, subprocess.TimeoutExpired): + return "" + + def _stop_pcap(self) -> None: + proc = self._pcap_proc + if proc is None: + return + self._pcap_proc = None + if proc.poll() is not None: + return + # Signal the group so tcpdump gets it directly. SIGINT is its clean-stop + # signal (flushes the pcap); escalate if it hangs. + try: + pgid = os.getpgid(proc.pid) + except ProcessLookupError: + return + timeout = self.config.stop_timeout + for sig in (signal.SIGINT, signal.SIGTERM, signal.SIGKILL): + if not self._signal_group(pgid, sig): + break + try: + proc.wait(timeout=timeout) + break + except subprocess.TimeoutExpired: + logger.warning( + f"tcpdump did not exit on {sig.name}; escalating path={self.config.pcap_path}" + ) + # The bash wrapper can die while a confined tcpdump survives its + # AppArmor-blocked signal (the unconfined fallback couldn't escalate) — + # so check tcpdump directly rather than trusting proc.wait(). + if self._tcpdump_pid() is not None: + self._scream_orphaned() + else: + logger.info(f"Mid360PcapRecorder stopped path={self.config.pcap_path}") + + def _signal_group(self, pgid: int, sig: signal.Signals) -> bool: + """Signal the tcpdump process group; False if it's already gone. + + tcpdump's AppArmor profile rejects signals from a confined (e.g. + vscode-labeled) sender with EPERM, so a plain killpg silently fails + there — fall back to re-issuing from an unconfined label, the same + escape the `kd` command uses. No-op where AppArmor isn't in the way.""" + try: + os.killpg(pgid, sig) + return True + except ProcessLookupError: + return False + except PermissionError: + pass + # kill - -- - (negative pid = the whole group) + aa = shutil.which("aa-exec") + if aa is None: + return True + cmd = [aa, "-p", "unconfined", "--", "kill", f"-{int(sig)}", "--", f"-{pgid}"] + if os.geteuid() != 0 and shutil.which("sudo"): + cmd = ["sudo", "-n", *cmd] + try: + subprocess.run(cmd, capture_output=True, timeout=3.0) + except (OSError, subprocess.TimeoutExpired): + pass + return True + + def _tcpdump_pid(self) -> int | None: + """PID of a tcpdump still writing our pcap, or None — used to detect an + orphan that survived the stop because its signal was AppArmor-blocked.""" + path = str(Path(self.config.pcap_path).expanduser()) + try: + result = subprocess.run( + ["pgrep", "-af", "tcpdump"], capture_output=True, text=True, timeout=2.0 + ) + except (OSError, subprocess.TimeoutExpired): + return None + for line in result.stdout.splitlines(): + if path in line: + try: + return int(line.split(None, 1)[0]) + except (ValueError, IndexError): + continue + return None + + def _scream_orphaned(self) -> None: + """Loudly report a tcpdump that outlived the stop, with the exact fix.""" + pid = self._tcpdump_pid() + aa = shutil.which("aa-exec") or "/usr/sbin/aa-exec" + kill = shutil.which("kill") or "/usr/bin/kill" + user = getpass.getuser() + # Narrow sudoers rule: passwordless for ONLY the unconfined kill. + rule = f"{user} ALL=(root) NOPASSWD: {aa} -p unconfined -- {kill} *" + banner = textwrap.dedent(f""" + ############################################################################ + !!! kill failed - mid360_record WILL EAT YOUR DISK IF YOU DONT KILL !!! + ############################################################################ + tcpdump pid={pid} is STILL RUNNING and writing {self.config.pcap_path}. + AppArmor's tcpdump profile rejected the kill from this (confined) process, + and the unconfined fallback could not escalate (sudo -n needs a password, + or aa-exec is missing). It will NOT be reaped on its own. + + Kill it now: + sudo {aa} -p unconfined -- {kill} -9 {pid} + + To let the recorder kill it itself next time — passwordless for ONLY this + unconfined kill, not all sudo — install a narrow sudoers rule: + echo '{rule}' | sudo tee /etc/sudoers.d/dimos-mid360-pcap-kill + sudo chmod 440 /etc/sudoers.d/dimos-mid360-pcap-kill + (Verify the paths match `command -v aa-exec` and `command -v kill`.) + ############################################################################ + """).strip() + logger.error(banner) + print(banner, flush=True) diff --git a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs b/dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs similarity index 75% rename from dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs rename to dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs index 7a53eaab2d..17303b70d7 100644 --- a/dimos/hardware/sensors/lidar/livox/virtual_mid360/src/main.rs +++ b/dimos/hardware/sensors/lidar/virtual_mid360/src/main.rs @@ -1,16 +1,15 @@ // Fake Livox Mid-360 — replays a recorded pcap over a virtual NIC and synthesizes // the Livox SDK2 control handshake so an unmodified, live-mode pointlio ingests it -// through the real Livox SDK as if from a live sensor. Runs in the "lidar" netns -// (peer "drv" runs pointlio); on a setup failure the error prints the exact -// netns/veth commands to run. +// through the real Livox SDK as if from a live sensor. Namespace-agnostic: it just +// binds lidar_ip and sends UDP, so it works wherever the host_ip/lidar_ip are +// reachable — IPs aliased on an interface (host ns, incl. macOS lo0) or a netns. -use dimos_module::{run, LcmTransport, Module}; -use serde::Deserialize; +use dimos_module::{native_config, run, LcmTransport, Module}; +use socket2::{Domain, Protocol, Socket, Type}; use std::net::{Ipv4Addr, SocketAddrV4, UdpSocket}; use std::sync::atomic::{AtomicBool, Ordering}; use std::sync::Arc; use std::time::{Duration, Instant, SystemTime, UNIX_EPOCH}; -use validator::Validate; // ---- Livox SDK2 control wire format (SdkPacket) ---- const SOF: u8 = 0xAA; @@ -27,8 +26,11 @@ const DST_STATUS: u16 = 56201; // cmd_id whose ACK means the host finished configuring -> start streaming const CMD_WORKMODE: u16 = 0x0100; -#[derive(Debug, Deserialize, Validate)] -#[serde(deny_unknown_fields)] +// native_config: every field required + supplied by the Python wrapper over +// stdin (no Rust-side serde defaults / Option). VirtualMid360Config sends all of +// these, so each is unconditionally present. Injects the +// Deserialize/Serialize/Validate derives + deny_unknown_fields + impl NativeConfig. +#[native_config] struct Config { /// Recorded Mid-360 pcap (point/IMU/status UDP). Read fully into RAM. pcap: String, @@ -36,25 +38,22 @@ struct Config { #[validate(range(min = 0.01, max = 1000.0))] rate: f64, /// Seconds to wait before streaming begins. - #[serde(default)] #[validate(range(min = 0.0, max = 3600.0))] delay: f64, - /// IP the fake lidar sends from (on this netns's veth). Required. + /// IP the fake lidar sends from. lidar_ip: String, - /// Host IP the data is delivered to (where the SDK listens). Required. + /// Host IP the data is delivered to (where the SDK listens). host_ip: String, - /// Network namespace the fake lidar runs in. Required. + /// Network namespace the fake lidar runs in. Accepted for wire-config + /// compatibility but not acted on: the process is *placed* in the netns by + /// the launcher (`ip netns exec`), so the binary itself stays agnostic. + #[allow(dead_code)] lidar_netns: String, /// Multicast group for point/IMU. 224.1.1.5 is the Livox default the SDK /// joins; override only to match a differently-configured consumer. - #[serde(default = "default_mcast_data")] mcast_data: String, } -fn default_mcast_data() -> String { - "224.1.1.5".into() -} - #[derive(Module)] #[module(setup = start)] struct VirtualMid360 { @@ -182,32 +181,34 @@ fn ensure_interface(cfg: &Config) -> Result { // (or we're in the wrong namespace). let probe = UdpSocket::bind(SocketAddrV4::new(lidar_ip, CMD_PORT)); if probe.is_err() { - let netns = &cfg.lidar_netns; let lidar_addr = &cfg.lidar_ip; let host_addr = &cfg.host_ip; let mcast_group = &cfg.mcast_data; + // The VirtualMid360 module sets the NIC up automatically (setup_network, + // via sudo); this fires only when that was skipped/failed. Show the + // by-hand recipe for the current platform. + let how = if cfg!(target_os = "macos") { + format!( + "macOS — alias the IPs onto loopback and route the Livox multicast there:\n \ + sudo ifconfig lo0 alias {host_addr} netmask 255.255.255.0\n \ + sudo ifconfig lo0 alias {lidar_addr} netmask 255.255.255.0\n \ + sudo route -n add -host {mcast_group} -interface lo0\n \ + sudo route -n add -host 255.255.255.255 -interface lo0" + ) + } else { + format!( + "Linux — alias the IPs onto a dummy interface (no netns needed):\n \ + sudo ip link add dimos-mid360 type dummy\n \ + sudo ip addr add {host_addr}/24 dev dimos-mid360\n \ + sudo ip addr add {lidar_addr}/24 dev dimos-mid360\n \ + sudo ip link set dimos-mid360 up\n \ + sudo ip link set dimos-mid360 multicast on\n \ + sudo ip route add {mcast_group}/32 dev dimos-mid360\n \ + sudo ip route add 255.255.255.255/32 dev dimos-mid360" + ) + }; return Err(format!( - "cannot bind {lidar_addr}:{CMD_PORT} — the virtual network interface isn't set up \ - (or this process isn't in the '{netns}' netns).\n\ - Run this once (creates the lidar/drv veth pair), then re-run the module:\n\ - \n sudo ip netns add drv\n sudo ip netns add {netns}\n \ - sudo ip link add veth-drv type veth peer name veth-lidar\n \ - sudo ip link set veth-drv netns drv\n \ - sudo ip link set veth-lidar netns {netns}\n \ - sudo ip netns exec drv ip addr add {host_addr}/24 dev veth-drv\n \ - sudo ip netns exec {netns} ip addr add {lidar_addr}/24 dev veth-lidar\n \ - sudo ip netns exec drv ip link set veth-drv up\n \ - sudo ip netns exec {netns} ip link set veth-lidar up\n \ - sudo ip netns exec drv ip link set lo up\n \ - sudo ip netns exec {netns} ip link set lo up\n \ - sudo ip netns exec drv ip link set veth-drv multicast on\n \ - sudo ip netns exec {netns} ip link set veth-lidar multicast on\n \ - sudo ip netns exec {netns} ip route add 255.255.255.255/32 dev veth-lidar\n \ - sudo ip netns exec {netns} ip route add {mcast_group}/32 dev veth-lidar # point/IMU multicast\n \ - sudo ip netns exec drv ip route add 224.0.0.0/4 dev lo # LCM (dimos transport)\n \ - sudo ip netns exec {netns} ip route add 224.0.0.0/4 dev lo # LCM (dimos transport)\n\ - \nThen launch this module inside the lidar netns:\n \ - sudo ip netns exec {netns} " + "cannot bind {lidar_addr}:{CMD_PORT} — the virtual NIC isn't set up.\n{how}" )); } Ok(lidar_ip) @@ -221,7 +222,6 @@ impl VirtualMid360 { Err(msg) => { // Exit non-zero so the coordinator surfaces the fix command. tracing::error!("{msg}"); - eprintln!("\n[virtual_mid360] {msg}\n"); std::process::exit(2); } }; @@ -229,7 +229,7 @@ impl VirtualMid360 { let mcast_data: Ipv4Addr = match cfg.mcast_data.parse() { Ok(ip) => ip, Err(_) => { - eprintln!( + tracing::error!( "[virtual_mid360] invalid mcast_data '{}' — expected an IPv4 multicast \ address matching the consumer's Livox multicast_ip (default 224.1.1.5).", cfg.mcast_data @@ -241,7 +241,7 @@ impl VirtualMid360 { let packets = match parse_pcap(&cfg.pcap) { Ok(parsed) if !parsed.is_empty() => Arc::new(parsed), Ok(_) => { - eprintln!( + tracing::error!( "[virtual_mid360] pcap '{}' has no Livox UDP data packets. \ Check the path / that it's a Mid-360 capture, then re-run.", cfg.pcap @@ -249,7 +249,7 @@ impl VirtualMid360 { std::process::exit(2); } Err(err) => { - eprintln!( + tracing::error!( "[virtual_mid360] failed to read pcap '{}': {err}. Fix the path, then re-run.", cfg.pcap ); @@ -262,8 +262,8 @@ impl VirtualMid360 { let rate = cfg.rate; let delay = cfg.delay; - // discovery responder (:56000) — answers the 0x0000 broadcast - spawn_discovery(lidar_ip, stop.clone()); + // discovery responder (:56000) — proactively announces + answers 0x0000 + spawn_discovery(lidar_ip, host_ip, stop.clone()); // control responder (:56100) — per-cmd ACKs; arms streaming on 0x0100 spawn_control(lidar_ip, armed.clone(), stop.clone()); // data streamer — point/IMU/status paced at `rate`, timestamps shifted to now @@ -274,39 +274,62 @@ impl VirtualMid360 { } } -fn spawn_discovery(lidar_ip: Ipv4Addr, stop: Arc) { +/// UDP socket bound with SO_REUSEADDR so it can share a port with the consumer +/// SDK's own sockets when both run in one network namespace — macOS (and Linux +/// alias mode) have no netns to separate the two endpoints. +fn reuse_bind(addr: SocketAddrV4) -> std::io::Result { + let socket = Socket::new(Domain::IPV4, Type::DGRAM, Some(Protocol::UDP))?; + socket.set_reuse_address(true)?; + // SO_REUSEPORT too: the consumer SDK opens its own :56000 sockets (one on + // INADDR_ANY), and on macOS a wildcard bind can't be added over an existing + // specific bind with SO_REUSEADDR alone — so without this the two race and + // whichever loses fails to bind. REUSEPORT makes the binds order-independent. + socket.set_reuse_port(true)?; + let bind_addr: std::net::SocketAddr = addr.into(); + socket.bind(&bind_addr.into())?; + Ok(socket.into()) +} + +fn spawn_discovery(lidar_ip: Ipv4Addr, host_ip: Ipv4Addr, stop: Arc) { std::thread::spawn(move || { - let socket = match UdpSocket::bind(SocketAddrV4::new(Ipv4Addr::UNSPECIFIED, DISCOVERY_PORT)) - { + // Bind the lidar's detection port (not INADDR_ANY): SO_REUSEADDR + a + // specific source IP lets this coexist with the consumer SDK's own + // :56000 sockets in a shared namespace, and makes our packets arrive + // *from* lidar_ip:56000 (which is how the SDK identifies the device). + let socket = match reuse_bind(SocketAddrV4::new(lidar_ip, DISCOVERY_PORT)) { Ok(socket) => socket, Err(err) => { - tracing::error!("discovery bind :{DISCOVERY_PORT} failed: {err}"); + tracing::error!("discovery bind {lidar_ip}:{DISCOVERY_PORT} failed: {err}"); return; } }; - let _ = socket.set_broadcast(true); socket - .set_read_timeout(Some(Duration::from_millis(500))) + .set_read_timeout(Some(Duration::from_millis(200))) .ok(); - let broadcast_addr = SocketAddrV4::new(Ipv4Addr::BROADCAST, DISCOVERY_PORT); + // The SDK solicits lidars by broadcasting to 255.255.255.255, which macOS + // refuses to send — so it can never reach us. Instead we *proactively* + // unicast the search-ACK to the host's detection port; the SDK accepts an + // unsolicited detection response (it matches no request seq — none is + // required for cmd 0x0000) and registers the device. Harmless on Linux, + // where the broadcast path also works. + let host_detect = SocketAddrV4::new(host_ip, DISCOVERY_PORT); + let announce = build_ack(0x0000, 0, &discovery_ack_payload(lidar_ip)); let mut buffer = [0u8; 2048]; while !stop.load(Ordering::Relaxed) { - let len = match socket.recv_from(&mut buffer) { - Ok((len, _)) => len, - Err(_) => continue, - }; - if len < WRAPPER || buffer[0] != SOF { - continue; + let _ = socket.send_to(&announce, host_detect); + // Also answer a real broadcast solicitation if one arrives, echoing + // its seq (the original live/netns path). + if let Ok((len, _)) = socket.recv_from(&mut buffer) { + if len >= WRAPPER + && buffer[0] == SOF + && u16::from_le_bytes([buffer[8], buffer[9]]) == 0x0000 + && buffer[10] == 0 + { + let seq = u32::from_le_bytes([buffer[4], buffer[5], buffer[6], buffer[7]]); + let ack = build_ack(0x0000, seq, &discovery_ack_payload(lidar_ip)); + let _ = socket.send_to(&ack, host_detect); + } } - let cmd_id = u16::from_le_bytes([buffer[8], buffer[9]]); - let cmd_type = buffer[10]; - if cmd_id != 0x0000 || cmd_type != 0 { - continue; - } - let seq = u32::from_le_bytes([buffer[4], buffer[5], buffer[6], buffer[7]]); - // ACK describes the device (dev_type, serial, lidar_ip, cmd port). - let ack = build_ack(0x0000, seq, &discovery_ack_payload(lidar_ip)); - let _ = socket.send_to(&ack, broadcast_addr); } }); } diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 9fd2d4b94e..b6970be389 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -54,7 +54,7 @@ "demo-object-scene-registration": "dimos.perception.demo_object_scene_registration:demo_object_scene_registration", "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", - "demo-virtual-mid360-fastlio": "dimos.hardware.sensors.lidar.livox.virtual_mid360.blueprints:demo_virtual_mid360_fastlio", + "demo-virtual-mid360-fastlio": "dimos.hardware.sensors.lidar.virtual_mid360.blueprints:demo_virtual_mid360_fastlio", "desk-marker-tf": "dimos.perception.fiducial.blueprints.desk_marker_tf:desk_marker_tf", "drone-agentic": "dimos.robot.drone.blueprints.agentic.drone_agentic:drone_agentic", "drone-basic": "dimos.robot.drone.blueprints.basic.drone_basic:drone_basic", @@ -179,7 +179,7 @@ "joystick-module": "dimos.robot.unitree.b1.joystick_module.JoystickModule", "keyboard-teleop": "dimos.robot.unitree.keyboard_teleop.KeyboardTeleop", "keyboard-teleop-module": "dimos.teleop.keyboard.keyboard_teleop_module.KeyboardTeleopModule", - "livox-pcap-recorder": "dimos.hardware.sensors.lidar.livox.pcap_recorder.LivoxPcapRecorder", + "mid360-pcap-recorder": "dimos.hardware.sensors.lidar.virtual_mid360.recorder.Mid360PcapRecorder", "local-planner": "dimos.navigation.nav_stack.modules.local_planner.local_planner.LocalPlanner", "manipulation-module": "dimos.manipulation.manipulation_module.ManipulationModule", "map": "dimos.robot.unitree.type.map.Map", @@ -236,7 +236,7 @@ "unitree-skill-container": "dimos.robot.unitree.unitree_skill_container.UnitreeSkillContainer", "unity-bridge-module": "dimos.simulation.unity.module.UnityBridgeModule", "video-arm-teleop-module": "dimos.teleop.quest.quest_extensions.VideoArmTeleopModule", - "virtual-mid360": "dimos.hardware.sensors.lidar.livox.virtual_mid360.module.VirtualMid360", + "virtual-mid360": "dimos.hardware.sensors.lidar.virtual_mid360.module.VirtualMid360", "vlm-agent": "dimos.agents.vlm_agent.VLMAgent", "vlm-stream-tester": "dimos.agents.vlm_stream_tester.VlmStreamTester", "voxel-grid-mapper": "dimos.mapping.voxels.VoxelGridMapper", From c458de8caccfbf188a3efe3fc7468444852fce93 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 06:00:16 -0700 Subject: [PATCH 137/185] fix(pointlio): use z.max()-z.min() not np.ptp in pcap_to_db .rrd gradient ndarray.ptp() is gone in NumPy 2.0; avoid the np.ptp ambiguity entirely with the explicit max-min so the height-gradient color can't crash _write_rrd. --- dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py index 57c9c914d1..b0330c5ecf 100644 --- a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py @@ -152,7 +152,7 @@ def _write_rrd(db_path: Path, odom_stream: str, lidar_stream: str, voxel: float) # Height gradient: hot pink (low) -> dark purple (high). z = agg[:, 2] - zn = (z - z.min()) / (np.ptp(z) + 1e-9) + zn = (z - z.min()) / ((z.max() - z.min()) + 1e-9) low = np.array([255, 20, 147], dtype=np.float64) high = np.array([60, 0, 80], dtype=np.float64) colors = (low * (1 - zn)[:, None] + high * zn[:, None]).astype(np.uint8) From 39cf3833d7c5404e4afa190013b05b3fd4cd9db3 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 06:04:45 -0700 Subject: [PATCH 138/185] fastlio2: make host_ip/lidar_ip optional + env-driven (mirror pointlio) Drop the baked-in 192.168.1.5/192.168.1.155 defaults. host_ip/lidar_ip now default to DIMOS_FASTLIO_HOST_IP / DIMOS_FASTLIO_LIDAR_IP (None if unset), matching PointLio. lidar_ip is network-specific and required: _validate_network raises a clear error if it's unset; host_ip stays optional (auto-derived from lidar_ip's /24 via resolve_host_ip). Standalone mid360_fastlio* blueprints now require DIMOS_FASTLIO_LIDAR_IP (as mid360_pointlio requires DIMOS_POINTLIO_LIDAR_IP). The nav blueprints (mobile/alfred/g1) already pass lidar_ip explicitly, so they're unaffected. --- .../hardware/sensors/lidar/fastlio2/module.py | 24 ++++++++++++------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 895fdde8b1..b8445478a4 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -20,10 +20,12 @@ from __future__ import annotations +import os from pathlib import Path import time from typing import TYPE_CHECKING, Annotated +from pydantic import Field from pydantic.experimental.pipeline import validate_as from reactivex.disposable import Disposable @@ -58,9 +60,11 @@ class FastLio2Config(NativeModuleConfig): cwd: str | None = "cpp" executable: str = "result/bin/fastlio2_native" build_command: str | None = "nix build .#fastlio2_native" - # Livox SDK hardware config - host_ip: str = "192.168.1.5" - lidar_ip: str = "192.168.1.155" + # Livox SDK hardware config. lidar_ip required; host_ip optional (auto-derived + # from lidar_ip's subnet). Both fall back to DIMOS_FASTLIO_LIDAR_IP / + # DIMOS_FASTLIO_HOST_IP. + host_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_FASTLIO_HOST_IP")) + lidar_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_FASTLIO_LIDAR_IP")) frequency: float = 10.0 # "odom" frame: FastLio2 gives smooth continuous odometry; PGO publishes the @@ -149,11 +153,15 @@ def stop(self) -> None: super().stop() def _validate_network(self) -> None: - # Auto-derive host_ip from a local NIC on the lidar's subnet (shared with - # the Mid360 driver / Point-LIO) when the configured value isn't local. - self.config.host_ip = resolve_host_ip( - self.config.lidar_ip, self.config.host_ip, label="FastLio2" - ) + lidar_ip = self.config.lidar_ip + if not lidar_ip: + raise RuntimeError( + "FastLio2: lidar_ip not set — it's network-specific. Set it in the config " + "or via the DIMOS_FASTLIO_LIDAR_IP env var." + ) + # host_ip optional: derive the local NIC on lidar_ip's /24 when unset or + # not one of our IPs (shared with the Mid360 driver). + self.config.host_ip = resolve_host_ip(lidar_ip, self.config.host_ip, label="FastLio2") # Verify protocol port compliance (mypy will flag missing ports) From 3fb1a54d3c561808b57b705d94f806600b7af14c Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 06:10:40 -0700 Subject: [PATCH 139/185] feat(pointlio): recorder stamps lidar poses; pcap_to_db auto-stops by pcap span - recorder: stamp the latest odometry pose onto each pointlio_lidar frame (metadata only, nearest within 0.1s). pointlio_lidar now carries the trajectory, so 'dimos map global --lidar pointlio_lidar' registers it directly (no pose-fill pass). - pcap_to_db: default the stop bound to the pcap's own duration (_pcap_sensor_span) + margin. Point-LIO dead-reckons (publishes odom+lidar at full rate after the pcap drains), so stream-stagnation never fires; the span bound stops the run shortly after the data ends so the db/.rrd finalize. --- .../sensors/lidar/pointlio/recorder.py | 28 +++++++++++- .../lidar/pointlio/scripts/pcap_to_db.py | 43 ++++++++++++++++++- 2 files changed, 68 insertions(+), 3 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/recorder.py b/dimos/hardware/sensors/lidar/pointlio/recorder.py index 3088667a77..e1afbd502c 100644 --- a/dimos/hardware/sensors/lidar/pointlio/recorder.py +++ b/dimos/hardware/sensors/lidar/pointlio/recorder.py @@ -20,6 +20,10 @@ can be appended to an existing db (e.g. a fastlio replay) and compared on one timeline. Owns the db lifecycle: refuses to clobber existing streams unless ``force``, and derives the alignment reference from whatever the db already holds. + +Each lidar frame is stamped with the latest odometry pose, so ``pointlio_lidar`` +carries the trajectory and ``dimos map global`` can register it directly (it +transforms the body-frame cloud by that pose) — no ``dimos map pose-fill`` pass. """ from __future__ import annotations @@ -32,6 +36,7 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In +from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 @@ -39,6 +44,10 @@ _SENSOR_CLOCK_MAX = 1e8 # Strictly-increasing tie-breaker so two samples never collide on ts. _EPS = 1e-9 +# Max sensor-ts gap to attach the latest odometry pose to a lidar frame, so +# pointlio_lidar carries the trajectory and `dimos map global` can register it +# (it transforms by the per-frame pose). Matches pose_fill's nearest-match window. +_POSE_MATCH_TOL = 0.1 def _existing_min_ts(db_path: Path) -> float: @@ -93,6 +102,10 @@ class PointlioRecorder(Module): _last_lidar_ts: float = 0.0 _odom_count: int = 0 _lidar_count: int = 0 + # Latest odometry pose + its raw sensor ts, stamped onto each lidar frame so + # pointlio_lidar carries the trajectory (no separate pose-fill pass). + _last_odom_pose: Pose | None = None + _last_odom_raw_ts: float = 0.0 async def main(self) -> AsyncIterator[None]: # Deferred: the store is opened in the worker process that runs main(), @@ -145,6 +158,8 @@ async def handle_odometry(self, msg: Odometry) -> None: pose = getattr(msg, "pose", None) pose_inner = getattr(pose, "pose", None) if pose is not None else None self._os.append(msg, ts=ts, pose=pose_inner) + self._last_odom_pose = pose_inner + self._last_odom_raw_ts = raw_ts self._odom_count += 1 async def handle_lidar(self, msg: PointCloud2) -> None: @@ -152,5 +167,16 @@ async def handle_lidar(self, msg: PointCloud2) -> None: raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() ts = self._aligned_ts(raw_ts, self._last_lidar_ts) self._last_lidar_ts = ts - self._ls.append(msg, ts=ts) + # Stamp the latest odometry pose (within tolerance) onto the frame so + # pointlio_lidar carries the trajectory; map global transforms the + # body-frame cloud by it. Both Point-LIO outputs share a publish ts, so + # the nearest odometry is at most ~one odom period stale. Frames with no + # match (e.g. before the first odometry) get None and are map-skipped. + pose = ( + self._last_odom_pose + if self._last_odom_pose is not None + and abs(raw_ts - self._last_odom_raw_ts) <= _POSE_MATCH_TOL + else None + ) + self._ls.append(msg, ts=ts, pose=pose) self._lidar_count += 1 diff --git a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py index b0330c5ecf..a39678b549 100644 --- a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py @@ -61,6 +61,35 @@ _STARTUP_TIMEOUT_SEC = 60.0 # Max |Δts| to match a lidar frame to an odometry pose when aggregating the .rrd. _POSE_MATCH_TOL = 0.1 +# Extra seconds past the pcap's own duration before auto-stopping, when no +# explicit --max-sensor-sec is given. +_DRAIN_MARGIN_SEC = 4.0 + + +def _pcap_sensor_span(pcap_path: Path) -> float: + """Span (s) between the first and last packet of a classic little-endian pcap, + walking only record headers (seeking past payloads). 0.0 if not parseable — + the caller then falls back to stream-stagnation drain detection.""" + import struct + + try: + with open(pcap_path, "rb") as handle: + if handle.read(24)[:4] != b"\xd4\xc3\xb2\xa1": + return 0.0 + first: float | None = None + last = 0.0 + while True: + header = handle.read(16) + if len(header) < 16: + break + ts_sec, ts_usec, incl_len, _orig = struct.unpack(" tuple[int, float, float]: @@ -278,10 +307,20 @@ def _run(args: argparse.Namespace) -> int: print(f"[pcap_to_db] missing --config: {config_path}", file=sys.stderr) return 2 + # Default the stop bound to the pcap's own duration: Point-LIO keeps + # dead-reckoning (publishing at full rate) after the pcap drains, so the + # stream-stagnation check never fires on its own. Adding the real span makes + # the run stop shortly after the data ends. --max-sensor-sec overrides. + max_sensor_sec = args.max_sensor_sec + if max_sensor_sec <= 0: + span = _pcap_sensor_span(pcap_path) + if span > 0: + max_sensor_sec = span + _DRAIN_MARGIN_SEC + print( f"[pcap_to_db] pcap={pcap_path.name} db={db_path.name} " f"({'append' if db_path.exists() else 'new'}) rate={args.rate} " - f"ips={args.host_ip}/{args.lidar_ip}", + f"ips={args.host_ip}/{args.lidar_ip} stop_at={max_sensor_sec or 'drain'}", flush=True, ) @@ -289,7 +328,7 @@ def _run(args: argparse.Namespace) -> int: try: coord = ModuleCoordinator.build(_build_blueprint(args, db_path, config_path)) drained = _poll_until_drained( - db_path, args.odom_stream_name, args.lidar_stream_name, args.max_sensor_sec + db_path, args.odom_stream_name, args.lidar_stream_name, max_sensor_sec ) finally: if coord is not None: From 6ffaad40b0799791f01f80f5dc8fe7b910d8efae Mon Sep 17 00:00:00 2001 From: Ivan Nikolic Date: Wed, 17 Jun 2026 22:02:25 +0800 Subject: [PATCH 140/185] feat(map): add --carve/--no-carve toggle to map global, default off MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit VoxelGrid defaults to carve_columns=True (keep only the latest frame's points per X,Y column) — correct for live ground-robot mapping but wrong for offline reconstruction of body-frame/handheld recordings (e.g. Point-LIO's pointlio_lidar going down stairs), where revisiting columns from many heights collapses vertical structure to the most recent frame. map global now exposes --carve/--no-carve and defaults to --no-carve (full 3D accumulation), threaded through _accumulate into VoxelMapTransformer at all three rebuild sites. VoxelGrid's own default is unchanged, so live mapping is unaffected. --- dimos/mapping/utils/cli/map.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/dimos/mapping/utils/cli/map.py b/dimos/mapping/utils/cli/map.py index c9e23fbf7f..ce1d2220de 100644 --- a/dimos/mapping/utils/cli/map.py +++ b/dimos/mapping/utils/cli/map.py @@ -98,6 +98,7 @@ def _accumulate( device: str, graph: PoseGraph | None = None, world_frame: bool = False, + carve_columns: bool = False, progress_cb: Callable[[Observation[Any]], None] | None = None, ) -> PointCloud2 | None: """Accumulate a voxel map from `obs_iter`, optionally PGO-correcting each frame. @@ -151,6 +152,7 @@ def prepared() -> Iterable[Observation[PointCloud2]]: voxel_size=voxel, block_count=block_count, device=device, + carve_columns=carve_columns, ) result = next(iter(vmt(iter(prepared()))), None) return result.data if result is not None else None @@ -332,6 +334,13 @@ def main( help="Clouds are already world-registered (e.g. fastlio); skip applying the " "per-frame pose. Default registers each (body-frame) cloud by its pose.", ), + carve: bool = typer.Option( + False, + "--carve/--no-carve", + help="Column carving: keep only the latest frame's points per (X,Y) column. " + "Off by default (full 3D accumulation); on collapses vertical structure " + "(stairs, revisited columns) to the most recent observation.", + ), markers: bool = typer.Option( False, "--markers", @@ -462,6 +471,7 @@ def main( device=device, graph=graph, world_frame=go2, + carve_columns=carve, progress_cb=progress(n_kept, "pgo pass 2 (rebuilding)"), ) @@ -475,6 +485,7 @@ def main( device=device, graph=graph, world_frame=go2, + carve_columns=carve, progress_cb=progress(total, "full pgo (rebuilding)"), ) @@ -485,6 +496,7 @@ def main( block_count=block_count, device=device, world_frame=go2, + carve_columns=carve, progress_cb=progress(n_kept, "reconstructing global map"), ) From ec86efa9e55727d439a019521414f1b7f06438c0 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 22:17:49 +0800 Subject: [PATCH 141/185] Revert "fix pre-existing mypy 3.10 errors (Unpack import, numpy no-any-return)" This reverts commit 28b01f80721c29704175c9b9a13b907c7979a8f1. --- dimos/mapping/loop_closure/pgo.py | 3 +-- dimos/mapping/loop_closure/pgo_auto.py | 3 +-- dimos/robot/unitree/go2/dds/cdr.py | 2 +- dimos/simulation/engines/mujoco_shm.py | 9 +++------ 4 files changed, 6 insertions(+), 11 deletions(-) diff --git a/dimos/mapping/loop_closure/pgo.py b/dimos/mapping/loop_closure/pgo.py index 7adbba262f..b6c2b595c7 100644 --- a/dimos/mapping/loop_closure/pgo.py +++ b/dimos/mapping/loop_closure/pgo.py @@ -46,13 +46,12 @@ from collections.abc import Callable, Iterator from dataclasses import dataclass -from typing import TYPE_CHECKING, Any, Literal, TypedDict, TypeVar, cast +from typing import TYPE_CHECKING, Any, Literal, TypedDict, TypeVar, Unpack, cast import numpy as np import open3d as o3d # type: ignore[import-untyped] import open3d.core as o3c # type: ignore[import-untyped] from scipy.spatial.transform import Rotation, Slerp -from typing_extensions import Unpack # typing.Unpack is 3.11+ from dimos.memory2.transform import Transformer from dimos.memory2.type.observation import Observation diff --git a/dimos/mapping/loop_closure/pgo_auto.py b/dimos/mapping/loop_closure/pgo_auto.py index 0c81fffcfe..37dbb7d36b 100644 --- a/dimos/mapping/loop_closure/pgo_auto.py +++ b/dimos/mapping/loop_closure/pgo_auto.py @@ -56,13 +56,12 @@ from collections.abc import Callable, Iterable, Iterator from dataclasses import dataclass -from typing import TYPE_CHECKING, Any, TypedDict, TypeVar +from typing import TYPE_CHECKING, Any, TypedDict, TypeVar, Unpack import numpy as np import open3d as o3d # type: ignore[import-untyped] import open3d.core as o3c # type: ignore[import-untyped] from scipy.spatial.transform import Rotation -from typing_extensions import Unpack # typing.Unpack is 3.11+ from dimos.memory2.store.memory import MemoryStore from dimos.memory2.stream import Stream diff --git a/dimos/robot/unitree/go2/dds/cdr.py b/dimos/robot/unitree/go2/dds/cdr.py index 19fdfbb4b3..581086596c 100644 --- a/dimos/robot/unitree/go2/dds/cdr.py +++ b/dimos/robot/unitree/go2/dds/cdr.py @@ -94,7 +94,7 @@ def prim(self, code: str) -> Any: def prim_array(self, code: str, n: int) -> np.ndarray: _, sz, dt = _PRIM[code] self.align(sz) - a: np.ndarray = np.frombuffer(self.b, dt, n, self.p).copy() + a = np.frombuffer(self.b, dt, n, self.p).copy() self.p += sz * n return a diff --git a/dimos/simulation/engines/mujoco_shm.py b/dimos/simulation/engines/mujoco_shm.py index 2783cf672d..f424147d65 100644 --- a/dimos/simulation/engines/mujoco_shm.py +++ b/dimos/simulation/engines/mujoco_shm.py @@ -283,8 +283,7 @@ def read_kp_command(self, num_joints: int) -> NDArray[np.float64] | None: return None self._last_kp_cmd_seq = seq arr = self._array(self.shm.kp_t, MAX_JOINTS, np.float64) - result: NDArray[np.float64] = arr[:num_joints].copy() - return result + return arr[:num_joints].copy() def read_kd_command(self, num_joints: int) -> NDArray[np.float64] | None: seq = self._get_seq(SEQ_KD_CMD) @@ -292,8 +291,7 @@ def read_kd_command(self, num_joints: int) -> NDArray[np.float64] | None: return None self._last_kd_cmd_seq = seq arr = self._array(self.shm.kd_t, MAX_JOINTS, np.float64) - result: NDArray[np.float64] = arr[:num_joints].copy() - return result + return arr[:num_joints].copy() def read_tau_command(self, num_joints: int) -> NDArray[np.float64] | None: """Per-joint feedforward torque if a new command landed since last call.""" @@ -302,8 +300,7 @@ def read_tau_command(self, num_joints: int) -> NDArray[np.float64] | None: return None self._last_tau_cmd_seq = seq arr = self._array(self.shm.tau_t, MAX_JOINTS, np.float64) - result: NDArray[np.float64] = arr[:num_joints].copy() - return result + return arr[:num_joints].copy() def signal_ready(self, num_joints: int) -> None: ctrl = self._control() From 4b22007341778f9a824c45e9b99b0bcca3e6baef Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 07:18:02 -0700 Subject: [PATCH 142/185] Revert "fix pre-existing mypy 3.10 errors (Unpack import, numpy no-any-return)" This reverts commit 28b01f80721c29704175c9b9a13b907c7979a8f1. --- dimos/mapping/loop_closure/pgo.py | 3 +-- dimos/mapping/loop_closure/pgo_auto.py | 3 +-- dimos/robot/unitree/go2/dds/cdr.py | 2 +- dimos/simulation/engines/mujoco_shm.py | 9 +++------ 4 files changed, 6 insertions(+), 11 deletions(-) diff --git a/dimos/mapping/loop_closure/pgo.py b/dimos/mapping/loop_closure/pgo.py index 7adbba262f..b6c2b595c7 100644 --- a/dimos/mapping/loop_closure/pgo.py +++ b/dimos/mapping/loop_closure/pgo.py @@ -46,13 +46,12 @@ from collections.abc import Callable, Iterator from dataclasses import dataclass -from typing import TYPE_CHECKING, Any, Literal, TypedDict, TypeVar, cast +from typing import TYPE_CHECKING, Any, Literal, TypedDict, TypeVar, Unpack, cast import numpy as np import open3d as o3d # type: ignore[import-untyped] import open3d.core as o3c # type: ignore[import-untyped] from scipy.spatial.transform import Rotation, Slerp -from typing_extensions import Unpack # typing.Unpack is 3.11+ from dimos.memory2.transform import Transformer from dimos.memory2.type.observation import Observation diff --git a/dimos/mapping/loop_closure/pgo_auto.py b/dimos/mapping/loop_closure/pgo_auto.py index 0c81fffcfe..37dbb7d36b 100644 --- a/dimos/mapping/loop_closure/pgo_auto.py +++ b/dimos/mapping/loop_closure/pgo_auto.py @@ -56,13 +56,12 @@ from collections.abc import Callable, Iterable, Iterator from dataclasses import dataclass -from typing import TYPE_CHECKING, Any, TypedDict, TypeVar +from typing import TYPE_CHECKING, Any, TypedDict, TypeVar, Unpack import numpy as np import open3d as o3d # type: ignore[import-untyped] import open3d.core as o3c # type: ignore[import-untyped] from scipy.spatial.transform import Rotation -from typing_extensions import Unpack # typing.Unpack is 3.11+ from dimos.memory2.store.memory import MemoryStore from dimos.memory2.stream import Stream diff --git a/dimos/robot/unitree/go2/dds/cdr.py b/dimos/robot/unitree/go2/dds/cdr.py index 19fdfbb4b3..581086596c 100644 --- a/dimos/robot/unitree/go2/dds/cdr.py +++ b/dimos/robot/unitree/go2/dds/cdr.py @@ -94,7 +94,7 @@ def prim(self, code: str) -> Any: def prim_array(self, code: str, n: int) -> np.ndarray: _, sz, dt = _PRIM[code] self.align(sz) - a: np.ndarray = np.frombuffer(self.b, dt, n, self.p).copy() + a = np.frombuffer(self.b, dt, n, self.p).copy() self.p += sz * n return a diff --git a/dimos/simulation/engines/mujoco_shm.py b/dimos/simulation/engines/mujoco_shm.py index 2783cf672d..f424147d65 100644 --- a/dimos/simulation/engines/mujoco_shm.py +++ b/dimos/simulation/engines/mujoco_shm.py @@ -283,8 +283,7 @@ def read_kp_command(self, num_joints: int) -> NDArray[np.float64] | None: return None self._last_kp_cmd_seq = seq arr = self._array(self.shm.kp_t, MAX_JOINTS, np.float64) - result: NDArray[np.float64] = arr[:num_joints].copy() - return result + return arr[:num_joints].copy() def read_kd_command(self, num_joints: int) -> NDArray[np.float64] | None: seq = self._get_seq(SEQ_KD_CMD) @@ -292,8 +291,7 @@ def read_kd_command(self, num_joints: int) -> NDArray[np.float64] | None: return None self._last_kd_cmd_seq = seq arr = self._array(self.shm.kd_t, MAX_JOINTS, np.float64) - result: NDArray[np.float64] = arr[:num_joints].copy() - return result + return arr[:num_joints].copy() def read_tau_command(self, num_joints: int) -> NDArray[np.float64] | None: """Per-joint feedforward torque if a new command landed since last call.""" @@ -302,8 +300,7 @@ def read_tau_command(self, num_joints: int) -> NDArray[np.float64] | None: return None self._last_tau_cmd_seq = seq arr = self._array(self.shm.tau_t, MAX_JOINTS, np.float64) - result: NDArray[np.float64] = arr[:num_joints].copy() - return result + return arr[:num_joints].copy() def signal_ready(self, num_joints: int) -> None: ctrl = self._control() From 721a290fd89954ea8276ce3fec0bb9f36b0ff8f4 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 20:24:48 -0700 Subject: [PATCH 143/185] fastlio2: move config from YAML files to FastLio2Config (literal-union enums) Delete the checked-in config/*.yaml presets + cpp/config/mid360.json. All FAST-LIO tuning now lives as typed fields on FastLio2Config; start() renders them to a throwaway YAML passed as --config_path (C++ unchanged). lidar_type and timestamp_unit are literal-union strings translated to FAST-LIO's int codes. The three nav blueprints that used default.yaml now pass its tighter covariance / live-extrinsic / shorter-range overrides explicitly. --- dimos/control/blueprints/mobile.py | 12 +- .../sensors/lidar/fastlio2/config/avia.yaml | 35 ----- .../lidar/fastlio2/config/default.yaml | 33 ----- .../lidar/fastlio2/config/horizon.yaml | 35 ----- .../sensors/lidar/fastlio2/config/marsim.yaml | 35 ----- .../sensors/lidar/fastlio2/config/mid360.yaml | 40 ------ .../lidar/fastlio2/config/ouster64.yaml | 36 ----- .../lidar/fastlio2/config/velodyne.yaml | 37 ------ .../sensors/lidar/fastlio2/cpp/README.md | 8 +- .../lidar/fastlio2/cpp/config/mid360.json | 38 ------ .../sensors/lidar/fastlio2/cpp/main.cpp | 2 +- .../hardware/sensors/lidar/fastlio2/module.py | 125 +++++++++++++++--- .../robot/diy/alfred/blueprints/alfred_nav.py | 10 +- .../primitive/unitree_g1_onboard.py | 10 +- 14 files changed, 139 insertions(+), 317 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/fastlio2/config/avia.yaml delete mode 100644 dimos/hardware/sensors/lidar/fastlio2/config/default.yaml delete mode 100644 dimos/hardware/sensors/lidar/fastlio2/config/horizon.yaml delete mode 100644 dimos/hardware/sensors/lidar/fastlio2/config/marsim.yaml delete mode 100644 dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml delete mode 100644 dimos/hardware/sensors/lidar/fastlio2/config/ouster64.yaml delete mode 100644 dimos/hardware/sensors/lidar/fastlio2/config/velodyne.yaml delete mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/config/mid360.json diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py index 1e4f16b43a..58ab774d8e 100644 --- a/dimos/control/blueprints/mobile.py +++ b/dimos/control/blueprints/mobile.py @@ -34,8 +34,6 @@ from dimos.control.coordinator import ControlCoordinator, TaskConfig from dimos.core.coordination.blueprints import autoconnect from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.navigation.nav_stack.main import create_nav_stack, nav_stack_rerun_config from dimos.robot.catalog.ufactory import xarm7 as _catalog_xarm7 @@ -124,7 +122,15 @@ def _flowbase_twist_base( FastLio2.blueprint( host_ip=os.getenv("LIDAR_HOST_IP", "192.168.1.5"), lidar_ip=os.getenv("LIDAR_IP", "192.168.1.189"), - config="default.yaml", + # nav tuning (was config/default.yaml): tighter covariance, live + # extrinsic calibration, shorter range, default 0.5 m IESKF voxel. + acc_cov=0.01, + gyr_cov=0.01, + det_range=60.0, + extrinsic_est_en=True, + filter_size_surf=0.5, + filter_size_map=0.5, + pcd_save_en=False, ), create_nav_stack( planner="simple", diff --git a/dimos/hardware/sensors/lidar/fastlio2/config/avia.yaml b/dimos/hardware/sensors/lidar/fastlio2/config/avia.yaml deleted file mode 100644 index 8447b64658..0000000000 --- a/dimos/hardware/sensors/lidar/fastlio2/config/avia.yaml +++ /dev/null @@ -1,35 +0,0 @@ -common: - lid_topic: "/livox/lidar" - imu_topic: "/livox/imu" - time_sync_en: false # ONLY turn on when external time synchronization is really not possible - time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). - # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 - -preprocess: - lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, - scan_line: 6 - blind: 4 - -mapping: - acc_cov: 0.1 - gyr_cov: 0.1 - b_acc_cov: 0.0001 - b_gyr_cov: 0.0001 - fov_degree: 90 - det_range: 450.0 - extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic - extrinsic_T: [ 0.04165, 0.02326, -0.0284 ] - extrinsic_R: [ 1, 0, 0, - 0, 1, 0, - 0, 0, 1] - -publish: - path_en: false - scan_publish_en: true # false: close all the point cloud output - dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. - scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame - -pcd_save: - pcd_save_en: true - interval: -1 # how many LiDAR frames saved in each pcd file; - # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/fastlio2/config/default.yaml b/dimos/hardware/sensors/lidar/fastlio2/config/default.yaml deleted file mode 100644 index 688597d850..0000000000 --- a/dimos/hardware/sensors/lidar/fastlio2/config/default.yaml +++ /dev/null @@ -1,33 +0,0 @@ -common: - lid_topic: "/livox/lidar" - imu_topic: "/livox/imu" - time_sync_en: false - time_offset_lidar_to_imu: 0.0 - -preprocess: - lidar_type: 1 # 1 for Livox serials LiDAR - scan_line: 4 - blind: 0.5 # spherical min range (metres) - -mapping: - acc_cov: 0.01 # tighter than mid360 default (0.1) - gyr_cov: 0.01 # tighter than mid360 default (0.1) - b_acc_cov: 0.0001 - b_gyr_cov: 0.0001 - fov_degree: 360 - det_range: 60.0 # reduced from 100 — less noise from distant points - extrinsic_est_en: true # enable live calibration - extrinsic_T: [ -0.011, -0.02329, 0.04412 ] - extrinsic_R: [ 1, 0, 0, - 0, 1, 0, - 0, 0, 1] - -publish: - path_en: false - scan_publish_en: true - dense_publish_en: true - scan_bodyframe_pub_en: true - -pcd_save: - pcd_save_en: false - interval: -1 diff --git a/dimos/hardware/sensors/lidar/fastlio2/config/horizon.yaml b/dimos/hardware/sensors/lidar/fastlio2/config/horizon.yaml deleted file mode 100644 index 43db0c3bff..0000000000 --- a/dimos/hardware/sensors/lidar/fastlio2/config/horizon.yaml +++ /dev/null @@ -1,35 +0,0 @@ -common: - lid_topic: "/livox/lidar" - imu_topic: "/livox/imu" - time_sync_en: false # ONLY turn on when external time synchronization is really not possible - time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). - # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 - -preprocess: - lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, - scan_line: 6 - blind: 4 - -mapping: - acc_cov: 0.1 - gyr_cov: 0.1 - b_acc_cov: 0.0001 - b_gyr_cov: 0.0001 - fov_degree: 100 - det_range: 260.0 - extrinsic_est_en: true # true: enable the online estimation of IMU-LiDAR extrinsic - extrinsic_T: [ 0.05512, 0.02226, -0.0297 ] - extrinsic_R: [ 1, 0, 0, - 0, 1, 0, - 0, 0, 1] - -publish: - path_en: false - scan_publish_en: true # false: close all the point cloud output - dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. - scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame - -pcd_save: - pcd_save_en: true - interval: -1 # how many LiDAR frames saved in each pcd file; - # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/fastlio2/config/marsim.yaml b/dimos/hardware/sensors/lidar/fastlio2/config/marsim.yaml deleted file mode 100644 index ad6c89121a..0000000000 --- a/dimos/hardware/sensors/lidar/fastlio2/config/marsim.yaml +++ /dev/null @@ -1,35 +0,0 @@ -common: - lid_topic: "/quad0_pcl_render_node/sensor_cloud" - imu_topic: "/quad_0/imu" - time_sync_en: false # ONLY turn on when external time synchronization is really not possible - time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). - # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 - -preprocess: - lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, - scan_line: 4 - blind: 0.5 - -mapping: - acc_cov: 0.1 - gyr_cov: 0.1 - b_acc_cov: 0.0001 - b_gyr_cov: 0.0001 - fov_degree: 90 - det_range: 50.0 - extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic - extrinsic_T: [ -0.0, -0.0, 0.0 ] - extrinsic_R: [ 1, 0, 0, - 0, 1, 0, - 0, 0, 1] - -publish: - path_en: false - scan_publish_en: true # false: close all the point cloud output - dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. - scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame - -pcd_save: - pcd_save_en: true - interval: -1 # how many LiDAR frames saved in each pcd file; - # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml b/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml deleted file mode 100644 index b3cca62af9..0000000000 --- a/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml +++ /dev/null @@ -1,40 +0,0 @@ -common: - lid_topic: "/livox/lidar" - imu_topic: "/livox/imu" - time_sync_en: false # ONLY turn on when external time synchronization is really not possible - time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). - # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 - -preprocess: - lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, - scan_line: 4 - blind: 0.5 - -mapping: - # acc_cov down-weights the IMU accel prediction; upstream 0.1 lets Go2 odom - # diverge to km/s, 1.0 holds bounded. See jhist dimos-fastlio-velocity-spike.md. - acc_cov: 1.0 - gyr_cov: 0.1 - b_acc_cov: 0.0001 - b_gyr_cov: 0.0001 - # Scan voxel for the IESKF; does NOT affect divergence (acc_cov is the guard). - filter_size_surf: 0.1 - filter_size_map: 0.1 - fov_degree: 360 - det_range: 100.0 - extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic - extrinsic_T: [ -0.011, -0.02329, 0.04412 ] - extrinsic_R: [ 1, 0, 0, - 0, 1, 0, - 0, 0, 1] - -publish: - path_en: false - scan_publish_en: true # false: close all the point cloud output - dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. - scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame - -pcd_save: - pcd_save_en: true - interval: -1 # how many LiDAR frames saved in each pcd file; - # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/fastlio2/config/ouster64.yaml b/dimos/hardware/sensors/lidar/fastlio2/config/ouster64.yaml deleted file mode 100644 index 9d891bbeba..0000000000 --- a/dimos/hardware/sensors/lidar/fastlio2/config/ouster64.yaml +++ /dev/null @@ -1,36 +0,0 @@ -common: - lid_topic: "/os_cloud_node/points" - imu_topic: "/os_cloud_node/imu" - time_sync_en: false # ONLY turn on when external time synchronization is really not possible - time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). - # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 - -preprocess: - lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, - scan_line: 64 - timestamp_unit: 3 # 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. - blind: 4 - -mapping: - acc_cov: 0.1 - gyr_cov: 0.1 - b_acc_cov: 0.0001 - b_gyr_cov: 0.0001 - fov_degree: 180 - det_range: 150.0 - extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic - extrinsic_T: [ 0.0, 0.0, 0.0 ] - extrinsic_R: [1, 0, 0, - 0, 1, 0, - 0, 0, 1] - -publish: - path_en: false - scan_publish_en: true # false: close all the point cloud output - dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. - scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame - -pcd_save: - pcd_save_en: true - interval: -1 # how many LiDAR frames saved in each pcd file; - # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/fastlio2/config/velodyne.yaml b/dimos/hardware/sensors/lidar/fastlio2/config/velodyne.yaml deleted file mode 100644 index 450eda48b8..0000000000 --- a/dimos/hardware/sensors/lidar/fastlio2/config/velodyne.yaml +++ /dev/null @@ -1,37 +0,0 @@ -common: - lid_topic: "/velodyne_points" - imu_topic: "/imu/data" - time_sync_en: false # ONLY turn on when external time synchronization is really not possible - time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). - # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 - -preprocess: - lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, - scan_line: 32 - scan_rate: 10 # only need to be set for velodyne, unit: Hz, - timestamp_unit: 2 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. - blind: 2 - -mapping: - acc_cov: 0.1 - gyr_cov: 0.1 - b_acc_cov: 0.0001 - b_gyr_cov: 0.0001 - fov_degree: 180 - det_range: 100.0 - extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, - extrinsic_T: [ 0, 0, 0.28] - extrinsic_R: [ 1, 0, 0, - 0, 1, 0, - 0, 0, 1] - -publish: - path_en: false - scan_publish_en: true # false: close all the point cloud output - dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. - scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame - -pcd_save: - pcd_save_en: true - interval: -1 # how many LiDAR frames saved in each pcd file; - # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md b/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md index bbbfdf1929..0c3e9e858e 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md @@ -92,18 +92,18 @@ lcm-spy ## Configuration -FAST-LIO2 config files live in `config/`. The YAML config controls filter -parameters, EKF tuning, and point cloud processing settings. +There are no checked-in config files. FAST-LIO2 tuning (filter sizes, EKF +covariance, extrinsics, point-cloud processing) lives on `FastLio2Config` in +`../module.py`; on `start()` the module renders those fields to a throwaway YAML +and passes it as `--config_path`. ## File overview | File | Description | |---------------------------|--------------------------------------------------------------| | `main.cpp` | Livox SDK2 + FAST-LIO2 integration, EKF SLAM, LCM publishing | -| `cloud_filter.hpp` | Point cloud filtering (range, voxel downsampling) | | `voxel_map.hpp` | Global voxel map accumulation | | `dimos_native_module.hpp` | Reusable header for parsing NativeModule CLI args | -| `config/` | FAST-LIO2 YAML configuration files | | `flake.nix` | Nix flake for hermetic builds | | `CMakeLists.txt` | Build config, fetches dimos-lcm headers automatically | | `../module.py` | Python NativeModule wrapper (`FastLio2`) | diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/config/mid360.json b/dimos/hardware/sensors/lidar/fastlio2/cpp/config/mid360.json deleted file mode 100644 index ff6cc6dbf6..0000000000 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/config/mid360.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "common": { - "time_sync_en": false, - "time_offset_lidar_to_imu": 0.0, - "msr_freq": 50.0, - "main_freq": 5000.0 - }, - "preprocess": { - "lidar_type": 1, - "scan_line": 1, - "blind": 1 - }, - "mapping": { - "acc_cov": 0.1, - "gyr_cov": 0.1, - "b_acc_cov": 0.0001, - "b_gyr_cov": 0.0001, - "fov_degree": 360, - "det_range": 100.0, - "extrinsic_est_en": true, - "extrinsic_T": [ - 0.04165, - 0.02326, - -0.0284 - ], - "extrinsic_R": [ - 1.0, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - 0.0, - 1.0 - ] - } -} diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index 0771a005ff..b006e29513 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -11,7 +11,7 @@ // ./fastlio2_native \ // --lidar '/lidar#sensor_msgs.PointCloud2' \ // --odometry '/odometry#nav_msgs.Odometry' \ -// --config_path /path/to/mid360.yaml \ +// --config_path /path/to/fastlio.yaml \ # generated by module.py from FastLio2Config // --host_ip 192.168.1.5 --lidar_ip 192.168.1.155 #include diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index b8445478a4..b772c22e88 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -16,18 +16,23 @@ Binds Livox SDK2 into FAST-LIO-NON-ROS for real-time LiDAR SLAM; outputs registered (world-frame) point clouds and odometry with covariance. + +FAST-LIO tuning lives directly on ``FastLio2Config`` (no YAML files). On +``start()`` the fields are rendered to a throwaway YAML that the C++ binary +reads via ``--config_path``. """ from __future__ import annotations import os from pathlib import Path +import tempfile import time -from typing import TYPE_CHECKING, Annotated +from typing import TYPE_CHECKING, Literal from pydantic import Field -from pydantic.experimental.pipeline import validate_as from reactivex.disposable import Disposable +import yaml from dimos.core.core import rpc from dimos.core.native_module import NativeModule, NativeModuleConfig @@ -53,7 +58,43 @@ from dimos.navigation.nav_stack.frames import FRAME_BODY, FRAME_ODOM from dimos.spec import perception -_CONFIG_DIR = Path(__file__).parent / "config" +# FAST-LIO encodes these as ints/codes; expose human-readable names and translate. +LidarType = Literal["livox", "velodyne", "ouster"] +_LIDAR_TYPE_CODE = {"livox": 1, "velodyne": 2, "ouster": 3} + +TimestampUnit = Literal["second", "millisecond", "microsecond", "nanosecond"] +_TIMESTAMP_UNIT_CODE = {"second": 0, "millisecond": 1, "microsecond": 2, "nanosecond": 3} + +# Field name -> FAST-LIO YAML (section, key). Only these fields are rendered into +# the generated config; everything else on FastLio2Config is module plumbing. +_YAML_LAYOUT: dict[str, tuple[str, str]] = { + "lid_topic": ("common", "lid_topic"), + "imu_topic": ("common", "imu_topic"), + "time_sync_en": ("common", "time_sync_en"), + "time_offset_lidar_to_imu": ("common", "time_offset_lidar_to_imu"), + "lidar_type": ("preprocess", "lidar_type"), + "scan_line": ("preprocess", "scan_line"), + "scan_rate": ("preprocess", "scan_rate"), + "timestamp_unit": ("preprocess", "timestamp_unit"), + "blind": ("preprocess", "blind"), + "acc_cov": ("mapping", "acc_cov"), + "gyr_cov": ("mapping", "gyr_cov"), + "b_acc_cov": ("mapping", "b_acc_cov"), + "b_gyr_cov": ("mapping", "b_gyr_cov"), + "filter_size_surf": ("mapping", "filter_size_surf"), + "filter_size_map": ("mapping", "filter_size_map"), + "fov_degree": ("mapping", "fov_degree"), + "det_range": ("mapping", "det_range"), + "extrinsic_est_en": ("mapping", "extrinsic_est_en"), + "extrinsic_t": ("mapping", "extrinsic_T"), + "extrinsic_r": ("mapping", "extrinsic_R"), + "path_en": ("publish", "path_en"), + "scan_publish_en": ("publish", "scan_publish_en"), + "dense_publish_en": ("publish", "dense_publish_en"), + "scan_bodyframe_pub_en": ("publish", "scan_bodyframe_pub_en"), + "pcd_save_en": ("pcd_save", "pcd_save_en"), + "interval": ("pcd_save", "interval"), +} class FastLio2Config(NativeModuleConfig): @@ -80,14 +121,43 @@ class FastLio2Config(NativeModuleConfig): pointcloud_freq: float = 10.0 odom_freq: float = 30.0 - # FAST-LIO YAML config (relative to config/ dir, or absolute path) - # C++ binary reads YAML directly via yaml-cpp - config: Annotated[ - Path, validate_as(...).transform(lambda p: p if p.is_absolute() else _CONFIG_DIR / p) - ] = Path("mid360.yaml") - debug: bool = False + # --- FAST-LIO tuning (rendered to the generated YAML; see _YAML_LAYOUT) --- + # common + lid_topic: str = "/livox/lidar" + imu_topic: str = "/livox/imu" + time_sync_en: bool = False + time_offset_lidar_to_imu: float = 0.0 + # preprocess + lidar_type: LidarType = "livox" + scan_line: int = 4 + scan_rate: int = 10 # velodyne only + timestamp_unit: TimestampUnit = "microsecond" # velodyne/ouster time field unit + blind: float = 0.5 # spherical min range (m) + # mapping + # acc_cov down-weights the IMU accel prediction; upstream 0.1 lets Go2 odom + # diverge to km/s, 1.0 holds bounded. See jhist dimos-fastlio-velocity-spike.md. + acc_cov: float = 1.0 + gyr_cov: float = 0.1 + b_acc_cov: float = 0.0001 + b_gyr_cov: float = 0.0001 + filter_size_surf: float = 0.1 # IESKF scan voxel; does not affect divergence + filter_size_map: float = 0.1 + fov_degree: float = 360.0 + det_range: float = 100.0 + extrinsic_est_en: bool = False # online IMU-LiDAR extrinsic estimation + extrinsic_t: list[float] = Field(default_factory=lambda: [-0.011, -0.02329, 0.04412]) + extrinsic_r: list[float] = Field(default_factory=lambda: [1, 0, 0, 0, 1, 0, 0, 0, 1]) + # publish + path_en: bool = False + scan_publish_en: bool = True + dense_publish_en: bool = True + scan_bodyframe_pub_en: bool = True + # pcd_save + pcd_save_en: bool = True + interval: int = -1 + # SDK port configuration (see livox/ports.py for defaults) cmd_data_port: int = SDK_CMD_DATA_PORT push_msg_port: int = SDK_PUSH_MSG_PORT @@ -100,18 +170,23 @@ class FastLio2Config(NativeModuleConfig): host_imu_data_port: int = SDK_HOST_IMU_DATA_PORT host_log_data_port: int = SDK_HOST_LOG_DATA_PORT - # Resolved in __post_init__, passed as --config_path to the binary + # Set in start() to the generated YAML; passed as --config_path to the binary. config_path: str | None = None - cli_exclude: frozenset[str] = frozenset({"config"}) + # FAST-LIO tuning fields feed the generated YAML, not CLI args. + cli_exclude: frozenset[str] = frozenset(_YAML_LAYOUT) - def model_post_init(self, __context: object) -> None: - """Resolve config_path.""" - super().model_post_init(__context) - cfg = self.config - if not cfg.is_absolute(): - cfg = _CONFIG_DIR / cfg - self.config_path = str(cfg.resolve()) + def render_config_yaml(self) -> str: + """Render the FAST-LIO tuning fields to YAML text the C++ binary reads.""" + doc: dict[str, dict[str, object]] = {} + for field, (section, key) in _YAML_LAYOUT.items(): + val: object = getattr(self, field) + if field == "lidar_type": + val = _LIDAR_TYPE_CODE[val] # type: ignore[index] + elif field == "timestamp_unit": + val = _TIMESTAMP_UNIT_CODE[val] # type: ignore[index] + doc.setdefault(section, {})[key] = val + return yaml.safe_dump(doc, sort_keys=False, default_flow_style=False) class FastLio2(NativeModule, perception.Lidar, perception.Odometry): @@ -120,14 +195,25 @@ class FastLio2(NativeModule, perception.Lidar, perception.Odometry): lidar: Out[PointCloud2] odometry: Out[Odometry] + _config_file: str | None = None + @rpc def start(self) -> None: self._validate_network() + self._write_config() super().start() self.register_disposable( Disposable(self.odometry.transport.subscribe(self._on_odom_for_tf, self.odometry)) ) + def _write_config(self) -> None: + """Render the config fields to a temp YAML and point the binary at it.""" + fd, path = tempfile.mkstemp(prefix="fastlio2_", suffix=".yaml") + with os.fdopen(fd, "w") as f: + f.write(self.config.render_config_yaml()) + self._config_file = path + self.config.config_path = path + def _on_odom_for_tf(self, msg: Odometry) -> None: self.tf.publish( Transform( @@ -151,6 +237,9 @@ def _on_odom_for_tf(self, msg: Odometry) -> None: @rpc def stop(self) -> None: super().stop() + if self._config_file is not None: + Path(self._config_file).unlink(missing_ok=True) + self._config_file = None def _validate_network(self) -> None: lidar_ip = self.config.lidar_ip diff --git a/dimos/robot/diy/alfred/blueprints/alfred_nav.py b/dimos/robot/diy/alfred/blueprints/alfred_nav.py index 7bff17dfc7..31eed66e6e 100644 --- a/dimos/robot/diy/alfred/blueprints/alfred_nav.py +++ b/dimos/robot/diy/alfred/blueprints/alfred_nav.py @@ -54,7 +54,15 @@ FastLio2.blueprint( host_ip=os.getenv("LIDAR_HOST_IP", "192.168.1.5"), lidar_ip=os.getenv("LIDAR_IP", "192.168.1.189"), - config="default.yaml", + # nav tuning (was config/default.yaml): tighter covariance, live + # extrinsic calibration, shorter range, default 0.5 m IESKF voxel. + acc_cov=0.01, + gyr_cov=0.01, + det_range=60.0, + extrinsic_est_en=True, + filter_size_surf=0.5, + filter_size_map=0.5, + pcd_save_en=False, ), create_nav_stack(**nav_config), MovementManager.blueprint(), diff --git a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py index 1738841eaf..473930c376 100644 --- a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py +++ b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py @@ -27,7 +27,15 @@ FastLio2.blueprint( host_ip=os.getenv("LIDAR_HOST_IP", "192.168.123.164"), lidar_ip=os.getenv("LIDAR_IP", "192.168.123.120"), - config="default.yaml", + # nav tuning (was config/default.yaml): tighter covariance, live + # extrinsic calibration, shorter range, default 0.5 m IESKF voxel. + acc_cov=0.01, + gyr_cov=0.01, + det_range=60.0, + extrinsic_est_en=True, + filter_size_surf=0.5, + filter_size_map=0.5, + pcd_save_en=False, ), G1HighLevelDdsSdk.blueprint(), unitree_g1_vis, From e17e6b17eebd8a75e70767d26e9d47381467474a Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 20:25:10 -0700 Subject: [PATCH 144/185] fastlio2 pcap_to_db: mirror pointlio rrd + span auto-stop; get_data db fallback - Write a .rrd quick-look (aggregated lidar + pose path), mirroring pointlio; fastlio clouds are already world-registered so frames aggregate as-is (no per-frame pose transform). --no-rrd / --voxel knobs. - Auto-stop by the pcap's own packet span (+margin) so the run ends shortly after the data drains, mirroring pointlio. - A missing --db is fetched via get_data (LFS) before falling back to building from scratch. - --config now takes a YAML/JSON of FastLio2Config field overrides. --- .../lidar/fastlio2/tools/pcap_to_db.py | 214 +++++++++++++++--- 1 file changed, 182 insertions(+), 32 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index 8a3cc6b5f2..c17ae968fe 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -18,22 +18,21 @@ # LFS archive, NOT the archive name) PCAP_PATH="$(python -c "from dimos.utils.data import get_data; print(get_data('mid360_shake_stairs/mid360_shake_stairs.pcap'))")" - # gen .db from pcap with your config (defaults to .db next to the pcap) + # gen .db from pcap (defaults to .db next to the pcap) + python -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db --pcap "$PCAP_PATH" + + # override any FastLio2Config field via a small YAML/JSON doc, e.g. {acc_cov: 0.1} python -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db \ - --config dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml \ - --pcap "$PCAP_PATH" + --pcap "$PCAP_PATH" --config overrides.yaml - # add to existing .db + # add to existing .db (a missing --db is fetched via get_data before falling + # back to building from scratch) DB="mem2.db" python -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db --db "$DB" --pcap "$PCAP_PATH" - # generate map - dimos map summary "$DB" - dimos map pose-fill "$DB" \ - --target fastlio_lidar \ - --pose-source fastlio_odometry \ - --out "${DB%.db}_posed.db" - dimos map global "${DB%.db}_posed.db" --lidar fastlio_lidar + # A quick-look .rrd (aggregated world lidar + pose path) is written next + # to the db automatically. View it with: + rerun "${DB%.db}.rrd" One coordinator runs three autoconnected modules: a ``VirtualMid360`` replays the pcap over the Livox wire (aliasing the host/lidar IPs onto a dummy interface on @@ -50,7 +49,7 @@ import sqlite3 import sys import time -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, Any if TYPE_CHECKING: from dimos.core.coordination.blueprints import Blueprint @@ -63,6 +62,35 @@ # artifact, bad pcap, SLAM-init crash); bounds the poll loop. Generous to cover # FAST-LIO's IMU-init latency. _STARTUP_TIMEOUT_SEC = 60.0 +# Extra seconds past the pcap's own duration before auto-stopping, when no +# explicit --max-sensor-sec is given. +_DRAIN_MARGIN_SEC = 4.0 + + +def _pcap_sensor_span(pcap_path: Path) -> float: + """Span (s) between the first and last packet of a classic little-endian pcap, + walking only record headers (seeking past payloads). 0.0 if not parseable — + the caller then falls back to stream-stagnation drain detection.""" + import struct + + try: + with open(pcap_path, "rb") as handle: + if handle.read(24)[:4] != b"\xd4\xc3\xb2\xa1": + return 0.0 + first: float | None = None + last = 0.0 + while True: + header = handle.read(16) + if len(header) < 16: + break + ts_sec, ts_usec, incl_len, _orig = struct.unpack(" tuple[int, float, float]: @@ -80,7 +108,74 @@ def _odom_stats(db_path: Path, table: str) -> tuple[int, float, float]: con.close() -def _build_blueprint(args: argparse.Namespace, db_path: Path, config_path: str) -> Blueprint: +def _write_rrd(db_path: Path, odom_stream: str, lidar_stream: str, voxel: float) -> Path | None: + """Aggregate the recorded lidar plus the pose path into a ``.rrd`` next to the + db, for a quick look. + + FastLio2 already publishes its cloud registered into the world frame, so each + frame is aggregated as-is (no per-frame pose transform) then voxel-deduped. + Best-effort: any failure is non-fatal to the recording. Returns the .rrd path, + or None.""" + import numpy as np + import rerun as rr + + from dimos.memory2.store.sqlite import SqliteStore + from dimos.msgs.nav_msgs.Odometry import Odometry + from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + from dimos.visualization.rerun.init import rerun_init + + store = SqliteStore(path=str(db_path), must_exist=True) + try: + odom = list(store.stream(odom_stream, Odometry).order_by("ts")) + if not odom: + return None + opos = np.array( + [ + [ + o.data.pose.pose.position.x, + o.data.pose.pose.position.y, + o.data.pose.pose.position.z, + ] + for o in odom + ] + ) + chunks: list[Any] = [] + for lid in store.stream(lidar_stream, PointCloud2).order_by("ts"): + pts = np.asarray(lid.data.as_numpy()[0])[:, :3].astype(np.float64) + if pts.shape[0] == 0: + continue + # Per-frame voxel-dedup to bound memory before the global merge. + _, idx = np.unique(np.floor(pts / voxel).astype(np.int64), axis=0, return_index=True) + chunks.append(pts[idx]) + if not chunks: + return None + allpts = np.concatenate(chunks) + _, idx = np.unique(np.floor(allpts / voxel).astype(np.int64), axis=0, return_index=True) + agg = allpts[idx].astype(np.float32) + + # Height gradient: hot pink (low) -> dark purple (high). + z = agg[:, 2] + zn = (z - z.min()) / ((z.max() - z.min()) + 1e-9) + low = np.array([255, 20, 147], dtype=np.float64) + high = np.array([60, 0, 80], dtype=np.float64) + colors = (low * (1 - zn)[:, None] + high * zn[:, None]).astype(np.uint8) + + rrd = db_path.with_suffix(".rrd") + rerun_init("pcap_to_db") + rr.save(str(rrd)) + rr.log("world/map", rr.Points3D(positions=agg, colors=colors, radii=[voxel / 8])) + rr.log( + "world/path", + rr.LineStrips3D(strips=[opos.astype(np.float32)], colors=[[231, 76, 60]], radii=[0.05]), + ) + return rrd + finally: + store.stop() + + +def _build_blueprint( + args: argparse.Namespace, db_path: Path, overrides: dict[str, Any] +) -> Blueprint: """autoconnect(VirtualMid360 + FastLio2 + FastLio2Recorder). FastLio2's ``odometry``/``lidar`` outputs auto-wire to the recorder's @@ -92,13 +187,10 @@ def _build_blueprint(args: argparse.Namespace, db_path: Path, config_path: str) from dimos.hardware.sensors.lidar.fastlio2.recorder import FastLio2Recorder from dimos.hardware.sensors.lidar.virtual_mid360.module import VirtualMid360 - # `config` is already an absolute path so it bypasses the config-dir-relative - # resolution. Omit when empty to keep the default mid360.yaml. - fastlio_kwargs: dict[str, object] = dict( + fastlio_kwargs: dict[str, Any] = dict( host_ip=args.host_ip, lidar_ip=args.lidar_ip, odom_freq=args.odom_freq, debug=False ) - if config_path: - fastlio_kwargs["config"] = config_path + fastlio_kwargs.update(overrides) return autoconnect( VirtualMid360.blueprint( @@ -169,6 +261,45 @@ def _poll_until_drained( stagnant_since = None +def _resolve_db_path(args: argparse.Namespace, pcap_path: Path) -> Path: + """Where to record. Omitted --db -> .db. A given --db that's missing is + fetched via get_data (LFS) before falling back to building from scratch.""" + if not args.db: + return pcap_path.with_suffix(".db") + db_path = Path(args.db).expanduser().resolve() + if not db_path.exists(): + try: + from dimos.utils.data import get_data + + fetched = get_data(args.db) + if fetched.exists(): + print(f"[pcap_to_db] fetched --db via get_data: {fetched}", flush=True) + return fetched.resolve() + except Exception as exc: # not an LFS-known db -> build from scratch + print( + f"[pcap_to_db] --db not found locally or via get_data ({exc}); " + "building from scratch", + file=sys.stderr, + flush=True, + ) + return db_path + + +def _load_overrides(config: str) -> dict[str, Any]: + """Load a YAML/JSON doc of FastLio2Config field overrides, e.g. {acc_cov: 0.1}.""" + if not config: + return {} + import yaml + + path = Path(config).expanduser().resolve() + if not path.exists(): + raise FileNotFoundError(f"--config not found: {path}") + data = yaml.safe_load(path.read_text()) or {} + if not isinstance(data, dict): + raise ValueError(f"--config must be a mapping of FastLio2Config fields, got {type(data)}") + return data + + def _run(args: argparse.Namespace) -> int: from dimos.core.coordination.module_coordinator import ModuleCoordinator @@ -177,28 +308,32 @@ def _run(args: argparse.Namespace) -> int: print(f"[pcap_to_db] missing pcap: {pcap_path}", file=sys.stderr) return 2 args.pcap_path = pcap_path - db_path = Path(args.db).expanduser().resolve() if args.db else pcap_path.with_suffix(".db") + db_path = _resolve_db_path(args, pcap_path) db_path.parent.mkdir(parents=True, exist_ok=True) - # Resolve --config against the *invoking* cwd (pwd-relative) up front; the - # FastLio2 config field otherwise resolves a relative path against its own - # config/ dir, never the pwd. Absolute path passes through unchanged. - config_path = str(Path(args.config).expanduser().resolve()) if args.config else "" - if config_path and not Path(config_path).exists(): - print(f"[pcap_to_db] missing --config: {config_path}", file=sys.stderr) - return 2 + overrides = _load_overrides(args.config) + + # Default the stop bound to the pcap's own duration: FAST-LIO keeps + # dead-reckoning (publishing at full rate) after the pcap drains, so the + # stream-stagnation check never fires on its own. Adding the real span makes + # the run stop shortly after the data ends. --max-sensor-sec overrides. + max_sensor_sec = args.max_sensor_sec + if max_sensor_sec <= 0: + span = _pcap_sensor_span(pcap_path) + if span > 0: + max_sensor_sec = span + _DRAIN_MARGIN_SEC print( f"[pcap_to_db] pcap={pcap_path.name} db={db_path.name} " f"({'append' if db_path.exists() else 'new'}) rate={args.rate} " - f"ips={args.host_ip}/{args.lidar_ip}", + f"ips={args.host_ip}/{args.lidar_ip} stop_at={max_sensor_sec or 'drain'}", flush=True, ) coord = None try: - coord = ModuleCoordinator.build(_build_blueprint(args, db_path, config_path)) + coord = ModuleCoordinator.build(_build_blueprint(args, db_path, overrides)) drained = _poll_until_drained( - db_path, args.odom_stream_name, args.lidar_stream_name, args.max_sensor_sec + db_path, args.odom_stream_name, args.lidar_stream_name, max_sensor_sec ) finally: if coord is not None: @@ -212,6 +347,13 @@ def _run(args: argparse.Namespace) -> int: f"[pcap_to_db] done odom={o_cnt} ts=[{o_min:.3f}, {o_max:.3f}] span={o_max - o_min:.1f}s", flush=True, ) + if not args.no_rrd: + try: + rrd = _write_rrd(db_path, args.odom_stream_name, args.lidar_stream_name, args.voxel) + if rrd is not None: + print(f"[pcap_to_db] wrote {rrd.name} (aggregated lidar + pose path)", flush=True) + except Exception as exc: # viz is a non-fatal bonus + print(f"[pcap_to_db] .rrd generation skipped ({exc})", file=sys.stderr, flush=True) return 0 @@ -221,8 +363,8 @@ def main(argv: list[str]) -> int: parser.add_argument( "--db", default=None, - help="target memory2 SQLite db. Existing -> append/align; missing -> built from " - "scratch. Omit to default to .db next to the pcap.", + help="target memory2 SQLite db. Existing -> append/align; missing -> fetched via " + "get_data (LFS), else built from scratch. Omit to default to .db.", ) parser.add_argument( "--rate", type=float, default=1.0, help="replay-speed multiplier (default 1.0)" @@ -243,6 +385,14 @@ def main(argv: list[str]) -> int: help="seconds added to every output ts (auto if omitted)", ) parser.add_argument("--force", action="store_true", help="overwrite existing fastlio streams") + parser.add_argument( + "--no-rrd", + action="store_true", + help="skip writing the .rrd quick-look (aggregated world lidar + pose path)", + ) + parser.add_argument( + "--voxel", type=float, default=0.2, help="voxel size (m) for the .rrd aggregated map" + ) parser.add_argument( "--warmup-sec", type=float, @@ -252,7 +402,7 @@ def main(argv: list[str]) -> int: parser.add_argument( "--config", default="", - help="FAST-LIO YAML config (pwd-relative or absolute; default: module's mid360.yaml)", + help="YAML/JSON doc of FastLio2Config field overrides (e.g. {acc_cov: 0.1})", ) parser.add_argument( "--odom-stream-name", From 9faea3b18c896a945c9cee22363cb8493b55c0ff Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 20:34:45 -0700 Subject: [PATCH 145/185] fastlio2/pointlio: recorders inherit memory2 Recorder, override pose hook Both LIO recorders were bespoke Modules; they now subclass memory2.Recorder. Add small reusable hooks to the base Recorder (backward-compatible defaults): _resolve_pose (the pose-setting method subclasses override), _resolve_ts, _stream_name (rename a port's db stream), _prepare_streams, and an APPEND on_existing mode (keep the db, replace only your streams). The recorders override _resolve_pose to stamp each lidar frame with the latest odometry pose, plus _resolve_ts (db-clock alignment), _prepare_streams (per-stream force) and _stream_name (fastlio_/pointlio_ names). --- .../sensors/lidar/fastlio2/recorder.py | 120 +++++++++------- .../sensors/lidar/pointlio/recorder.py | 133 ++++++++---------- dimos/memory2/module.py | 51 +++++-- 3 files changed, 168 insertions(+), 136 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/recorder.py b/dimos/hardware/sensors/lidar/fastlio2/recorder.py index b530a7b744..f67d97daee 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/recorder.py +++ b/dimos/hardware/sensors/lidar/fastlio2/recorder.py @@ -14,24 +14,27 @@ """Record FAST-LIO odometry + lidar into a memory2 SQLite db. -Subscribes to a FastLio2's ``odometry`` / ``lidar`` outputs (auto-connected by -matching stream name + type — no remappings needed) and appends them to a -memory2 store. Timestamps are converted onto the db's existing clock so a run -can be appended to an existing db and compared on one timeline. Owns the db -lifecycle: refuses to clobber existing streams unless ``force``, and derives the -alignment reference from whatever the db already holds. +A memory2 ``Recorder`` whose ``odometry`` / ``lidar`` In ports auto-connect to a +FastLio2's same-named outputs. Beyond the base recorder it: records under +configurable stream names, re-bases timestamps onto the db's existing clock so a +run can be appended and compared on one timeline, replaces only its own streams +when appending (``force``), and sets poses from the odometry stream rather than +tf — each lidar frame is stamped with the latest odometry pose, so +``fastlio_lidar`` carries the trajectory and ``dimos map global`` can register it +directly. """ from __future__ import annotations -from collections.abc import AsyncIterator import math from pathlib import Path import sqlite3 import time +from typing import Any -from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In +from dimos.memory2.module import OnExisting, Recorder, RecorderConfig +from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 @@ -39,6 +42,9 @@ _SENSOR_CLOCK_MAX = 1e8 # Strictly-increasing tie-breaker so two samples never collide on ts. _EPS = 1e-9 +# Max sensor-ts gap to attach the latest odometry pose to a lidar frame. Matches +# pose_fill's nearest-match window; odometry is ~30 Hz so this nearly always hits. +_POSE_MATCH_TOL = 0.1 def _existing_min_ts(db_path: Path) -> float: @@ -69,10 +75,9 @@ def _existing_min_ts(db_path: Path) -> float: con.close() -class FastLio2RecorderConfig(ModuleConfig): - """Configures the recorder with the target db and timing conversion.""" +class FastLio2RecorderConfig(RecorderConfig): + """Target db + timing conversion for the FAST-LIO recorder.""" - db_path: str = "" # db stream/table names the FastLio2 outputs are recorded under. odom_stream_name: str = "fastlio_odometry" lidar_stream_name: str = "fastlio_lidar" @@ -80,44 +85,45 @@ class FastLio2RecorderConfig(ModuleConfig): time_offset: float = float("nan") # Drop pre-existing odom/lidar streams instead of refusing to overwrite. force: bool = False + # Append into a populated db (keep other streams); replace only our two. + on_existing: OnExisting = OnExisting.APPEND -class FastLio2Recorder(Module): +class FastLio2Recorder(Recorder): config: FastLio2RecorderConfig - lidar: In[PointCloud2] odometry: In[Odometry] + lidar: In[PointCloud2] + _offset: float | None = None _ref_start_ts: float = -1.0 _last_odom_ts: float = 0.0 _last_lidar_ts: float = 0.0 - _last_pose: object = None - _odom_count: int = 0 - _lidar_count: int = 0 - - async def main(self) -> AsyncIterator[None]: - # Deferred: the store is opened in the worker process that runs main(), - # not at module-scan/import time on the host. - from dimos.memory2.store.sqlite import SqliteStore - + # Latest odometry pose + its raw sensor ts, stamped onto each lidar frame. + _last_odom_pose: Pose | None = None + _last_odom_raw_ts: float = 0.0 + + def _stream_name(self, port_name: str) -> str: + if port_name == "odometry": + return self.config.odom_stream_name + if port_name == "lidar": + return self.config.lidar_stream_name + return port_name + + def _prepare_streams(self) -> None: cfg = self.config - self._store = SqliteStore(path=cfg.db_path) names = (cfg.odom_stream_name, cfg.lidar_stream_name) - existing = sorted(set(self._store.list_streams()) & set(names)) + existing = sorted(set(self.store.list_streams()) & set(names)) if existing and not cfg.force: raise RuntimeError( f"FastLio2Recorder: {Path(cfg.db_path).name} already has {existing}; " "set force=True to overwrite" ) for name in existing: - self._store.delete_stream(name) + self.store.delete_stream(name) # Reference is the db's earliest ts *after* dropping our own old streams, # so only the data we're aligning against counts. self._ref_start_ts = _existing_min_ts(Path(cfg.db_path)) - self._os = self._store.stream(cfg.odom_stream_name, Odometry) - self._ls = self._store.stream(cfg.lidar_stream_name, PointCloud2) - yield - self._store.stop() def _resolve_offset(self, first_ts: float) -> float: override = self.config.time_offset @@ -130,28 +136,38 @@ def _resolve_offset(self, first_ts: float) -> float: # db on wall-clock time -> start-align FastLio2 onto the db's earliest ts. return ref - first_ts - def _aligned_ts(self, raw_ts: float, last_ts: float) -> float: - """Convert a sensor ts onto the db clock, kept strictly above last_ts.""" - if self._offset is None: - self._offset = self._resolve_offset(raw_ts) - return max(raw_ts + self._offset, last_ts + _EPS) - - async def handle_odometry(self, msg: Odometry) -> None: + def _resolve_ts(self, name: str, msg: Any) -> float: # `is not None`, not `or`: a real sensor ts of 0.0 must not fall back to # wall time (would misclassify the stream's clock in _resolve_offset). - raw_ts_raw = getattr(msg, "ts", None) - raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() - ts = self._aligned_ts(raw_ts, self._last_odom_ts) - self._last_odom_ts = ts - pose = getattr(msg, "pose", None) - self._last_pose = getattr(pose, "pose", None) if pose is not None else None - self._os.append(msg, ts=ts, pose=self._last_pose) - self._odom_count += 1 - - async def handle_lidar(self, msg: PointCloud2) -> None: - raw_ts_raw = getattr(msg, "ts", None) - raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() - ts = self._aligned_ts(raw_ts, self._last_lidar_ts) - self._last_lidar_ts = ts - self._ls.append(msg, ts=ts, pose=self._last_pose) - self._lidar_count += 1 + raw = getattr(msg, "ts", None) + raw = raw if raw is not None else time.time() + if self._offset is None: + self._offset = self._resolve_offset(raw) + last = self._last_odom_ts if name == "odometry" else self._last_lidar_ts + ts = max(raw + self._offset, last + _EPS) + if name == "odometry": + self._last_odom_ts = ts + else: + self._last_lidar_ts = ts + return ts + + def _resolve_pose(self, name: str, msg: Any, ts: float) -> Pose | None: + if name == "odometry": + pose = getattr(msg, "pose", None) + inner = getattr(pose, "pose", None) if pose is not None else None + self._last_odom_pose = inner + raw = getattr(msg, "ts", None) + self._last_odom_raw_ts = raw if raw is not None else 0.0 + return inner + # lidar: stamp the latest odometry pose when it's recent enough. Both + # FastLio2 outputs share a publish ts, so the nearest odometry is at most + # ~one odom period stale. Frames with no match (e.g. before the first + # odometry) get None and are map-skipped. + raw = getattr(msg, "ts", None) + raw = raw if raw is not None else 0.0 + if ( + self._last_odom_pose is not None + and abs(raw - self._last_odom_raw_ts) <= _POSE_MATCH_TOL + ): + return self._last_odom_pose + return None diff --git a/dimos/hardware/sensors/lidar/pointlio/recorder.py b/dimos/hardware/sensors/lidar/pointlio/recorder.py index e1afbd502c..e141728252 100644 --- a/dimos/hardware/sensors/lidar/pointlio/recorder.py +++ b/dimos/hardware/sensors/lidar/pointlio/recorder.py @@ -14,28 +14,26 @@ """Record Point-LIO odometry + lidar into a memory2 SQLite db. -Subscribes to a PointLio's ``odometry`` / ``lidar`` outputs (auto-connected by -matching stream name + type — no remappings needed) and appends them to a -memory2 store. Timestamps are converted onto the db's existing clock so a run -can be appended to an existing db (e.g. a fastlio replay) and compared on one -timeline. Owns the db lifecycle: refuses to clobber existing streams unless -``force``, and derives the alignment reference from whatever the db already holds. - -Each lidar frame is stamped with the latest odometry pose, so ``pointlio_lidar`` -carries the trajectory and ``dimos map global`` can register it directly (it -transforms the body-frame cloud by that pose) — no ``dimos map pose-fill`` pass. +A memory2 ``Recorder`` whose ``odometry`` / ``lidar`` In ports auto-connect to a +PointLio's same-named outputs. Beyond the base recorder it: records under +configurable stream names, re-bases timestamps onto the db's existing clock so a +run can be appended (e.g. onto a fastlio replay) and compared on one timeline, +replaces only its own streams when appending (``force``), and sets poses from the +odometry stream rather than tf — each lidar frame is stamped with the latest +odometry pose, so ``pointlio_lidar`` carries the trajectory and ``dimos map +global`` can register the body-frame cloud directly (no ``pose-fill`` pass). """ from __future__ import annotations -from collections.abc import AsyncIterator import math from pathlib import Path import sqlite3 import time +from typing import Any -from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In +from dimos.memory2.module import OnExisting, Recorder, RecorderConfig from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 @@ -44,9 +42,8 @@ _SENSOR_CLOCK_MAX = 1e8 # Strictly-increasing tie-breaker so two samples never collide on ts. _EPS = 1e-9 -# Max sensor-ts gap to attach the latest odometry pose to a lidar frame, so -# pointlio_lidar carries the trajectory and `dimos map global` can register it -# (it transforms by the per-frame pose). Matches pose_fill's nearest-match window. +# Max sensor-ts gap to attach the latest odometry pose to a lidar frame. Matches +# pose_fill's nearest-match window; odometry is ~30 Hz so this nearly always hits. _POSE_MATCH_TOL = 0.1 @@ -78,10 +75,9 @@ def _existing_min_ts(db_path: Path) -> float: con.close() -class PointlioRecorderConfig(ModuleConfig): - """Configures the recorder with the target db and timing conversion.""" +class PointlioRecorderConfig(RecorderConfig): + """Target db + timing conversion for the Point-LIO recorder.""" - db_path: str = "" # db stream/table names the Point-LIO outputs are recorded under. odom_stream_name: str = "pointlio_odometry" lidar_stream_name: str = "pointlio_lidar" @@ -89,47 +85,45 @@ class PointlioRecorderConfig(ModuleConfig): time_offset: float = float("nan") # Drop pre-existing odom/lidar streams instead of refusing to overwrite. force: bool = False + # Append into a populated db (keep other streams); replace only our two. + on_existing: OnExisting = OnExisting.APPEND -class PointlioRecorder(Module): +class PointlioRecorder(Recorder): config: PointlioRecorderConfig - lidar: In[PointCloud2] odometry: In[Odometry] + lidar: In[PointCloud2] + _offset: float | None = None _ref_start_ts: float = -1.0 _last_odom_ts: float = 0.0 _last_lidar_ts: float = 0.0 - _odom_count: int = 0 - _lidar_count: int = 0 - # Latest odometry pose + its raw sensor ts, stamped onto each lidar frame so - # pointlio_lidar carries the trajectory (no separate pose-fill pass). + # Latest odometry pose + its raw sensor ts, stamped onto each lidar frame. _last_odom_pose: Pose | None = None _last_odom_raw_ts: float = 0.0 - async def main(self) -> AsyncIterator[None]: - # Deferred: the store is opened in the worker process that runs main(), - # not at module-scan/import time on the host. - from dimos.memory2.store.sqlite import SqliteStore + def _stream_name(self, port_name: str) -> str: + if port_name == "odometry": + return self.config.odom_stream_name + if port_name == "lidar": + return self.config.lidar_stream_name + return port_name + def _prepare_streams(self) -> None: cfg = self.config - self._store = SqliteStore(path=cfg.db_path) names = (cfg.odom_stream_name, cfg.lidar_stream_name) - existing = sorted(set(self._store.list_streams()) & set(names)) + existing = sorted(set(self.store.list_streams()) & set(names)) if existing and not cfg.force: raise RuntimeError( f"PointlioRecorder: {Path(cfg.db_path).name} already has {existing}; " "set force=True to overwrite" ) for name in existing: - self._store.delete_stream(name) + self.store.delete_stream(name) # Reference is the db's earliest ts *after* dropping our own old streams, # so only the data we're aligning against (e.g. a fastlio replay) counts. self._ref_start_ts = _existing_min_ts(Path(cfg.db_path)) - self._os = self._store.stream(cfg.odom_stream_name, Odometry) - self._ls = self._store.stream(cfg.lidar_stream_name, PointCloud2) - yield - self._store.stop() def _resolve_offset(self, first_ts: float) -> float: override = self.config.time_offset @@ -142,41 +136,38 @@ def _resolve_offset(self, first_ts: float) -> float: # db on wall-clock time -> start-align Point-LIO onto the db's earliest ts. return ref - first_ts - def _aligned_ts(self, raw_ts: float, last_ts: float) -> float: - """Convert a sensor ts onto the db clock, kept strictly above last_ts.""" - if self._offset is None: - self._offset = self._resolve_offset(raw_ts) - return max(raw_ts + self._offset, last_ts + _EPS) - - async def handle_odometry(self, msg: Odometry) -> None: + def _resolve_ts(self, name: str, msg: Any) -> float: # `is not None`, not `or`: a real sensor ts of 0.0 must not fall back to # wall time (would misclassify the stream's clock in _resolve_offset). - raw_ts_raw = getattr(msg, "ts", None) - raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() - ts = self._aligned_ts(raw_ts, self._last_odom_ts) - self._last_odom_ts = ts - pose = getattr(msg, "pose", None) - pose_inner = getattr(pose, "pose", None) if pose is not None else None - self._os.append(msg, ts=ts, pose=pose_inner) - self._last_odom_pose = pose_inner - self._last_odom_raw_ts = raw_ts - self._odom_count += 1 - - async def handle_lidar(self, msg: PointCloud2) -> None: - raw_ts_raw = getattr(msg, "ts", None) - raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() - ts = self._aligned_ts(raw_ts, self._last_lidar_ts) - self._last_lidar_ts = ts - # Stamp the latest odometry pose (within tolerance) onto the frame so - # pointlio_lidar carries the trajectory; map global transforms the - # body-frame cloud by it. Both Point-LIO outputs share a publish ts, so - # the nearest odometry is at most ~one odom period stale. Frames with no - # match (e.g. before the first odometry) get None and are map-skipped. - pose = ( - self._last_odom_pose - if self._last_odom_pose is not None - and abs(raw_ts - self._last_odom_raw_ts) <= _POSE_MATCH_TOL - else None - ) - self._ls.append(msg, ts=ts, pose=pose) - self._lidar_count += 1 + raw = getattr(msg, "ts", None) + raw = raw if raw is not None else time.time() + if self._offset is None: + self._offset = self._resolve_offset(raw) + last = self._last_odom_ts if name == "odometry" else self._last_lidar_ts + ts = max(raw + self._offset, last + _EPS) + if name == "odometry": + self._last_odom_ts = ts + else: + self._last_lidar_ts = ts + return ts + + def _resolve_pose(self, name: str, msg: Any, ts: float) -> Pose | None: + if name == "odometry": + pose = getattr(msg, "pose", None) + inner = getattr(pose, "pose", None) if pose is not None else None + self._last_odom_pose = inner + raw = getattr(msg, "ts", None) + self._last_odom_raw_ts = raw if raw is not None else 0.0 + return inner + # lidar: stamp the latest odometry pose when it's recent enough. Both + # Point-LIO outputs share a publish ts, so the nearest odometry is at most + # ~one odom period stale. Frames with no match (e.g. before the first + # odometry) get None and are map-skipped. + raw = getattr(msg, "ts", None) + raw = raw if raw is not None else 0.0 + if ( + self._last_odom_pose is not None + and abs(raw - self._last_odom_raw_ts) <= _POSE_MATCH_TOL + ): + return self._last_odom_pose + return None diff --git a/dimos/memory2/module.py b/dimos/memory2/module.py index ac4bd35f24..87feff1658 100644 --- a/dimos/memory2/module.py +++ b/dimos/memory2/module.py @@ -44,6 +44,7 @@ from reactivex.abc import DisposableBase from dimos.core.stream import In, Out + from dimos.msgs.geometry_msgs.Pose import Pose logger = setup_logger() @@ -251,6 +252,10 @@ class OnExisting(str, enum.Enum): OVERWRITE = "overwrite" ERROR = "error" BACKUP = "backup" + # Leave the db untouched (keep all existing streams); subclasses that append + # into a populated db handle their own per-stream replacement in + # ``_prepare_streams``. + APPEND = "append" class RecorderConfig(MemoryModuleConfig): @@ -291,7 +296,9 @@ def start(self) -> None: # this module in a deployed blueprint. db_path = Path(self.config.db_path) if db_path.exists(): - if self.config.on_existing is OnExisting.OVERWRITE: + if self.config.on_existing is OnExisting.APPEND: + pass # keep the db; _prepare_streams handles any per-stream replacement + elif self.config.on_existing is OnExisting.OVERWRITE: db_path.unlink() logger.info("Deleted existing recording %s", db_path) elif self.config.on_existing is OnExisting.BACKUP: @@ -303,14 +310,18 @@ def start(self) -> None: else: raise FileExistsError(f"Recording already exists: {db_path}") + self._prepare_streams() + if not self.inputs: logger.warning("Recorder has no In ports — nothing to record, subclass the Recorder") return for name, port in self.inputs.items(): - stream: Stream[Any] = self.store.stream(name, port.type) + stream: Stream[Any] = self.store.stream(self._stream_name(name), port.type) self._port_to_stream(name, port, stream) - logger.info("Recording %s (%s)", name, port.type.__name__) + logger.info( + "Recording %s -> %s (%s)", name, self._stream_name(name), port.type.__name__ + ) def _port_to_stream(self, name: str, input_topic: In[Any], stream: Stream[Any]) -> None: """Append each message from *input_topic* to *stream*, attaching world pose via tf. @@ -323,23 +334,37 @@ def _port_to_stream(self, name: str, input_topic: In[Any], stream: Stream[Any]) Registers the subscription as a disposable on this module. """ - default_frame_id = self.config.default_frame_id - tf_tolerance = self.config.tf_tolerance - def on_msg(msg: Any) -> None: - ts = getattr(msg, "ts", None) or time.time() - frame_id = getattr(msg, "frame_id", None) or default_frame_id - transform = self.tf.get("world", frame_id, time_point=ts, time_tolerance=tf_tolerance) - pose = transform.to_pose() if transform is not None else None - + ts = self._resolve_ts(name, msg) + pose = self._resolve_pose(name, msg, ts) if not pose: logger.warning( - "[%s] No tf available for frame '%s' at time %s (msg ts: %s), storing without pose", + "[%s] No pose for time %s (msg ts: %s), storing without pose", name, - frame_id, ts, getattr(msg, "ts", None), ) stream.append(msg, ts=ts, pose=pose) self.register_disposable(Disposable(input_topic.subscribe(on_msg))) + + def _stream_name(self, port_name: str) -> str: + """db stream/table name to record *port_name* under. Override to rename.""" + return port_name + + def _prepare_streams(self) -> None: + """Hook run after the on_existing check, before ports are wired. Override + to replace specific streams when appending into a populated db.""" + + def _resolve_ts(self, name: str, msg: Any) -> float: + """Timestamp to record *msg* at. Override to re-base onto another clock.""" + return getattr(msg, "ts", None) or time.time() + + def _resolve_pose(self, name: str, msg: Any, ts: float) -> Pose | None: + """Pose to anchor *msg* with. Default: world<-frame_id via tf. Override to + source poses elsewhere (e.g. from an odometry stream).""" + frame_id = getattr(msg, "frame_id", None) or self.config.default_frame_id + transform = self.tf.get( + "world", frame_id, time_point=ts, time_tolerance=self.config.tf_tolerance + ) + return transform.to_pose() if transform is not None else None From c995b0ab840b044c4aa850ce7b2b066766877bc9 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 20:40:36 -0700 Subject: [PATCH 146/185] fastlio2: fov_degree is an int in the generated YAML FAST-LIO's yaml-cpp reads mapping/fov_degree as int; emitting 360.0 threw TypedBadConversion and aborted the binary. Caught by an end-to-end VM replay (acc_cov=1.0 -> bounded 4.8 m over 25 s, rrd written). --- dimos/hardware/sensors/lidar/fastlio2/module.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index b772c22e88..90262fc48f 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -144,7 +144,7 @@ class FastLio2Config(NativeModuleConfig): b_gyr_cov: float = 0.0001 filter_size_surf: float = 0.1 # IESKF scan voxel; does not affect divergence filter_size_map: float = 0.1 - fov_degree: float = 360.0 + fov_degree: int = 360 # FAST-LIO reads this as an int det_range: float = 100.0 extrinsic_est_en: bool = False # online IMU-LiDAR extrinsic estimation extrinsic_t: list[float] = Field(default_factory=lambda: [-0.011, -0.02329, 0.04412]) From f2aeab4ed601148bc6209b45e9b152be6ab5066f Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 21:12:29 -0700 Subject: [PATCH 147/185] pointlio: move config from default.yaml to PointLioConfig (literal-union enums) Mirror the fastlio migration: delete pointlio/config/default.yaml. All Point-LIO tuning is now typed fields on PointLioConfig; start() renders them to a throwaway YAML passed as --config_path (C++ unchanged). lidar_type / timestamp_unit / ivox_nearby_type are literal-union strings translated to Point-LIO's int codes; every field keeps the exact YAML type (verified: generated == default.yaml). pcap_to_db --config now takes a dict of PointLioConfig field overrides. Also fix stale mid360.yaml/default.yaml mentions in the cpp READMEs + native_modules doc. --- .../sensors/lidar/fastlio2/cpp/README.md | 2 +- .../lidar/pointlio/config/default.yaml | 69 ------- .../sensors/lidar/pointlio/cpp/main.cpp | 2 +- .../hardware/sensors/lidar/pointlio/module.py | 177 ++++++++++++++++-- .../lidar/pointlio/scripts/pcap_to_db.py | 47 +++-- docs/usage/native_modules.md | 10 +- 6 files changed, 195 insertions(+), 112 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/pointlio/config/default.yaml diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md b/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md index 0c3e9e858e..7b8d46120c 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md @@ -74,7 +74,7 @@ autoconnect( --odometry '/odometry#nav_msgs.Odometry' \ --host_ip 192.168.1.5 \ --lidar_ip 192.168.1.155 \ - --config_path ../config/mid360.yaml + --config_path /path/to/fastlio.yaml # FAST-LIO tuning; module.py generates this from FastLio2Config ``` Topic strings must include the `#type` suffix -- this is the actual LCM channel diff --git a/dimos/hardware/sensors/lidar/pointlio/config/default.yaml b/dimos/hardware/sensors/lidar/pointlio/config/default.yaml deleted file mode 100644 index 1fd09ec9fa..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/config/default.yaml +++ /dev/null @@ -1,69 +0,0 @@ -# Point-LIO config. Mid-360-specific values to retune for a different sensor: -# preprocess.lidar_type (Livox), blind/scan_line, mapping.extrinsic_T/R (Mid-360 -# IMU->lidar mount), det_range, fov_degree. -common: - con_frame: false - con_frame_num: 1 - cut_frame: false - cut_frame_time_interval: 0.1 - time_lag_imu_to_lidar: 0.0 - -preprocess: - # LID_TYPE enum (Point-LIO src/preprocess.h): - # 1 = AVIA (Livox), 2 = VELO16, 3 = OUST64, 4 = HESAIxt32, 5 = UNILIDAR - # 1 selects the Livox branch (preprocess.cpp avia_handler), which expects the - # Livox CustomMsg point layout the Mid-360 emits: - # https://github.com/Livox-SDK/livox_ros_driver2/blob/master/msg/CustomMsg.msg - lidar_type: 1 - scan_line: 4 - scan_rate: 10 - timestamp_unit: 3 # 3 = nanosecond - blind: 0.5 - # Pre-KF input decimation: keep every Nth raw point. 1 = keep all (disable). - point_filter_num: 3 - -mapping: - use_imu_as_input: false # false = IMU-as-output model (Point-LIO's robust path) - prop_at_freq_of_imu: true - check_satu: true - init_map_size: 10 - # Pre-KF voxel downsample of each scan before the filter. false = feed the - # full scan (disable). Leaf size is filter_size_surf below. - space_down_sample: true - satu_acc: 3.0 # g; accel >= this is treated as saturated (residual zeroed) to bound velocity - satu_gyro: 35.0 - acc_norm: 1.0 # IMU accel unit: g - plane_thr: 0.1 - filter_size_surf: 0.2 # pre-KF scan downsample leaf size (m), used iff space_down_sample - filter_size_map: 0.5 - ivox_grid_resolution: 2.0 # iVox local-map grid (m) - ivox_nearby_type: 6 # NEARBY6 - cube_side_length: 1000.0 - det_range: 100.0 - fov_degree: 360.0 - imu_en: true - start_in_aggressive_motion: false - extrinsic_est_en: false - imu_time_inte: 0.005 - lidar_meas_cov: 0.01 - acc_cov_input: 0.1 - vel_cov: 20.0 - gyr_cov_input: 0.01 - gyr_cov_output: 1000.0 - acc_cov_output: 500.0 - b_gyr_cov: 0.0001 - b_acc_cov: 0.0001 - imu_meas_acc_cov: 0.01 - imu_meas_omg_cov: 0.01 - match_s: 81.0 - gravity_align: true - gravity: [0.0, 0.0, -9.810] - gravity_init: [0.0, 0.0, -9.810] - extrinsic_T: [-0.011, -0.02329, 0.04412] # Mid-360 IMU->lidar offset (m) - extrinsic_R: [1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0] - -odometry: - publish_odometry_without_downsample: false - odom_only: false diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 72e7a25094..8caef55333 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -11,7 +11,7 @@ // ./pointlio_native \ // --lidar '/lidar#sensor_msgs.PointCloud2' \ // --odometry '/odometry#nav_msgs.Odometry' \ -// --config_path /path/to/default.yaml \ +// --config_path /path/to/pointlio.yaml \ # generated by module.py from PointLioConfig // --host_ip 192.168.1.5 --lidar_ip 192.168.1.155 #include diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index 86cac95413..eef14e080a 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -24,17 +24,22 @@ PointLio.blueprint(host_ip="192.168.1.5", lidar_ip="192.168.1.155"), SomeConsumer.blueprint(), )).loop() + +Point-LIO tuning lives directly on ``PointLioConfig`` (no YAML files). On +``start()`` the fields are rendered to a throwaway YAML that the C++ binary reads +via ``--config_path``. """ from __future__ import annotations import os from pathlib import Path -from typing import TYPE_CHECKING, Annotated +import tempfile +from typing import TYPE_CHECKING, Literal from pydantic import Field -from pydantic.experimental.pipeline import validate_as from reactivex.disposable import Disposable +import yaml from dimos.core.core import rpc from dimos.core.native_module import NativeModule, NativeModuleConfig @@ -60,7 +65,72 @@ from dimos.navigation.nav_stack.frames import FRAME_ODOM from dimos.spec import perception -_CONFIG_DIR = Path(__file__).parent / "config" +# Point-LIO encodes these as ints/codes; expose human-readable names and translate. +# LID_TYPE enum (Point-LIO src/preprocess.h). 1 = AVIA selects the Livox branch +# the Mid-360 emits. +LidarType = Literal["avia", "velodyne", "ouster", "hesai", "unilidar"] +_LIDAR_TYPE_CODE = {"avia": 1, "velodyne": 2, "ouster": 3, "hesai": 4, "unilidar": 5} + +TimestampUnit = Literal["second", "millisecond", "microsecond", "nanosecond"] +_TIMESTAMP_UNIT_CODE = {"second": 0, "millisecond": 1, "microsecond": 2, "nanosecond": 3} + +# iVox local-map neighbour stencil. +IvoxNearbyType = Literal["center", "nearby6", "nearby18", "nearby26"] +_IVOX_NEARBY_CODE = {"center": 0, "nearby6": 6, "nearby18": 18, "nearby26": 26} + +# Field name -> Point-LIO YAML (section, key). Only these fields are rendered into +# the generated config; everything else on PointLioConfig is module plumbing. +_YAML_LAYOUT: dict[str, tuple[str, str]] = { + "con_frame": ("common", "con_frame"), + "con_frame_num": ("common", "con_frame_num"), + "cut_frame": ("common", "cut_frame"), + "cut_frame_time_interval": ("common", "cut_frame_time_interval"), + "time_lag_imu_to_lidar": ("common", "time_lag_imu_to_lidar"), + "lidar_type": ("preprocess", "lidar_type"), + "scan_line": ("preprocess", "scan_line"), + "scan_rate": ("preprocess", "scan_rate"), + "timestamp_unit": ("preprocess", "timestamp_unit"), + "blind": ("preprocess", "blind"), + "point_filter_num": ("preprocess", "point_filter_num"), + "use_imu_as_input": ("mapping", "use_imu_as_input"), + "prop_at_freq_of_imu": ("mapping", "prop_at_freq_of_imu"), + "check_satu": ("mapping", "check_satu"), + "init_map_size": ("mapping", "init_map_size"), + "space_down_sample": ("mapping", "space_down_sample"), + "satu_acc": ("mapping", "satu_acc"), + "satu_gyro": ("mapping", "satu_gyro"), + "acc_norm": ("mapping", "acc_norm"), + "plane_thr": ("mapping", "plane_thr"), + "filter_size_surf": ("mapping", "filter_size_surf"), + "filter_size_map": ("mapping", "filter_size_map"), + "ivox_grid_resolution": ("mapping", "ivox_grid_resolution"), + "ivox_nearby_type": ("mapping", "ivox_nearby_type"), + "cube_side_length": ("mapping", "cube_side_length"), + "det_range": ("mapping", "det_range"), + "fov_degree": ("mapping", "fov_degree"), + "imu_en": ("mapping", "imu_en"), + "start_in_aggressive_motion": ("mapping", "start_in_aggressive_motion"), + "extrinsic_est_en": ("mapping", "extrinsic_est_en"), + "imu_time_inte": ("mapping", "imu_time_inte"), + "lidar_meas_cov": ("mapping", "lidar_meas_cov"), + "acc_cov_input": ("mapping", "acc_cov_input"), + "vel_cov": ("mapping", "vel_cov"), + "gyr_cov_input": ("mapping", "gyr_cov_input"), + "gyr_cov_output": ("mapping", "gyr_cov_output"), + "acc_cov_output": ("mapping", "acc_cov_output"), + "b_gyr_cov": ("mapping", "b_gyr_cov"), + "b_acc_cov": ("mapping", "b_acc_cov"), + "imu_meas_acc_cov": ("mapping", "imu_meas_acc_cov"), + "imu_meas_omg_cov": ("mapping", "imu_meas_omg_cov"), + "match_s": ("mapping", "match_s"), + "gravity_align": ("mapping", "gravity_align"), + "gravity": ("mapping", "gravity"), + "gravity_init": ("mapping", "gravity_init"), + "extrinsic_t": ("mapping", "extrinsic_T"), + "extrinsic_r": ("mapping", "extrinsic_R"), + "publish_odometry_without_downsample": ("odometry", "publish_odometry_without_downsample"), + "odom_only": ("odometry", "odom_only"), +} class PointLioConfig(NativeModuleConfig): @@ -86,14 +156,65 @@ class PointLioConfig(NativeModuleConfig): pointcloud_freq: float = 10.0 odom_freq: float = 30.0 - # Point-LIO YAML config (relative to config/ dir, or absolute path). - config: Annotated[ - Path, - validate_as(...).transform(lambda path: path if path.is_absolute() else _CONFIG_DIR / path), - ] = Path("default.yaml") - debug: bool = False + # --- Point-LIO tuning (rendered to the generated YAML; see _YAML_LAYOUT) --- + # common + con_frame: bool = False + con_frame_num: int = 1 + cut_frame: bool = False + cut_frame_time_interval: float = 0.1 + time_lag_imu_to_lidar: float = 0.0 + # preprocess + lidar_type: LidarType = "avia" # 1 = AVIA (Livox) branch the Mid-360 emits + scan_line: int = 4 + scan_rate: int = 10 + timestamp_unit: TimestampUnit = "nanosecond" + blind: float = 0.5 # spherical min range (m) + point_filter_num: int = 3 # pre-KF decimation: keep every Nth raw point (1 = all) + # mapping + use_imu_as_input: bool = False # false = IMU-as-output model (robust path) + prop_at_freq_of_imu: bool = True + check_satu: bool = True + init_map_size: int = 10 + space_down_sample: bool = True # pre-KF voxel downsample (leaf = filter_size_surf) + satu_acc: float = 3.0 # g; accel >= this is treated as saturated, bounding velocity + satu_gyro: float = 35.0 + acc_norm: float = 1.0 # IMU accel unit: g + plane_thr: float = 0.1 + filter_size_surf: float = 0.2 # pre-KF scan downsample leaf (m), iff space_down_sample + filter_size_map: float = 0.5 + ivox_grid_resolution: float = 2.0 # iVox local-map grid (m) + ivox_nearby_type: IvoxNearbyType = "nearby6" + cube_side_length: float = 1000.0 + det_range: float = 100.0 + fov_degree: float = 360.0 + imu_en: bool = True + start_in_aggressive_motion: bool = False + extrinsic_est_en: bool = False + imu_time_inte: float = 0.005 + lidar_meas_cov: float = 0.01 + acc_cov_input: float = 0.1 + vel_cov: float = 20.0 + gyr_cov_input: float = 0.01 + gyr_cov_output: float = 1000.0 + acc_cov_output: float = 500.0 + b_gyr_cov: float = 0.0001 + b_acc_cov: float = 0.0001 + imu_meas_acc_cov: float = 0.01 + imu_meas_omg_cov: float = 0.01 + match_s: float = 81.0 + gravity_align: bool = True + gravity: list[float] = Field(default_factory=lambda: [0.0, 0.0, -9.81]) + gravity_init: list[float] = Field(default_factory=lambda: [0.0, 0.0, -9.81]) + extrinsic_t: list[float] = Field(default_factory=lambda: [-0.011, -0.02329, 0.04412]) + extrinsic_r: list[float] = Field( + default_factory=lambda: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + ) + # odometry + publish_odometry_without_downsample: bool = False + odom_only: bool = False + # SDK port configuration (see livox/ports.py for defaults) cmd_data_port: int = SDK_CMD_DATA_PORT push_msg_port: int = SDK_PUSH_MSG_PORT @@ -106,17 +227,25 @@ class PointLioConfig(NativeModuleConfig): host_imu_data_port: int = SDK_HOST_IMU_DATA_PORT host_log_data_port: int = SDK_HOST_LOG_DATA_PORT - # Resolved in __post_init__, passed as --config_path to the binary + # Set in start() to the generated YAML; passed as --config_path to the binary. config_path: str | None = None - cli_exclude: frozenset[str] = frozenset({"config", "body_start_frame_id"}) + # Point-LIO tuning fields feed the generated YAML, not CLI args. + cli_exclude: frozenset[str] = frozenset({"body_start_frame_id", *_YAML_LAYOUT}) - def model_post_init(self, __context: object) -> None: - super().model_post_init(__context) - cfg = self.config - if not cfg.is_absolute(): - cfg = _CONFIG_DIR / cfg - self.config_path = str(cfg.resolve()) + def render_config_yaml(self) -> str: + """Render the Point-LIO tuning fields to YAML text the C++ binary reads.""" + doc: dict[str, dict[str, object]] = {} + for field, (section, key) in _YAML_LAYOUT.items(): + val: object = getattr(self, field) + if field == "lidar_type": + val = _LIDAR_TYPE_CODE[val] # type: ignore[index] + elif field == "timestamp_unit": + val = _TIMESTAMP_UNIT_CODE[val] # type: ignore[index] + elif field == "ivox_nearby_type": + val = _IVOX_NEARBY_CODE[val] # type: ignore[index] + doc.setdefault(section, {})[key] = val + return yaml.safe_dump(doc, sort_keys=False, default_flow_style=False) class PointLio(NativeModule, perception.Lidar, perception.Odometry): @@ -125,14 +254,25 @@ class PointLio(NativeModule, perception.Lidar, perception.Odometry): lidar: Out[PointCloud2] odometry: Out[Odometry] + _config_file: str | None = None + @rpc def start(self) -> None: self._validate_network() + self._write_config() super().start() self.register_disposable( Disposable(self.odometry.transport.subscribe(self._on_odom_for_tf, self.odometry)) ) + def _write_config(self) -> None: + """Render the config fields to a temp YAML and point the binary at it.""" + fd, path = tempfile.mkstemp(prefix="pointlio_", suffix=".yaml") + with os.fdopen(fd, "w") as f: + f.write(self.config.render_config_yaml()) + self._config_file = path + self.config.config_path = path + def _on_odom_for_tf(self, msg: Odometry) -> None: self.tf.publish( Transform( @@ -158,6 +298,9 @@ def _on_odom_for_tf(self, msg: Odometry) -> None: @rpc def stop(self) -> None: super().stop() + if self._config_file is not None: + Path(self._config_file).unlink(missing_ok=True) + self._config_file = None def _validate_network(self) -> None: lidar_ip = self.config.lidar_ip diff --git a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py index a39678b549..ed84b4fe75 100644 --- a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py @@ -18,10 +18,12 @@ # LFS archive, NOT the archive name) PCAP_PATH="$(python -c "from dimos.utils.data import get_data; print(get_data('mid360_shake_stairs/mid360_shake_stairs.pcap'))")" - # gen .db from pcap with your config (defaults to .db next to the pcap) + # gen .db from pcap (defaults to .db next to the pcap) + python -m dimos.hardware.sensors.lidar.pointlio.scripts.pcap_to_db --pcap "$PCAP_PATH" + + # override any PointLioConfig field via a small YAML/JSON doc, e.g. {acc_cov_input: 0.3} python -m dimos.hardware.sensors.lidar.pointlio.scripts.pcap_to_db \ - --config dimos/hardware/sensors/lidar/pointlio/config/default.yaml \ - --pcap "$PCAP_PATH" + --pcap "$PCAP_PATH" --config overrides.yaml # add to existing .db DB="mem2.db" @@ -199,7 +201,9 @@ def _write_rrd(db_path: Path, odom_stream: str, lidar_stream: str, voxel: float) store.stop() -def _build_blueprint(args: argparse.Namespace, db_path: Path, config_path: str) -> Blueprint: +def _build_blueprint( + args: argparse.Namespace, db_path: Path, overrides: dict[str, Any] +) -> Blueprint: """autoconnect(VirtualMid360 + PointLio + PointlioRecorder). PointLio's ``odometry``/``lidar`` outputs auto-wire to the recorder's @@ -211,14 +215,10 @@ def _build_blueprint(args: argparse.Namespace, db_path: Path, config_path: str) from dimos.hardware.sensors.lidar.pointlio.recorder import PointlioRecorder from dimos.hardware.sensors.lidar.virtual_mid360.module import VirtualMid360 - # `config` (not `config_path`, which PointLioConfig derives itself); already - # an absolute path so it bypasses the config-dir-relative resolution. Omit - # when empty to keep the default.yaml. - pointlio_kwargs: dict[str, object] = dict( + pointlio_kwargs: dict[str, Any] = dict( host_ip=args.host_ip, lidar_ip=args.lidar_ip, odom_freq=args.odom_freq, debug=False ) - if config_path: - pointlio_kwargs["config"] = config_path + pointlio_kwargs.update(overrides) return autoconnect( VirtualMid360.blueprint( @@ -289,6 +289,21 @@ def _poll_until_drained( stagnant_since = None +def _load_overrides(config: str) -> dict[str, Any]: + """Load a YAML/JSON doc of PointLioConfig field overrides, e.g. {acc_cov_input: 0.3}.""" + if not config: + return {} + import yaml + + path = Path(config).expanduser().resolve() + if not path.exists(): + raise FileNotFoundError(f"--config not found: {path}") + data = yaml.safe_load(path.read_text()) or {} + if not isinstance(data, dict): + raise ValueError(f"--config must be a mapping of PointLioConfig fields, got {type(data)}") + return data + + def _run(args: argparse.Namespace) -> int: from dimos.core.coordination.module_coordinator import ModuleCoordinator @@ -299,13 +314,7 @@ def _run(args: argparse.Namespace) -> int: args.pcap_path = pcap_path db_path = Path(args.db).expanduser().resolve() if args.db else pcap_path.with_suffix(".db") db_path.parent.mkdir(parents=True, exist_ok=True) - # Resolve --config against the *invoking* cwd (pwd-relative) up front; the - # PointLio config field otherwise resolves a relative path against its own - # config/ dir, never the pwd. Absolute path passes through unchanged. - config_path = str(Path(args.config).expanduser().resolve()) if args.config else "" - if config_path and not Path(config_path).exists(): - print(f"[pcap_to_db] missing --config: {config_path}", file=sys.stderr) - return 2 + overrides = _load_overrides(args.config) # Default the stop bound to the pcap's own duration: Point-LIO keeps # dead-reckoning (publishing at full rate) after the pcap drains, so the @@ -326,7 +335,7 @@ def _run(args: argparse.Namespace) -> int: coord = None try: - coord = ModuleCoordinator.build(_build_blueprint(args, db_path, config_path)) + coord = ModuleCoordinator.build(_build_blueprint(args, db_path, overrides)) drained = _poll_until_drained( db_path, args.odom_stream_name, args.lidar_stream_name, max_sensor_sec ) @@ -397,7 +406,7 @@ def main(argv: list[str]) -> int: parser.add_argument( "--config", default="", - help="Point-LIO YAML config (pwd-relative or absolute; default: module's default.yaml)", + help="YAML/JSON doc of PointLioConfig field overrides (e.g. {acc_cov_input: 0.3})", ) parser.add_argument( "--odom-stream-name", diff --git a/docs/usage/native_modules.md b/docs/usage/native_modules.md index 3121ba8bf4..bb31cdd456 100644 --- a/docs/usage/native_modules.md +++ b/docs/usage/native_modules.md @@ -116,11 +116,11 @@ class MyConfig(NativeModuleConfig): If a config field shouldn't be a CLI arg, add it to `cli_exclude`: ```python skip -class FastLio2Config(NativeModuleConfig): - executable: str = "./build/fastlio2" - config: str = "mid360.yaml" # human-friendly name - config_path: str = Field(default_factory=lambda m: str(Path(m["config"]).resolve())) - cli_exclude: frozenset[str] = frozenset({"config"}) # only config_path is passed +class MyNativeConfig(NativeModuleConfig): + executable: str = "./build/my_native" + acc_cov: float = 1.0 # rendered into a config file, not a CLI arg + config_path: str | None = None # set at start() to the generated file + cli_exclude: frozenset[str] = frozenset({"acc_cov"}) # only config_path is passed ``` ## Using with blueprints From 9b38016539336201e98c62d43dee00edd8bdabe3 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 21:23:07 -0700 Subject: [PATCH 148/185] memory2: add TfRecorder; fastlio/pointlio recorders inherit it via @pose_setter_for Add memory2/tf_recorder.py: a Recorder that also records the live tf stream and lets subclasses register per-stream pose setters with @pose_setter_for("stream") (dispatched from the base _resolve_pose hook; default falls back to the tf lookup). FastLio2Recorder and PointlioRecorder now inherit TfRecorder and set poses straight from the odometry stream: the lidar frame takes the most-recent odometry pose directly (no tf/static-transform composition). Keeps the db-clock alignment, per-stream force, and stream renaming as overrides. --- .../sensors/lidar/fastlio2/recorder.py | 56 ++++----- .../sensors/lidar/pointlio/recorder.py | 54 ++++----- dimos/memory2/tf_recorder.py | 110 ++++++++++++++++++ dimos/robot/all_blueprints.py | 1 + 4 files changed, 154 insertions(+), 67 deletions(-) create mode 100644 dimos/memory2/tf_recorder.py diff --git a/dimos/hardware/sensors/lidar/fastlio2/recorder.py b/dimos/hardware/sensors/lidar/fastlio2/recorder.py index f67d97daee..5fc009271a 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/recorder.py +++ b/dimos/hardware/sensors/lidar/fastlio2/recorder.py @@ -14,14 +14,14 @@ """Record FAST-LIO odometry + lidar into a memory2 SQLite db. -A memory2 ``Recorder`` whose ``odometry`` / ``lidar`` In ports auto-connect to a +A ``TfRecorder`` whose ``odometry`` / ``lidar`` In ports auto-connect to a FastLio2's same-named outputs. Beyond the base recorder it: records under configurable stream names, re-bases timestamps onto the db's existing clock so a -run can be appended and compared on one timeline, replaces only its own streams -when appending (``force``), and sets poses from the odometry stream rather than -tf — each lidar frame is stamped with the latest odometry pose, so -``fastlio_lidar`` carries the trajectory and ``dimos map global`` can register it -directly. +run can be appended and compared on one timeline, and replaces only its own +streams when appending (``force``). Poses come straight from the odometry stream +(``@pose_setter_for``): each lidar frame is stamped with the latest odometry pose +so ``fastlio_lidar`` carries the trajectory and ``dimos map global`` can register +it. """ from __future__ import annotations @@ -33,7 +33,8 @@ from typing import Any from dimos.core.stream import In -from dimos.memory2.module import OnExisting, Recorder, RecorderConfig +from dimos.memory2.module import OnExisting +from dimos.memory2.tf_recorder import TfRecorder, TfRecorderConfig, 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 @@ -42,9 +43,6 @@ _SENSOR_CLOCK_MAX = 1e8 # Strictly-increasing tie-breaker so two samples never collide on ts. _EPS = 1e-9 -# Max sensor-ts gap to attach the latest odometry pose to a lidar frame. Matches -# pose_fill's nearest-match window; odometry is ~30 Hz so this nearly always hits. -_POSE_MATCH_TOL = 0.1 def _existing_min_ts(db_path: Path) -> float: @@ -75,7 +73,7 @@ def _existing_min_ts(db_path: Path) -> float: con.close() -class FastLio2RecorderConfig(RecorderConfig): +class FastLio2RecorderConfig(TfRecorderConfig): """Target db + timing conversion for the FAST-LIO recorder.""" # db stream/table names the FastLio2 outputs are recorded under. @@ -89,7 +87,7 @@ class FastLio2RecorderConfig(RecorderConfig): on_existing: OnExisting = OnExisting.APPEND -class FastLio2Recorder(Recorder): +class FastLio2Recorder(TfRecorder): config: FastLio2RecorderConfig odometry: In[Odometry] @@ -99,9 +97,7 @@ class FastLio2Recorder(Recorder): _ref_start_ts: float = -1.0 _last_odom_ts: float = 0.0 _last_lidar_ts: float = 0.0 - # Latest odometry pose + its raw sensor ts, stamped onto each lidar frame. _last_odom_pose: Pose | None = None - _last_odom_raw_ts: float = 0.0 def _stream_name(self, port_name: str) -> str: if port_name == "odometry": @@ -151,23 +147,15 @@ def _resolve_ts(self, name: str, msg: Any) -> float: self._last_lidar_ts = ts return ts - def _resolve_pose(self, name: str, msg: Any, ts: float) -> Pose | None: - if name == "odometry": - pose = getattr(msg, "pose", None) - inner = getattr(pose, "pose", None) if pose is not None else None - self._last_odom_pose = inner - raw = getattr(msg, "ts", None) - self._last_odom_raw_ts = raw if raw is not None else 0.0 - return inner - # lidar: stamp the latest odometry pose when it's recent enough. Both - # FastLio2 outputs share a publish ts, so the nearest odometry is at most - # ~one odom period stale. Frames with no match (e.g. before the first - # odometry) get None and are map-skipped. - raw = getattr(msg, "ts", None) - raw = raw if raw is not None else 0.0 - if ( - self._last_odom_pose is not None - and abs(raw - self._last_odom_raw_ts) <= _POSE_MATCH_TOL - ): - return self._last_odom_pose - return None + @pose_setter_for("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("lidar") + def _lidar_pose(self, msg: PointCloud2) -> Pose | None: + # Stamp each (sensor-frame) cloud with the most-recent odometry pose + # directly — no tf / static-transform composition. None before the first + # odometry, in which case the frame is stored unposed and map-skipped. + return self._last_odom_pose diff --git a/dimos/hardware/sensors/lidar/pointlio/recorder.py b/dimos/hardware/sensors/lidar/pointlio/recorder.py index e141728252..a5ea6d50d2 100644 --- a/dimos/hardware/sensors/lidar/pointlio/recorder.py +++ b/dimos/hardware/sensors/lidar/pointlio/recorder.py @@ -14,14 +14,14 @@ """Record Point-LIO odometry + lidar into a memory2 SQLite db. -A memory2 ``Recorder`` whose ``odometry`` / ``lidar`` In ports auto-connect to a +A ``TfRecorder`` whose ``odometry`` / ``lidar`` In ports auto-connect to a PointLio's same-named outputs. Beyond the base recorder it: records under configurable stream names, re-bases timestamps onto the db's existing clock so a run can be appended (e.g. onto a fastlio replay) and compared on one timeline, -replaces only its own streams when appending (``force``), and sets poses from the -odometry stream rather than tf — each lidar frame is stamped with the latest -odometry pose, so ``pointlio_lidar`` carries the trajectory and ``dimos map -global`` can register the body-frame cloud directly (no ``pose-fill`` pass). +and replaces only its own streams when appending (``force``). Poses come straight +from the odometry stream (``@pose_setter_for``): each lidar frame is stamped with +the latest odometry pose so ``pointlio_lidar`` carries the trajectory and ``dimos +map global`` can register the body-frame cloud directly (no ``pose-fill`` pass). """ from __future__ import annotations @@ -33,7 +33,8 @@ from typing import Any from dimos.core.stream import In -from dimos.memory2.module import OnExisting, Recorder, RecorderConfig +from dimos.memory2.module import OnExisting +from dimos.memory2.tf_recorder import TfRecorder, TfRecorderConfig, 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 @@ -42,9 +43,6 @@ _SENSOR_CLOCK_MAX = 1e8 # Strictly-increasing tie-breaker so two samples never collide on ts. _EPS = 1e-9 -# Max sensor-ts gap to attach the latest odometry pose to a lidar frame. Matches -# pose_fill's nearest-match window; odometry is ~30 Hz so this nearly always hits. -_POSE_MATCH_TOL = 0.1 def _existing_min_ts(db_path: Path) -> float: @@ -75,7 +73,7 @@ def _existing_min_ts(db_path: Path) -> float: con.close() -class PointlioRecorderConfig(RecorderConfig): +class PointlioRecorderConfig(TfRecorderConfig): """Target db + timing conversion for the Point-LIO recorder.""" # db stream/table names the Point-LIO outputs are recorded under. @@ -89,7 +87,7 @@ class PointlioRecorderConfig(RecorderConfig): on_existing: OnExisting = OnExisting.APPEND -class PointlioRecorder(Recorder): +class PointlioRecorder(TfRecorder): config: PointlioRecorderConfig odometry: In[Odometry] @@ -99,9 +97,7 @@ class PointlioRecorder(Recorder): _ref_start_ts: float = -1.0 _last_odom_ts: float = 0.0 _last_lidar_ts: float = 0.0 - # Latest odometry pose + its raw sensor ts, stamped onto each lidar frame. _last_odom_pose: Pose | None = None - _last_odom_raw_ts: float = 0.0 def _stream_name(self, port_name: str) -> str: if port_name == "odometry": @@ -151,23 +147,15 @@ def _resolve_ts(self, name: str, msg: Any) -> float: self._last_lidar_ts = ts return ts - def _resolve_pose(self, name: str, msg: Any, ts: float) -> Pose | None: - if name == "odometry": - pose = getattr(msg, "pose", None) - inner = getattr(pose, "pose", None) if pose is not None else None - self._last_odom_pose = inner - raw = getattr(msg, "ts", None) - self._last_odom_raw_ts = raw if raw is not None else 0.0 - return inner - # lidar: stamp the latest odometry pose when it's recent enough. Both - # Point-LIO outputs share a publish ts, so the nearest odometry is at most - # ~one odom period stale. Frames with no match (e.g. before the first - # odometry) get None and are map-skipped. - raw = getattr(msg, "ts", None) - raw = raw if raw is not None else 0.0 - if ( - self._last_odom_pose is not None - and abs(raw - self._last_odom_raw_ts) <= _POSE_MATCH_TOL - ): - return self._last_odom_pose - return None + @pose_setter_for("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("lidar") + def _lidar_pose(self, msg: PointCloud2) -> Pose | None: + # Stamp each (sensor-frame) cloud with the most-recent odometry pose + # directly — no tf / static-transform composition. None before the first + # odometry, in which case the frame is stored unposed and map-skipped. + return self._last_odom_pose diff --git a/dimos/memory2/tf_recorder.py b/dimos/memory2/tf_recorder.py new file mode 100644 index 0000000000..6b9fee1ed8 --- /dev/null +++ b/dimos/memory2/tf_recorder.py @@ -0,0 +1,110 @@ +# 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. + +"""A Recorder that also records the tf tree and lets subclasses set per-stream poses. + +Decorate a method with ``@pose_setter_for("stream")`` to control how a recorded +stream's pose is resolved; streams without a setter fall back to the base +Recorder's tf-based ``world <- frame_id`` lookup:: + + class MyRecorder(TfRecorder): + odometry: In[Odometry] + lidar: In[PointCloud2] + + @pose_setter_for("odometry") + def _odom_pose(self, msg): + self._last_pose = msg.pose.pose + return self._last_pose + + @pose_setter_for("lidar") + def _lidar_pose(self, msg): + return self._last_pose # stamp the cloud with the latest odom pose +""" + +from __future__ import annotations + +from collections.abc import Callable +from typing import Any + +from reactivex.disposable import Disposable + +from dimos.core.core import rpc +from dimos.memory2.module import Recorder, RecorderConfig +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +PoseSetter = Callable[[Any], "Pose | None"] + + +def pose_setter_for(*stream_names: str) -> Callable[[Any], Any]: + """Mark a method ``(self, msg) -> Pose | None`` as the pose setter for the + given recorded stream(s).""" + + def decorate(fn: Any) -> Any: + fn._pose_setter_for = tuple(stream_names) + return fn + + return decorate + + +class TfRecorderConfig(RecorderConfig): + # Also record the live tf stream alongside the In ports. + record_tf: bool = True + + +class TfRecorder(Recorder): + config: TfRecorderConfig + + _pose_setters: dict[str, PoseSetter] = {} + + @rpc + def start(self) -> None: + self._pose_setters = self._collect_pose_setters() + super().start() + if self.config.g.replay: + return + if self.config.record_tf: + self._record_tf() + + def _collect_pose_setters(self) -> dict[str, PoseSetter]: + """Map stream name -> bound @pose_setter_for method.""" + setters: dict[str, PoseSetter] = {} + for attr_name in dir(type(self)): + fn = getattr(type(self), attr_name, None) + for stream in getattr(fn, "_pose_setter_for", ()): + setters[stream] = getattr(self, attr_name) + return setters + + def _resolve_pose(self, name: str, msg: Any, ts: float) -> Pose | None: + """Dispatch to the stream's @pose_setter_for, else the base tf lookup.""" + setter = self._pose_setters.get(name) + if setter is not None: + return setter(msg) + return super()._resolve_pose(name, msg, ts) + + def _record_tf(self) -> None: + topic = getattr(self.tf.config, "topic", None) + if not topic: + logger.warning("TfRecorder: no tf topic configured — not recording tf") + return + tf_stream = self.store.stream("tf", TFMessage) + + def on_tf(msg: TFMessage, _topic: object) -> None: + for transform in msg.transforms: + tf_stream.append(TFMessage(transform), ts=transform.ts, pose=None) + + self.register_disposable(Disposable(self.tf.pubsub.subscribe(topic, on_tf))) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 0b35c6e75e..0ba446a1fe 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -236,6 +236,7 @@ "temporal-memory": "dimos.perception.experimental.temporal_memory.temporal_memory.TemporalMemory", "terrain-analysis": "dimos.navigation.nav_stack.modules.terrain_analysis.terrain_analysis.TerrainAnalysis", "terrain-map-ext": "dimos.navigation.nav_stack.modules.terrain_map_ext.terrain_map_ext.TerrainMapExt", + "tf-recorder": "dimos.memory2.tf_recorder.TfRecorder", "twist-teleop-module": "dimos.teleop.quest.quest_extensions.TwistTeleopModule", "unitree-g1-skill-container": "dimos.robot.unitree.g1.skill_container.UnitreeG1SkillContainer", "unitree-skill-container": "dimos.robot.unitree.unitree_skill_container.UnitreeSkillContainer", From 9335298398cefa38e6d685cd90f538b15b866a49 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 22:10:24 -0700 Subject: [PATCH 149/185] fastlio2: publish sensor/body-frame lidar (get_body_cloud), like pointlio MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The FAST-LIO core only exposed get_world_cloud (world-registered). Patch the pinned source in the flake to add get_body_cloud (returns the undistorted scan in the LiDAR/sensor frame, matching pointlio) and publish that instead; the lidar header now carries the body/child frame. Consumers register the cloud via the odometry pose — the recorders' @pose_setter_for already stamps each frame with the latest odom pose, and the pcap_to_db .rrd now transforms body-frame clouds to world by that pose. Guard TfRecorder's tf callback against the teardown race (late LCM callback on a closing store). KNOWN BREAK: the nav stack's registered_scan consumers expect world-frame — they're now fed body-frame and need a separate world-registration step (TBD). --- .../fastlio2/cpp/fast-lio-body-cloud.patch | 30 +++++++++++++ .../sensors/lidar/fastlio2/cpp/flake.nix | 14 +++++- .../sensors/lidar/fastlio2/cpp/main.cpp | 19 ++++---- .../hardware/sensors/lidar/fastlio2/module.py | 3 +- .../lidar/fastlio2/tools/pcap_to_db.py | 45 +++++++++++++++---- dimos/memory2/tf_recorder.py | 9 +++- 6 files changed, 100 insertions(+), 20 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/fast-lio-body-cloud.patch diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/fast-lio-body-cloud.patch b/dimos/hardware/sensors/lidar/fastlio2/cpp/fast-lio-body-cloud.patch new file mode 100644 index 0000000000..f8150f7e94 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/fast-lio-body-cloud.patch @@ -0,0 +1,30 @@ +diff --git a/src/fast_lio.hpp b/src/fast_lio.hpp +index a835e26..9237e50 100644 +--- a/src/fast_lio.hpp ++++ b/src/fast_lio.hpp +@@ -16,6 +16,7 @@ public: + std::vector get_pose(); + const custom_messages::Odometry& get_odometry() const { return *odom_result; } + PointCloudXYZI::Ptr get_world_cloud() const { return laser_mapping->get_world_cloud(); } ++ PointCloudXYZI::Ptr get_body_cloud() const { return laser_mapping->get_body_cloud(); } + void write_to_file(const std::vector &pose); + void write_to_file(const double &time); + private: +diff --git a/src/laserMapping.hpp b/src/laserMapping.hpp +index 561dae0..254c6c5 100644 +--- a/src/laserMapping.hpp ++++ b/src/laserMapping.hpp +@@ -81,6 +81,13 @@ public: + return cloud; + } + ++ /** Return the full undistorted scan in the LiDAR/sensor frame (no world ++ * registration). */ ++ PointCloudXYZI::Ptr get_body_cloud() const { ++ if (!feats_undistort || feats_undistort->empty()) return nullptr; ++ return PointCloudXYZI::Ptr(new PointCloudXYZI(*feats_undistort)); ++ } ++ + private: + void pointBodyToWorld_ikfom(PointType const * const pi, PointType * const po, state_ikfom &s); + void pointBodyToWorld(PointType const * const pi, PointType * const po); diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix index d73c4fd631..a9101e19e9 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix @@ -66,6 +66,18 @@ livox-common = ../../common; + # Patch the pinned FAST-LIO source to add get_body_cloud() (returns the + # undistorted scan in the LiDAR/sensor frame) so the module can publish + # sensor-frame clouds instead of world-registered ones. + fastlioSrc = pkgs.stdenv.mkDerivation { + name = "fast-lio-src-patched"; + src = fast-lio; + patches = [ ./fast-lio-body-cloud.patch ]; + dontConfigure = true; + dontBuild = true; + installPhase = "cp -r . $out"; + }; + fastlio2_native = pkgs.stdenv.mkDerivation { pname = "fastlio2_native"; version = "0.2.0"; @@ -87,7 +99,7 @@ cmakeFlags = [ "-DCMAKE_POLICY_VERSION_MINIMUM=3.5" "-DFETCHCONTENT_SOURCE_DIR_DIMOS_LCM=${dimos-lcm}" - "-DFASTLIO_DIR=${fast-lio}" + "-DFASTLIO_DIR=${fastlioSrc}" "-DLIVOX_COMMON_DIR=${livox-common}" ]; }; diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index b006e29513..fc2345782f 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -4,8 +4,9 @@ // FAST-LIO2 + Livox Mid-360 native module for dimos NativeModule framework. // // Binds Livox SDK2 directly into FAST-LIO-NON-ROS: SDK callbacks feed -// CustomMsg/Imu to FastLio, which performs EKF-LOAM SLAM. Registered -// (world-frame) point clouds and odometry are published on LCM. +// CustomMsg/Imu to FastLio, which performs EKF-LOAM SLAM. Sensor/body-frame +// point clouds and odometry are published on LCM (consumers register the cloud +// via the odometry pose). // // Usage: // ./fastlio2_native \ @@ -89,7 +90,7 @@ static uint64_t get_timestamp_ns(const LivoxLidarEthernetPacket* pkt) { using dimos::time_from_seconds; using dimos::make_header; -// Publish lidar (world-frame point cloud) +// Publish lidar (sensor/body-frame point cloud) static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, const std::string& topic = "") { @@ -99,7 +100,8 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, int num_points = static_cast(cloud->size()); sensor_msgs::PointCloud2 pc; - pc.header = make_header(g_frame_id, timestamp); + // Cloud is in the sensor/body frame (the odometry child frame), not world. + pc.header = make_header(g_child_frame_id, timestamp); pc.height = 1; pc.width = num_points; pc.is_bigendian = 0; @@ -440,11 +442,12 @@ int main(int argc, char** argv) { auto pose = fast_lio.get_pose(); if (!pose.empty() && (pose[0] != 0.0 || pose[1] != 0.0 || pose[2] != 0.0)) { double ts = get_publish_ts(); - auto world_cloud = fast_lio.get_world_cloud(); - if (world_cloud && !world_cloud->empty()) { - // World-frame cloud at pointcloud_freq, published as-is (no downsampling). + auto body_cloud = fast_lio.get_body_cloud(); + if (body_cloud && !body_cloud->empty()) { + // Sensor/body-frame cloud at pointcloud_freq, published as-is (no + // downsampling). Consumers register it via the odometry pose. if (!g_lidar_topic.empty() && now - *last_pc_publish >= pc_interval) { - publish_lidar(world_cloud, ts); + publish_lidar(body_cloud, ts); last_pc_publish = now; } } diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 90262fc48f..6c1359bf69 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -15,7 +15,8 @@ """Python NativeModule wrapper for the FAST-LIO2 + Livox Mid-360 binary. Binds Livox SDK2 into FAST-LIO-NON-ROS for real-time LiDAR SLAM; outputs -registered (world-frame) point clouds and odometry with covariance. +sensor/body-frame point clouds (register via the odometry pose) and odometry +with covariance. FAST-LIO tuning lives directly on ``FastLio2Config`` (no YAML files). On ``start()`` the fields are rendered to a throwaway YAML that the C++ binary diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index c17ae968fe..89905fddb9 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -65,6 +65,20 @@ # Extra seconds past the pcap's own duration before auto-stopping, when no # explicit --max-sensor-sec is given. _DRAIN_MARGIN_SEC = 4.0 +# Max |Δts| to match a lidar frame to an odometry pose when aggregating the .rrd. +_POSE_MATCH_TOL = 0.1 + + +def _quat_to_rot(qx: float, qy: float, qz: float, qw: float) -> Any: + import numpy as np + + return np.array( + [ + [1 - 2 * (qy * qy + qz * qz), 2 * (qx * qy - qz * qw), 2 * (qx * qz + qy * qw)], + [2 * (qx * qy + qz * qw), 1 - 2 * (qx * qx + qz * qz), 2 * (qy * qz - qx * qw)], + [2 * (qx * qz - qy * qw), 2 * (qy * qz + qx * qw), 1 - 2 * (qx * qx + qy * qy)], + ] + ) def _pcap_sensor_span(pcap_path: Path) -> float: @@ -109,13 +123,12 @@ def _odom_stats(db_path: Path, table: str) -> tuple[int, float, float]: def _write_rrd(db_path: Path, odom_stream: str, lidar_stream: str, voxel: float) -> Path | None: - """Aggregate the recorded lidar plus the pose path into a ``.rrd`` next to the - db, for a quick look. + """Aggregate the recorded lidar (registered into world via the nearest + odometry pose) plus the pose path into a ``.rrd`` next to the db. - FastLio2 already publishes its cloud registered into the world frame, so each - frame is aggregated as-is (no per-frame pose transform) then voxel-deduped. - Best-effort: any failure is non-fatal to the recording. Returns the .rrd path, - or None.""" + FastLio2 publishes its cloud in the sensor/body frame, so each frame is + transformed to world by its pose here, then voxel-deduped. Best-effort: any + failure is non-fatal to the recording. Returns the .rrd path, or None.""" import numpy as np import rerun as rr @@ -129,6 +142,7 @@ def _write_rrd(db_path: Path, odom_stream: str, lidar_stream: str, voxel: float) odom = list(store.stream(odom_stream, Odometry).order_by("ts")) if not odom: return None + ots = np.array([o.ts for o in odom]) opos = np.array( [ [ @@ -139,14 +153,29 @@ def _write_rrd(db_path: Path, odom_stream: str, lidar_stream: str, voxel: float) for o in odom ] ) + oquat = np.array( + [ + [ + o.data.pose.pose.orientation.x, + o.data.pose.pose.orientation.y, + o.data.pose.pose.orientation.z, + o.data.pose.pose.orientation.w, + ] + for o in odom + ] + ) chunks: list[Any] = [] for lid in store.stream(lidar_stream, PointCloud2).order_by("ts"): + j = int(np.argmin(np.abs(ots - lid.ts))) + if abs(ots[j] - lid.ts) > _POSE_MATCH_TOL: + continue pts = np.asarray(lid.data.as_numpy()[0])[:, :3].astype(np.float64) if pts.shape[0] == 0: continue + world = pts @ _quat_to_rot(*oquat[j]).T + opos[j] # Per-frame voxel-dedup to bound memory before the global merge. - _, idx = np.unique(np.floor(pts / voxel).astype(np.int64), axis=0, return_index=True) - chunks.append(pts[idx]) + _, idx = np.unique(np.floor(world / voxel).astype(np.int64), axis=0, return_index=True) + chunks.append(world[idx]) if not chunks: return None allpts = np.concatenate(chunks) diff --git a/dimos/memory2/tf_recorder.py b/dimos/memory2/tf_recorder.py index 6b9fee1ed8..905bc4b86d 100644 --- a/dimos/memory2/tf_recorder.py +++ b/dimos/memory2/tf_recorder.py @@ -104,7 +104,12 @@ def _record_tf(self) -> None: tf_stream = self.store.stream("tf", TFMessage) def on_tf(msg: TFMessage, _topic: object) -> None: - for transform in msg.transforms: - tf_stream.append(TFMessage(transform), ts=transform.ts, pose=None) + try: + for transform in msg.transforms: + tf_stream.append(TFMessage(transform), ts=transform.ts, pose=None) + except Exception: + # A late LCM callback during teardown can hit an already-closed + # store; tf is a best-effort quick-look stream, so drop it. + pass self.register_disposable(Disposable(self.tf.pubsub.subscribe(topic, on_tf))) From b2eb4c75915c7360ac8951bdfe0dd44ad0211f96 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 22:16:41 -0700 Subject: [PATCH 150/185] fastlio2: reference dimos-module-fastlio2 body-cloud branch (drop flake patch) get_body_cloud now lives on the fast-lio source branch jeff/feat/fastlio-body-cloud (github.com/dimensionalOS/dimos-module-fastlio2 @ 26f18cf) instead of an in-tree flake patch. Re-point the fast-lio input + relock; remove fast-lio-body-cloud.patch and the fastlioSrc patched-source derivation. Binary rebuilt + e2e-verified. --- .../fastlio2/cpp/fast-lio-body-cloud.patch | 30 ------------------- .../sensors/lidar/fastlio2/cpp/flake.lock | 8 ++--- .../sensors/lidar/fastlio2/cpp/flake.nix | 17 ++--------- 3 files changed, 7 insertions(+), 48 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/fast-lio-body-cloud.patch diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/fast-lio-body-cloud.patch b/dimos/hardware/sensors/lidar/fastlio2/cpp/fast-lio-body-cloud.patch deleted file mode 100644 index f8150f7e94..0000000000 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/fast-lio-body-cloud.patch +++ /dev/null @@ -1,30 +0,0 @@ -diff --git a/src/fast_lio.hpp b/src/fast_lio.hpp -index a835e26..9237e50 100644 ---- a/src/fast_lio.hpp -+++ b/src/fast_lio.hpp -@@ -16,6 +16,7 @@ public: - std::vector get_pose(); - const custom_messages::Odometry& get_odometry() const { return *odom_result; } - PointCloudXYZI::Ptr get_world_cloud() const { return laser_mapping->get_world_cloud(); } -+ PointCloudXYZI::Ptr get_body_cloud() const { return laser_mapping->get_body_cloud(); } - void write_to_file(const std::vector &pose); - void write_to_file(const double &time); - private: -diff --git a/src/laserMapping.hpp b/src/laserMapping.hpp -index 561dae0..254c6c5 100644 ---- a/src/laserMapping.hpp -+++ b/src/laserMapping.hpp -@@ -81,6 +81,13 @@ public: - return cloud; - } - -+ /** Return the full undistorted scan in the LiDAR/sensor frame (no world -+ * registration). */ -+ PointCloudXYZI::Ptr get_body_cloud() const { -+ if (!feats_undistort || feats_undistort->empty()) return nullptr; -+ return PointCloudXYZI::Ptr(new PointCloudXYZI(*feats_undistort)); -+ } -+ - private: - void pointBodyToWorld_ikfom(PointType const * const pi, PointType * const po, state_ikfom &s); - void pointBodyToWorld(PointType const * const pi, PointType * const po); diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock index ed10ba8629..0d02fb1ada 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock @@ -37,16 +37,16 @@ "fast-lio": { "flake": false, "locked": { - "lastModified": 1778784133, - "narHash": "sha256-bZEmIJlrzcqrHLxCiXXXf7fgi2yjVRuad8z08HN1eMU=", + "lastModified": 1781759607, + "narHash": "sha256-qjjMuMLp0pBkKjhKTjzhjWTz10+qYzuXiOkX9xNWcNA=", "owner": "dimensionalOS", "repo": "dimos-module-fastlio2", - "rev": "d93bc6795babe9e63cc77d2bf2b72294d9afa839", + "rev": "26f18cf0a055ef44ba17095bfee394000ecaa558", "type": "github" }, "original": { "owner": "dimensionalOS", - "ref": "v0.3.0-quiet-logs", + "ref": "jeff/feat/fastlio-body-cloud", "repo": "dimos-module-fastlio2", "type": "github" } diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix index a9101e19e9..d9267b7749 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix @@ -13,7 +13,8 @@ flake = false; }; fast-lio = { - url = "github:dimensionalOS/dimos-module-fastlio2/v0.3.0-quiet-logs"; + # v0.3.0-quiet-logs + get_body_cloud() (sensor-frame cloud). + url = "github:dimensionalOS/dimos-module-fastlio2?ref=jeff/feat/fastlio-body-cloud"; flake = false; }; lcm-extended = { @@ -66,18 +67,6 @@ livox-common = ../../common; - # Patch the pinned FAST-LIO source to add get_body_cloud() (returns the - # undistorted scan in the LiDAR/sensor frame) so the module can publish - # sensor-frame clouds instead of world-registered ones. - fastlioSrc = pkgs.stdenv.mkDerivation { - name = "fast-lio-src-patched"; - src = fast-lio; - patches = [ ./fast-lio-body-cloud.patch ]; - dontConfigure = true; - dontBuild = true; - installPhase = "cp -r . $out"; - }; - fastlio2_native = pkgs.stdenv.mkDerivation { pname = "fastlio2_native"; version = "0.2.0"; @@ -99,7 +88,7 @@ cmakeFlags = [ "-DCMAKE_POLICY_VERSION_MINIMUM=3.5" "-DFETCHCONTENT_SOURCE_DIR_DIMOS_LCM=${dimos-lcm}" - "-DFASTLIO_DIR=${fastlioSrc}" + "-DFASTLIO_DIR=${fast-lio}" "-DLIVOX_COMMON_DIR=${livox-common}" ]; }; From f5ca33f05051d3d8d5964bb0dfa396c943ce6b59 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 23:29:31 -0700 Subject: [PATCH 151/185] fastlio2/pointlio: code-review cleanup - delete dead cpp/voxel_map.hpp (unincluded since global_map removal) + its README row - remove dead max_velocity_norm_ms CLI arg in main.cpp (parsed, never forwarded) - pcd_save_en defaults False (FAST-LIO writes .pcd to disk when on); drop the now- redundant pcd_save_en=False from the nav blueprints - drop the '# --- ... ---' banner comments; tighten a few 3-line comments to 2 - render_config_yaml indexes the typed enum fields (no more # type: ignore[index]) - narrow broad excepts: TfRecorder.on_tf -> sqlite3.ProgrammingError; pcap_to_db get_data fallback -> (FileNotFoundError, RuntimeError, OSError) - extrinsic_r default floats; import Any where used --- dimos/control/blueprints/mobile.py | 1 - .../sensors/lidar/fastlio2/cpp/README.md | 1 - .../sensors/lidar/fastlio2/cpp/main.cpp | 5 - .../sensors/lidar/fastlio2/cpp/voxel_map.hpp | 297 ------------------ .../hardware/sensors/lidar/fastlio2/module.py | 22 +- .../sensors/lidar/fastlio2/recorder.py | 5 +- .../lidar/fastlio2/tools/pcap_to_db.py | 2 +- .../hardware/sensors/lidar/pointlio/module.py | 19 +- .../sensors/lidar/pointlio/recorder.py | 5 +- dimos/memory2/module.py | 3 +- dimos/memory2/tf_recorder.py | 8 +- .../robot/diy/alfred/blueprints/alfred_nav.py | 1 - .../primitive/unitree_g1_onboard.py | 1 - 13 files changed, 29 insertions(+), 341 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/voxel_map.hpp diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py index 58ab774d8e..0e184d6a4d 100644 --- a/dimos/control/blueprints/mobile.py +++ b/dimos/control/blueprints/mobile.py @@ -130,7 +130,6 @@ def _flowbase_twist_base( extrinsic_est_en=True, filter_size_surf=0.5, filter_size_map=0.5, - pcd_save_en=False, ), create_nav_stack( planner="simple", diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md b/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md index 7b8d46120c..55db08d01c 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md @@ -102,7 +102,6 @@ and passes it as `--config_path`. | File | Description | |---------------------------|--------------------------------------------------------------| | `main.cpp` | Livox SDK2 + FAST-LIO2 integration, EKF SLAM, LCM publishing | -| `voxel_map.hpp` | Global voxel map accumulation | | `dimos_native_module.hpp` | Reusable header for parsing NativeModule CLI args | | `flake.nix` | Nix flake for hermetic builds | | `CMakeLists.txt` | Build config, fetches dimos-lcm headers automatically | diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index fc2345782f..725f4d1d42 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -310,11 +310,6 @@ int main(int argc, char** argv) { double msr_freq = mod.arg_float("msr_freq", 50.0f); double main_freq = mod.arg_float("main_freq", 5000.0f); - // Post-IESKF velocity cap: if |v_world| exceeds this, restore the EKF to the - // last accepted scan (vel=0) and skip map_incremental, breaking the multi-km/s - // divergence runaway on aggressive motion / IMU gaps. Zero disables. - double max_velocity_norm_ms = mod.arg_float("max_velocity_norm_ms", 0.0f); - // Livox hardware config std::string host_ip = mod.arg("host_ip", "192.168.1.5"); std::string lidar_ip = mod.arg("lidar_ip", "192.168.1.155"); diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/voxel_map.hpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/voxel_map.hpp deleted file mode 100644 index a50740cd04..0000000000 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/voxel_map.hpp +++ /dev/null @@ -1,297 +0,0 @@ -// Copyright 2026 Dimensional Inc. -// SPDX-License-Identifier: Apache-2.0 -// -// Efficient global voxel map using a hash map. -// Supports O(1) insert/update, distance-based pruning, and -// raycasting-based free space clearing via Amanatides & Woo 3D DDA. -// FOV is discovered dynamically from incoming point cloud data. - -#ifndef VOXEL_MAP_HPP_ -#define VOXEL_MAP_HPP_ - -#include -#include -#include - -#include -#include - -struct VoxelKey { - int32_t x, y, z; - bool operator==(const VoxelKey& o) const { return x == o.x && y == o.y && z == o.z; } -}; - -struct VoxelKeyHash { - size_t operator()(const VoxelKey& k) const { - // Fast spatial hash — large primes reduce collisions for grid coords - size_t h = static_cast(k.x) * 73856093u; - h ^= static_cast(k.y) * 19349669u; - h ^= static_cast(k.z) * 83492791u; - return h; - } -}; - -struct Voxel { - float x, y, z; // running centroid - float intensity; - uint32_t count; // points merged into this voxel - uint8_t miss_count; // consecutive scans where a ray passed through without hitting -}; - -/// Config for raycast-based free space clearing. -struct RaycastConfig { - int subsample = 4; // raycast every Nth point - int max_misses = 3; // erase after this many consecutive misses - float fov_margin_rad = 0.035f; // ~2° safety margin added to discovered FOV -}; - -class VoxelMap { -public: - explicit VoxelMap(float voxel_size, float max_range = 100.0f) - : voxel_size_(voxel_size), max_range_(max_range) { - map_.reserve(500000); - } - - /// Insert a point cloud into the map, merging into existing voxels. - /// Resets miss_count for hit voxels. - template - void insert(const typename pcl::PointCloud::Ptr& cloud) { - if (!cloud) return; - float inv = 1.0f / voxel_size_; - for (const auto& pt : cloud->points) { - VoxelKey key{ - static_cast(std::floor(pt.x * inv)), - static_cast(std::floor(pt.y * inv)), - static_cast(std::floor(pt.z * inv))}; - - auto it = map_.find(key); - if (it != map_.end()) { - // Running average update - auto& v = it->second; - float n = static_cast(v.count); - float n1 = n + 1.0f; - v.x = (v.x * n + pt.x) / n1; - v.y = (v.y * n + pt.y) / n1; - v.z = (v.z * n + pt.z) / n1; - v.intensity = (v.intensity * n + pt.intensity) / n1; - v.count++; - v.miss_count = 0; - } else { - map_.emplace(key, Voxel{pt.x, pt.y, pt.z, pt.intensity, 1, 0}); - } - } - } - - /// Cast rays from sensor origin through each point in the cloud. - /// Discovers the sensor FOV from the cloud's elevation angle range, - /// then marks intermediate voxels as missed and erases those exceeding - /// the miss threshold within the discovered FOV. - /// - /// Orientation quaternion (qx,qy,qz,qw) is body→world. - template - void raycast_clear(float ox, float oy, float oz, - float qx, float qy, float qz, float qw, - const typename pcl::PointCloud::Ptr& cloud, - const RaycastConfig& cfg) { - if (!cloud || cloud->empty() || cfg.max_misses <= 0) return; - - // Phase 0: discover FOV from this scan's elevation angles in sensor-local frame - update_fov(ox, oy, oz, qx, qy, qz, qw, cloud); - - // Skip raycasting until we have a valid FOV (need at least a few scans) - if (!fov_valid_) return; - - float inv = 1.0f / voxel_size_; - int n_pts = static_cast(cloud->size()); - float fov_up = fov_up_ + cfg.fov_margin_rad; - float fov_down = fov_down_ - cfg.fov_margin_rad; - - // Phase 1: walk rays, increment miss_count for intermediate voxels - for (int i = 0; i < n_pts; i += cfg.subsample) { - const auto& pt = cloud->points[i]; - raycast_single(ox, oy, oz, pt.x, pt.y, pt.z, inv); - } - - // Phase 2: erase voxels that exceeded miss threshold and are within FOV - for (auto it = map_.begin(); it != map_.end();) { - if (it->second.miss_count > static_cast(cfg.max_misses)) { - if (in_sensor_fov(ox, oy, oz, qx, qy, qz, qw, - it->second.x, it->second.y, it->second.z, - fov_up, fov_down)) { - it = map_.erase(it); - continue; - } - } - ++it; - } - } - - /// Remove voxels farther than max_range from the given position. - void prune(float px, float py, float pz) { - float r2 = max_range_ * max_range_; - for (auto it = map_.begin(); it != map_.end();) { - float dx = it->second.x - px; - float dy = it->second.y - py; - float dz = it->second.z - pz; - if (dx * dx + dy * dy + dz * dz > r2) - it = map_.erase(it); - else - ++it; - } - } - - /// Export all voxel centroids as a point cloud. - template - typename pcl::PointCloud::Ptr to_cloud() const { - typename pcl::PointCloud::Ptr cloud( - new pcl::PointCloud(map_.size(), 1)); - size_t i = 0; - for (const auto& [key, v] : map_) { - auto& pt = cloud->points[i++]; - pt.x = v.x; - pt.y = v.y; - pt.z = v.z; - pt.intensity = v.intensity; - } - return cloud; - } - - size_t size() const { return map_.size(); } - void clear() { map_.clear(); } - void set_max_range(float r) { max_range_ = r; } - float fov_up_deg() const { return fov_up_ * 180.0f / static_cast(M_PI); } - float fov_down_deg() const { return fov_down_ * 180.0f / static_cast(M_PI); } - bool fov_valid() const { return fov_valid_; } - -private: - std::unordered_map map_; - float voxel_size_; - float max_range_; - - // Dynamically discovered sensor FOV (accumulated over scans) - float fov_up_ = -static_cast(M_PI); // start narrow, expand from data - float fov_down_ = static_cast(M_PI); - int fov_scan_count_ = 0; - bool fov_valid_ = false; - static constexpr int FOV_WARMUP_SCANS = 5; // require N scans before trusting FOV - - /// Update discovered FOV from a scan's elevation angles in sensor-local frame. - template - void update_fov(float ox, float oy, float oz, - float qx, float qy, float qz, float qw, - const typename pcl::PointCloud::Ptr& cloud) { - // Inverse quaternion for world→sensor rotation - float nqx = -qx, nqy = -qy, nqz = -qz; - - for (const auto& pt : cloud->points) { - float wx = pt.x - ox, wy = pt.y - oy, wz = pt.z - oz; - - // Rotate to sensor-local frame - float tx = 2.0f * (nqy * wz - nqz * wy); - float ty = 2.0f * (nqz * wx - nqx * wz); - float tz = 2.0f * (nqx * wy - nqy * wx); - float lx = wx + qw * tx + (nqy * tz - nqz * ty); - float ly = wy + qw * ty + (nqz * tx - nqx * tz); - float lz = wz + qw * tz + (nqx * ty - nqy * tx); - - float horiz_dist = std::sqrt(lx * lx + ly * ly); - if (horiz_dist < 1e-6f) continue; - float elevation = std::atan2(lz, horiz_dist); - - if (elevation > fov_up_) fov_up_ = elevation; - if (elevation < fov_down_) fov_down_ = elevation; - } - - if (++fov_scan_count_ >= FOV_WARMUP_SCANS && !fov_valid_) { - fov_valid_ = true; - printf("[voxel_map] FOV discovered: [%.1f, %.1f] deg\n", - fov_down_deg(), fov_up_deg()); - } - } - - /// Amanatides & Woo 3D DDA: walk from (ox,oy,oz) to (px,py,pz), - /// incrementing miss_count for all intermediate voxels. - void raycast_single(float ox, float oy, float oz, - float px, float py, float pz, float inv) { - float dx = px - ox, dy = py - oy, dz = pz - oz; - float len = std::sqrt(dx * dx + dy * dy + dz * dz); - if (len < 1e-6f) return; - dx /= len; dy /= len; dz /= len; - - int32_t cx = static_cast(std::floor(ox * inv)); - int32_t cy = static_cast(std::floor(oy * inv)); - int32_t cz = static_cast(std::floor(oz * inv)); - int32_t ex = static_cast(std::floor(px * inv)); - int32_t ey = static_cast(std::floor(py * inv)); - int32_t ez = static_cast(std::floor(pz * inv)); - - int sx = (dx >= 0) ? 1 : -1; - int sy = (dy >= 0) ? 1 : -1; - int sz = (dz >= 0) ? 1 : -1; - - // tMax: parametric distance along ray to next voxel boundary per axis - // tDelta: parametric distance to cross one full voxel per axis - float tMaxX = (std::abs(dx) < 1e-10f) ? 1e30f - : (((dx > 0 ? cx + 1 : cx) * voxel_size_ - ox) / dx); - float tMaxY = (std::abs(dy) < 1e-10f) ? 1e30f - : (((dy > 0 ? cy + 1 : cy) * voxel_size_ - oy) / dy); - float tMaxZ = (std::abs(dz) < 1e-10f) ? 1e30f - : (((dz > 0 ? cz + 1 : cz) * voxel_size_ - oz) / dz); - - float tDeltaX = (std::abs(dx) < 1e-10f) ? 1e30f : std::abs(voxel_size_ / dx); - float tDeltaY = (std::abs(dy) < 1e-10f) ? 1e30f : std::abs(voxel_size_ / dy); - float tDeltaZ = (std::abs(dz) < 1e-10f) ? 1e30f : std::abs(voxel_size_ / dz); - - // Walk through voxels (skip endpoint — it was hit) - int max_steps = static_cast(len * inv) + 3; // safety bound - for (int step = 0; step < max_steps; ++step) { - if (cx == ex && cy == ey && cz == ez) break; // reached endpoint - - VoxelKey key{cx, cy, cz}; - auto it = map_.find(key); - if (it != map_.end() && it->second.miss_count < 255) { - it->second.miss_count++; - } - - // Step to next voxel on the axis with smallest tMax - if (tMaxX < tMaxY && tMaxX < tMaxZ) { - cx += sx; tMaxX += tDeltaX; - } else if (tMaxY < tMaxZ) { - cy += sy; tMaxY += tDeltaY; - } else { - cz += sz; tMaxZ += tDeltaZ; - } - } - } - - /// Check if a voxel centroid falls within the sensor's vertical FOV. - /// Rotates the vector (sensor→voxel) into sensor-local frame using the - /// inverse of the body→world quaternion, then checks elevation angle. - static bool in_sensor_fov(float ox, float oy, float oz, - float qx, float qy, float qz, float qw, - float vx, float vy, float vz, - float fov_up_rad, float fov_down_rad) { - // Vector from sensor origin to voxel in world frame - float wx = vx - ox, wy = vy - oy, wz = vz - oz; - - // Rotate by quaternion inverse (conjugate): q* = (-qx,-qy,-qz,qw) - float nqx = -qx, nqy = -qy, nqz = -qz; - // t = 2 * cross(q.xyz, v) - float tx = 2.0f * (nqy * wz - nqz * wy); - float ty = 2.0f * (nqz * wx - nqx * wz); - float tz = 2.0f * (nqx * wy - nqy * wx); - // v' = v + qw * t + cross(q.xyz, t) - float lx = wx + qw * tx + (nqy * tz - nqz * ty); - float ly = wy + qw * ty + (nqz * tx - nqx * tz); - float lz = wz + qw * tz + (nqx * ty - nqy * tx); - - // Elevation angle in sensor-local frame - float horiz_dist = std::sqrt(lx * lx + ly * ly); - if (horiz_dist < 1e-6f) return true; // directly above/below, treat as in FOV - float elevation = std::atan2(lz, horiz_dist); - - return elevation >= fov_down_rad && elevation <= fov_up_rad; - } -}; - -#endif diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 6c1359bf69..b40afebf2f 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -29,7 +29,7 @@ from pathlib import Path import tempfile import time -from typing import TYPE_CHECKING, Literal +from typing import TYPE_CHECKING, Any, Literal from pydantic import Field from reactivex.disposable import Disposable @@ -124,7 +124,7 @@ class FastLio2Config(NativeModuleConfig): debug: bool = False - # --- FAST-LIO tuning (rendered to the generated YAML; see _YAML_LAYOUT) --- + # FAST-LIO tuning, rendered to the generated YAML (see _YAML_LAYOUT). # common lid_topic: str = "/livox/lidar" imu_topic: str = "/livox/imu" @@ -149,14 +149,16 @@ class FastLio2Config(NativeModuleConfig): det_range: float = 100.0 extrinsic_est_en: bool = False # online IMU-LiDAR extrinsic estimation extrinsic_t: list[float] = Field(default_factory=lambda: [-0.011, -0.02329, 0.04412]) - extrinsic_r: list[float] = Field(default_factory=lambda: [1, 0, 0, 0, 1, 0, 0, 0, 1]) + extrinsic_r: list[float] = Field( + default_factory=lambda: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + ) # publish path_en: bool = False scan_publish_en: bool = True dense_publish_en: bool = True scan_bodyframe_pub_en: bool = True # pcd_save - pcd_save_en: bool = True + pcd_save_en: bool = False # FAST-LIO writes .pcd files to disk when on interval: int = -1 # SDK port configuration (see livox/ports.py for defaults) @@ -179,14 +181,12 @@ class FastLio2Config(NativeModuleConfig): def render_config_yaml(self) -> str: """Render the FAST-LIO tuning fields to YAML text the C++ binary reads.""" - doc: dict[str, dict[str, object]] = {} + doc: dict[str, dict[str, Any]] = {} for field, (section, key) in _YAML_LAYOUT.items(): - val: object = getattr(self, field) - if field == "lidar_type": - val = _LIDAR_TYPE_CODE[val] # type: ignore[index] - elif field == "timestamp_unit": - val = _TIMESTAMP_UNIT_CODE[val] # type: ignore[index] - doc.setdefault(section, {})[key] = val + doc.setdefault(section, {})[key] = getattr(self, field) + # Enum-like strings -> FAST-LIO int codes. + doc["preprocess"]["lidar_type"] = _LIDAR_TYPE_CODE[self.lidar_type] + doc["preprocess"]["timestamp_unit"] = _TIMESTAMP_UNIT_CODE[self.timestamp_unit] return yaml.safe_dump(doc, sort_keys=False, default_flow_style=False) diff --git a/dimos/hardware/sensors/lidar/fastlio2/recorder.py b/dimos/hardware/sensors/lidar/fastlio2/recorder.py index 5fc009271a..8bed8849f8 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/recorder.py +++ b/dimos/hardware/sensors/lidar/fastlio2/recorder.py @@ -155,7 +155,6 @@ def _odom_pose(self, msg: Odometry) -> Pose | None: @pose_setter_for("lidar") def _lidar_pose(self, msg: PointCloud2) -> Pose | None: - # Stamp each (sensor-frame) cloud with the most-recent odometry pose - # directly — no tf / static-transform composition. None before the first - # odometry, in which case the frame is stored unposed and map-skipped. + # 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/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index 89905fddb9..735d12d1a1 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -304,7 +304,7 @@ def _resolve_db_path(args: argparse.Namespace, pcap_path: Path) -> Path: if fetched.exists(): print(f"[pcap_to_db] fetched --db via get_data: {fetched}", flush=True) return fetched.resolve() - except Exception as exc: # not an LFS-known db -> build from scratch + except (FileNotFoundError, RuntimeError, OSError) as exc: # not an LFS db -> build fresh print( f"[pcap_to_db] --db not found locally or via get_data ({exc}); " "building from scratch", diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index eef14e080a..01e6454947 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -35,7 +35,7 @@ import os from pathlib import Path import tempfile -from typing import TYPE_CHECKING, Literal +from typing import TYPE_CHECKING, Any, Literal from pydantic import Field from reactivex.disposable import Disposable @@ -158,7 +158,7 @@ class PointLioConfig(NativeModuleConfig): debug: bool = False - # --- Point-LIO tuning (rendered to the generated YAML; see _YAML_LAYOUT) --- + # Point-LIO tuning, rendered to the generated YAML (see _YAML_LAYOUT). # common con_frame: bool = False con_frame_num: int = 1 @@ -235,16 +235,13 @@ class PointLioConfig(NativeModuleConfig): def render_config_yaml(self) -> str: """Render the Point-LIO tuning fields to YAML text the C++ binary reads.""" - doc: dict[str, dict[str, object]] = {} + doc: dict[str, dict[str, Any]] = {} for field, (section, key) in _YAML_LAYOUT.items(): - val: object = getattr(self, field) - if field == "lidar_type": - val = _LIDAR_TYPE_CODE[val] # type: ignore[index] - elif field == "timestamp_unit": - val = _TIMESTAMP_UNIT_CODE[val] # type: ignore[index] - elif field == "ivox_nearby_type": - val = _IVOX_NEARBY_CODE[val] # type: ignore[index] - doc.setdefault(section, {})[key] = val + doc.setdefault(section, {})[key] = getattr(self, field) + # Enum-like strings -> Point-LIO int codes. + doc["preprocess"]["lidar_type"] = _LIDAR_TYPE_CODE[self.lidar_type] + doc["preprocess"]["timestamp_unit"] = _TIMESTAMP_UNIT_CODE[self.timestamp_unit] + doc["mapping"]["ivox_nearby_type"] = _IVOX_NEARBY_CODE[self.ivox_nearby_type] return yaml.safe_dump(doc, sort_keys=False, default_flow_style=False) diff --git a/dimos/hardware/sensors/lidar/pointlio/recorder.py b/dimos/hardware/sensors/lidar/pointlio/recorder.py index a5ea6d50d2..d2cf6f86e7 100644 --- a/dimos/hardware/sensors/lidar/pointlio/recorder.py +++ b/dimos/hardware/sensors/lidar/pointlio/recorder.py @@ -155,7 +155,6 @@ def _odom_pose(self, msg: Odometry) -> Pose | None: @pose_setter_for("lidar") def _lidar_pose(self, msg: PointCloud2) -> Pose | None: - # Stamp each (sensor-frame) cloud with the most-recent odometry pose - # directly — no tf / static-transform composition. None before the first - # odometry, in which case the frame is stored unposed and map-skipped. + # 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/memory2/module.py b/dimos/memory2/module.py index 87feff1658..37c80c0c80 100644 --- a/dimos/memory2/module.py +++ b/dimos/memory2/module.py @@ -252,8 +252,7 @@ class OnExisting(str, enum.Enum): OVERWRITE = "overwrite" ERROR = "error" BACKUP = "backup" - # Leave the db untouched (keep all existing streams); subclasses that append - # into a populated db handle their own per-stream replacement in + # Leave the db untouched; subclasses replace only their own streams in # ``_prepare_streams``. APPEND = "append" diff --git a/dimos/memory2/tf_recorder.py b/dimos/memory2/tf_recorder.py index 905bc4b86d..3ea9f7ba26 100644 --- a/dimos/memory2/tf_recorder.py +++ b/dimos/memory2/tf_recorder.py @@ -35,6 +35,7 @@ def _lidar_pose(self, msg): from __future__ import annotations from collections.abc import Callable +import sqlite3 from typing import Any from reactivex.disposable import Disposable @@ -103,13 +104,12 @@ def _record_tf(self) -> None: return tf_stream = self.store.stream("tf", TFMessage) - def on_tf(msg: TFMessage, _topic: object) -> None: + def on_tf(msg: TFMessage, _topic: Any) -> None: try: for transform in msg.transforms: tf_stream.append(TFMessage(transform), ts=transform.ts, pose=None) - except Exception: - # A late LCM callback during teardown can hit an already-closed - # store; tf is a best-effort quick-look stream, so drop it. + except sqlite3.ProgrammingError: + # A late LCM callback raced teardown and hit the closed store. pass self.register_disposable(Disposable(self.tf.pubsub.subscribe(topic, on_tf))) diff --git a/dimos/robot/diy/alfred/blueprints/alfred_nav.py b/dimos/robot/diy/alfred/blueprints/alfred_nav.py index 31eed66e6e..a55eff46e4 100644 --- a/dimos/robot/diy/alfred/blueprints/alfred_nav.py +++ b/dimos/robot/diy/alfred/blueprints/alfred_nav.py @@ -62,7 +62,6 @@ extrinsic_est_en=True, filter_size_surf=0.5, filter_size_map=0.5, - pcd_save_en=False, ), create_nav_stack(**nav_config), MovementManager.blueprint(), diff --git a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py index 473930c376..851e1aa085 100644 --- a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py +++ b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py @@ -35,7 +35,6 @@ extrinsic_est_en=True, filter_size_surf=0.5, filter_size_map=0.5, - pcd_save_en=False, ), G1HighLevelDdsSdk.blueprint(), unitree_g1_vis, From e2ae0b8b11135af28dd74e4eed8405c2b42d7115 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 01:46:26 -0700 Subject: [PATCH 152/185] fastlio2: honor filter_size + publish flags; drop virtual_now() - vendored core (dimos-module-fastlio2 @ 50367cb) now reads mapping.filter_size_surf/map from the YAML (was hardcoded 0.5); relock flake. - main.cpp honors the publish flags (passed as CLI args): scan_publish_en gates the lidar output, scan_bodyframe_pub_en picks sensor/body vs world frame, dense_publish_en voxel-downsamples the cloud when off. - nav blueprints set scan_bodyframe_pub_en=False so registered_scan is world-frame again (fixes the body-frame break). - FastLio2Config drops the lid_topic/imu_topic/path_en/pcd_save_en/interval fields the fork never reads; keeps the live tuning + the 3 publish flags. - remove the pointless virtual_now() wrapper + its unreachable failure branch. --- dimos/control/blueprints/mobile.py | 4 +- .../sensors/lidar/fastlio2/cpp/flake.lock | 6 +- .../sensors/lidar/fastlio2/cpp/main.cpp | 84 ++++++++++--------- .../hardware/sensors/lidar/fastlio2/module.py | 31 ++----- .../robot/diy/alfred/blueprints/alfred_nav.py | 4 +- .../primitive/unitree_g1_onboard.py | 6 +- 6 files changed, 65 insertions(+), 70 deletions(-) diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py index 0e184d6a4d..77a59f1d59 100644 --- a/dimos/control/blueprints/mobile.py +++ b/dimos/control/blueprints/mobile.py @@ -123,13 +123,15 @@ def _flowbase_twist_base( host_ip=os.getenv("LIDAR_HOST_IP", "192.168.1.5"), lidar_ip=os.getenv("LIDAR_IP", "192.168.1.189"), # nav tuning (was config/default.yaml): tighter covariance, live - # extrinsic calibration, shorter range, default 0.5 m IESKF voxel. + # extrinsic calibration, shorter range, 0.5 m IESKF voxel, world-frame + # cloud so the nav stack's registered_scan is world-registered. acc_cov=0.01, gyr_cov=0.01, det_range=60.0, extrinsic_est_en=True, filter_size_surf=0.5, filter_size_map=0.5, + scan_bodyframe_pub_en=False, ), create_nav_stack( planner="simple", diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock index 0d02fb1ada..4802e3f830 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock @@ -37,11 +37,11 @@ "fast-lio": { "flake": false, "locked": { - "lastModified": 1781759607, - "narHash": "sha256-qjjMuMLp0pBkKjhKTjzhjWTz10+qYzuXiOkX9xNWcNA=", + "lastModified": 1781771749, + "narHash": "sha256-/H47XVgjNtajgqbt+cJBHCpap0nPt5jWVTCSIrMcgqY=", "owner": "dimensionalOS", "repo": "dimos-module-fastlio2", - "rev": "26f18cf0a055ef44ba17095bfee394000ecaa558", + "rev": "50367cb1b99efc97c69defc43832063e780f2ccf", "type": "github" }, "original": { diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index 725f4d1d42..8918606026 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -25,7 +25,6 @@ #include #include #include -#include #include #include #include @@ -46,6 +45,8 @@ #include "fast_lio.hpp" #include "fast_lio_debug.hpp" +#include + using livox_common::GRAVITY_MS2; using livox_common::DATA_TYPE_IMU; using livox_common::DATA_TYPE_CARTESIAN_HIGH; @@ -62,11 +63,6 @@ static double get_publish_ts() { std::chrono::system_clock::now().time_since_epoch()).count(); } -// Steady clock for the main loop's frame/publish rate limiters. -static std::optional virtual_now() { - return std::chrono::steady_clock::now(); -} - static std::string g_lidar_topic; static std::string g_odometry_topic; static std::string g_frame_id; // required via --frame_id @@ -90,18 +86,29 @@ static uint64_t get_timestamp_ns(const LivoxLidarEthernetPacket* pkt) { using dimos::time_from_seconds; using dimos::make_header; -// Publish lidar (sensor/body-frame point cloud) +// Leaf size (m) for the non-dense (dense_publish_en=false) downsampled output. +static constexpr float PUBLISH_VOXEL_LEAF = 0.1f; + +static PointCloudXYZI::Ptr voxel_downsample(const PointCloudXYZI::Ptr& cloud, float leaf) { + PointCloudXYZI::Ptr out(new PointCloudXYZI()); + pcl::VoxelGrid vg; + vg.setInputCloud(cloud); + vg.setLeafSize(leaf, leaf, leaf); + vg.filter(*out); + return out; +} + +// Publish a lidar point cloud, stamped with `frame_id`. static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, - const std::string& topic = "") { + const std::string& frame_id, const std::string& topic = "") { const std::string& chan = topic.empty() ? g_lidar_topic : topic; if (!g_lcm || !cloud || cloud->empty() || chan.empty()) return; int num_points = static_cast(cloud->size()); sensor_msgs::PointCloud2 pc; - // Cloud is in the sensor/body frame (the odometry child frame), not world. - pc.header = make_header(g_child_frame_id, timestamp); + pc.header = make_header(frame_id, timestamp); pc.height = 1; pc.width = num_points; pc.is_bigendian = 0; @@ -319,6 +326,13 @@ int main(int argc, char** argv) { float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f); float odom_freq = mod.arg_float("odom_freq", 50.0f); + // Cloud-publish behaviour. scan_publish_en gates the lidar output; + // scan_bodyframe_pub_en picks the sensor/body (true) or world (false) frame; + // dense_publish_en false voxel-downsamples the published cloud. + bool scan_publish_en = mod.arg_bool("scan_publish_en", true); + bool dense_publish_en = mod.arg_bool("dense_publish_en", true); + bool scan_bodyframe_pub_en = mod.arg_bool("scan_bodyframe_pub_en", true); + // Verbose logging — propagates to the FAST-LIO C++ core via the // `fastlio_debug` global. Default false → only real errors print. bool debug = mod.arg_bool("debug", false); @@ -373,30 +387,19 @@ int main(int argc, char** argv) { // wall-clock-paced main thread. auto frame_interval = std::chrono::microseconds( static_cast(1e6 / g_frequency)); - std::optional last_emit; const double process_period_ms = 1000.0 / main_freq; auto pc_interval = std::chrono::microseconds( static_cast(1e6 / pointcloud_freq)); auto odom_interval = std::chrono::microseconds( static_cast(1e6 / odom_freq)); - std::optional last_pc_publish; - std::optional last_odom_publish; - auto run_main_iter = [&](std::chrono::steady_clock::time_point now) { - // Lazy-seed the rate-limit bookmarks on the first iteration so they line - // up with the wall clock instead of firing immediately. - auto seed = now; - if (!last_emit.has_value()) { - last_emit = seed; - } - if (!last_pc_publish.has_value()) { - last_pc_publish = seed; - } - if (!last_odom_publish.has_value()) { - last_odom_publish = seed; - } + // Rate-limit bookmarks, seeded to now so they don't all fire on iteration 1. + auto last_emit = std::chrono::steady_clock::now(); + auto last_pc_publish = last_emit; + auto last_odom_publish = last_emit; + auto run_main_iter = [&](std::chrono::steady_clock::time_point now) { // At frame rate, drain accumulated raw points into a CustomMsg and feed // FAST-LIO. Hold g_pc_mutex across the rate-limit check + swap so a // callback can't slip a packet in between the decision and the swap. @@ -405,7 +408,7 @@ int main(int argc, char** argv) { { std::lock_guard lock(g_pc_mutex); auto check_now = now; - if (check_now - *last_emit >= frame_interval) { + if (check_now - last_emit >= frame_interval) { if (!g_accumulated_points.empty()) { points.swap(g_accumulated_points); frame_start = g_frame_start_ns; @@ -437,18 +440,22 @@ int main(int argc, char** argv) { auto pose = fast_lio.get_pose(); if (!pose.empty() && (pose[0] != 0.0 || pose[1] != 0.0 || pose[2] != 0.0)) { double ts = get_publish_ts(); - auto body_cloud = fast_lio.get_body_cloud(); - if (body_cloud && !body_cloud->empty()) { - // Sensor/body-frame cloud at pointcloud_freq, published as-is (no - // downsampling). Consumers register it via the odometry pose. - if (!g_lidar_topic.empty() && now - *last_pc_publish >= pc_interval) { - publish_lidar(body_cloud, ts); - last_pc_publish = now; + if (scan_publish_en && !g_lidar_topic.empty() + && now - last_pc_publish >= pc_interval) { + // Body frame: register downstream via the odometry pose. World + // frame: already registered. + auto cloud = scan_bodyframe_pub_en ? fast_lio.get_body_cloud() + : fast_lio.get_world_cloud(); + if (cloud && !cloud->empty()) { + if (!dense_publish_en) cloud = voxel_downsample(cloud, PUBLISH_VOXEL_LEAF); + publish_lidar(cloud, ts, + scan_bodyframe_pub_en ? g_child_frame_id : g_frame_id); } + last_pc_publish = now; } // Pose + covariance, rate-limited to odom_freq. - if (!g_odometry_topic.empty() && now - *last_odom_publish >= odom_interval) { + if (!g_odometry_topic.empty() && now - last_odom_publish >= odom_interval) { publish_odometry(fast_lio.get_odometry(), ts); last_odom_publish = now; } @@ -472,12 +479,7 @@ int main(int argc, char** argv) { while (g_running.load()) { auto loop_start = std::chrono::high_resolution_clock::now(); - auto now_opt = virtual_now(); - if (!now_opt.has_value()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - continue; - } - run_main_iter(*now_opt); + run_main_iter(std::chrono::steady_clock::now()); // Drain LCM messages. lcm.handleTimeout(0); diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index b40afebf2f..4b64303130 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -66,11 +66,10 @@ TimestampUnit = Literal["second", "millisecond", "microsecond", "nanosecond"] _TIMESTAMP_UNIT_CODE = {"second": 0, "millisecond": 1, "microsecond": 2, "nanosecond": 3} -# Field name -> FAST-LIO YAML (section, key). Only these fields are rendered into -# the generated config; everything else on FastLio2Config is module plumbing. +# Field name -> FAST-LIO YAML (section, key) for the keys the C++ core reads. +# The publish flags aren't here — the core ignores them; they're passed to the +# dimos binary as CLI args (see main.cpp). _YAML_LAYOUT: dict[str, tuple[str, str]] = { - "lid_topic": ("common", "lid_topic"), - "imu_topic": ("common", "imu_topic"), "time_sync_en": ("common", "time_sync_en"), "time_offset_lidar_to_imu": ("common", "time_offset_lidar_to_imu"), "lidar_type": ("preprocess", "lidar_type"), @@ -89,12 +88,6 @@ "extrinsic_est_en": ("mapping", "extrinsic_est_en"), "extrinsic_t": ("mapping", "extrinsic_T"), "extrinsic_r": ("mapping", "extrinsic_R"), - "path_en": ("publish", "path_en"), - "scan_publish_en": ("publish", "scan_publish_en"), - "dense_publish_en": ("publish", "dense_publish_en"), - "scan_bodyframe_pub_en": ("publish", "scan_bodyframe_pub_en"), - "pcd_save_en": ("pcd_save", "pcd_save_en"), - "interval": ("pcd_save", "interval"), } @@ -126,8 +119,6 @@ class FastLio2Config(NativeModuleConfig): # FAST-LIO tuning, rendered to the generated YAML (see _YAML_LAYOUT). # common - lid_topic: str = "/livox/lidar" - imu_topic: str = "/livox/imu" time_sync_en: bool = False time_offset_lidar_to_imu: float = 0.0 # preprocess @@ -143,8 +134,8 @@ class FastLio2Config(NativeModuleConfig): gyr_cov: float = 0.1 b_acc_cov: float = 0.0001 b_gyr_cov: float = 0.0001 - filter_size_surf: float = 0.1 # IESKF scan voxel; does not affect divergence - filter_size_map: float = 0.1 + filter_size_surf: float = 0.1 # IESKF scan voxel leaf (m) + filter_size_map: float = 0.1 # ikd-tree map voxel leaf (m) fov_degree: int = 360 # FAST-LIO reads this as an int det_range: float = 100.0 extrinsic_est_en: bool = False # online IMU-LiDAR extrinsic estimation @@ -152,14 +143,10 @@ class FastLio2Config(NativeModuleConfig): extrinsic_r: list[float] = Field( default_factory=lambda: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] ) - # publish - path_en: bool = False - scan_publish_en: bool = True - dense_publish_en: bool = True - scan_bodyframe_pub_en: bool = True - # pcd_save - pcd_save_en: bool = False # FAST-LIO writes .pcd files to disk when on - interval: int = -1 + # publish behaviour (passed to the binary as CLI args, not the YAML) + scan_publish_en: bool = True # false closes the lidar output + dense_publish_en: bool = True # false voxel-downsamples the published cloud + scan_bodyframe_pub_en: bool = True # true: sensor/body frame; false: world frame # SDK port configuration (see livox/ports.py for defaults) cmd_data_port: int = SDK_CMD_DATA_PORT diff --git a/dimos/robot/diy/alfred/blueprints/alfred_nav.py b/dimos/robot/diy/alfred/blueprints/alfred_nav.py index a55eff46e4..801fdcd4c3 100644 --- a/dimos/robot/diy/alfred/blueprints/alfred_nav.py +++ b/dimos/robot/diy/alfred/blueprints/alfred_nav.py @@ -55,13 +55,15 @@ host_ip=os.getenv("LIDAR_HOST_IP", "192.168.1.5"), lidar_ip=os.getenv("LIDAR_IP", "192.168.1.189"), # nav tuning (was config/default.yaml): tighter covariance, live - # extrinsic calibration, shorter range, default 0.5 m IESKF voxel. + # extrinsic calibration, shorter range, 0.5 m IESKF voxel, world-frame + # cloud so the nav stack's registered_scan is world-registered. acc_cov=0.01, gyr_cov=0.01, det_range=60.0, extrinsic_est_en=True, filter_size_surf=0.5, filter_size_map=0.5, + scan_bodyframe_pub_en=False, ), create_nav_stack(**nav_config), MovementManager.blueprint(), diff --git a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py index 851e1aa085..97b4b75dab 100644 --- a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py +++ b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py @@ -27,14 +27,16 @@ FastLio2.blueprint( host_ip=os.getenv("LIDAR_HOST_IP", "192.168.123.164"), lidar_ip=os.getenv("LIDAR_IP", "192.168.123.120"), - # nav tuning (was config/default.yaml): tighter covariance, live - # extrinsic calibration, shorter range, default 0.5 m IESKF voxel. + # nav tuning (was config/default.yaml): tighter covariance, live extrinsic + # calibration, shorter range, 0.5 m IESKF voxel, world-frame cloud so the + # nav stack's registered_scan is world-registered. acc_cov=0.01, gyr_cov=0.01, det_range=60.0, extrinsic_est_en=True, filter_size_surf=0.5, filter_size_map=0.5, + scan_bodyframe_pub_en=False, ), G1HighLevelDdsSdk.blueprint(), unitree_g1_vis, From ffd8628b3b29ab583d59b8410d817d84d1909190 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 01:59:32 -0700 Subject: [PATCH 153/185] fastlio2: remove scan_bodyframe_pub_en (always publish sensor/body frame) Drop the body/world toggle; the module always publishes the sensor/body-frame cloud (get_body_cloud). Remove the field, the main.cpp arg + frame branch, and the nav blueprints' scan_bodyframe_pub_en=False overrides. --- dimos/control/blueprints/mobile.py | 4 +--- dimos/hardware/sensors/lidar/fastlio2/cpp/README.md | 4 ++-- dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp | 13 ++++--------- dimos/hardware/sensors/lidar/fastlio2/module.py | 1 - dimos/robot/diy/alfred/blueprints/alfred_nav.py | 4 +--- .../g1/blueprints/primitive/unitree_g1_onboard.py | 4 +--- 6 files changed, 9 insertions(+), 21 deletions(-) diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py index 77a59f1d59..9f8145bf35 100644 --- a/dimos/control/blueprints/mobile.py +++ b/dimos/control/blueprints/mobile.py @@ -123,15 +123,13 @@ def _flowbase_twist_base( host_ip=os.getenv("LIDAR_HOST_IP", "192.168.1.5"), lidar_ip=os.getenv("LIDAR_IP", "192.168.1.189"), # nav tuning (was config/default.yaml): tighter covariance, live - # extrinsic calibration, shorter range, 0.5 m IESKF voxel, world-frame - # cloud so the nav stack's registered_scan is world-registered. + # extrinsic calibration, shorter range, 0.5 m IESKF voxel. acc_cov=0.01, gyr_cov=0.01, det_range=60.0, extrinsic_est_en=True, filter_size_surf=0.5, filter_size_map=0.5, - scan_bodyframe_pub_en=False, ), create_nav_stack( planner="simple", diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md b/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md index 55db08d01c..2ce86ecfe1 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md @@ -2,8 +2,8 @@ Real-time LiDAR SLAM using FAST-LIO2 with integrated Livox Mid-360 driver. Binds Livox SDK2 directly into FAST-LIO-NON-ROS: SDK callbacks feed -CustomMsg/Imu to FastLio, which performs EKF-LOAM SLAM. Registered -(world-frame) point clouds and odometry are published on LCM. +CustomMsg/Imu to FastLio, which performs EKF-LOAM SLAM. Sensor/body-frame +point clouds and odometry are published on LCM. ## Build diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index 8918606026..2696319bcb 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -326,12 +326,10 @@ int main(int argc, char** argv) { float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f); float odom_freq = mod.arg_float("odom_freq", 50.0f); - // Cloud-publish behaviour. scan_publish_en gates the lidar output; - // scan_bodyframe_pub_en picks the sensor/body (true) or world (false) frame; + // Cloud-publish behaviour: scan_publish_en gates the lidar output; // dense_publish_en false voxel-downsamples the published cloud. bool scan_publish_en = mod.arg_bool("scan_publish_en", true); bool dense_publish_en = mod.arg_bool("dense_publish_en", true); - bool scan_bodyframe_pub_en = mod.arg_bool("scan_bodyframe_pub_en", true); // Verbose logging — propagates to the FAST-LIO C++ core via the // `fastlio_debug` global. Default false → only real errors print. @@ -442,14 +440,11 @@ int main(int argc, char** argv) { double ts = get_publish_ts(); if (scan_publish_en && !g_lidar_topic.empty() && now - last_pc_publish >= pc_interval) { - // Body frame: register downstream via the odometry pose. World - // frame: already registered. - auto cloud = scan_bodyframe_pub_en ? fast_lio.get_body_cloud() - : fast_lio.get_world_cloud(); + // Sensor/body-frame cloud; register downstream via the odom pose. + auto cloud = fast_lio.get_body_cloud(); if (cloud && !cloud->empty()) { if (!dense_publish_en) cloud = voxel_downsample(cloud, PUBLISH_VOXEL_LEAF); - publish_lidar(cloud, ts, - scan_bodyframe_pub_en ? g_child_frame_id : g_frame_id); + publish_lidar(cloud, ts, g_child_frame_id); } last_pc_publish = now; } diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 4b64303130..18dbe72388 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -146,7 +146,6 @@ class FastLio2Config(NativeModuleConfig): # publish behaviour (passed to the binary as CLI args, not the YAML) scan_publish_en: bool = True # false closes the lidar output dense_publish_en: bool = True # false voxel-downsamples the published cloud - scan_bodyframe_pub_en: bool = True # true: sensor/body frame; false: world frame # SDK port configuration (see livox/ports.py for defaults) cmd_data_port: int = SDK_CMD_DATA_PORT diff --git a/dimos/robot/diy/alfred/blueprints/alfred_nav.py b/dimos/robot/diy/alfred/blueprints/alfred_nav.py index 801fdcd4c3..be01ef7387 100644 --- a/dimos/robot/diy/alfred/blueprints/alfred_nav.py +++ b/dimos/robot/diy/alfred/blueprints/alfred_nav.py @@ -55,15 +55,13 @@ host_ip=os.getenv("LIDAR_HOST_IP", "192.168.1.5"), lidar_ip=os.getenv("LIDAR_IP", "192.168.1.189"), # nav tuning (was config/default.yaml): tighter covariance, live - # extrinsic calibration, shorter range, 0.5 m IESKF voxel, world-frame - # cloud so the nav stack's registered_scan is world-registered. + # extrinsic calibration, shorter range, 0.5 m IESKF voxel. acc_cov=0.01, gyr_cov=0.01, det_range=60.0, extrinsic_est_en=True, filter_size_surf=0.5, filter_size_map=0.5, - scan_bodyframe_pub_en=False, ), create_nav_stack(**nav_config), MovementManager.blueprint(), diff --git a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py index 97b4b75dab..8882ac7003 100644 --- a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py +++ b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py @@ -28,15 +28,13 @@ host_ip=os.getenv("LIDAR_HOST_IP", "192.168.123.164"), lidar_ip=os.getenv("LIDAR_IP", "192.168.123.120"), # nav tuning (was config/default.yaml): tighter covariance, live extrinsic - # calibration, shorter range, 0.5 m IESKF voxel, world-frame cloud so the - # nav stack's registered_scan is world-registered. + # calibration, shorter range, 0.5 m IESKF voxel. acc_cov=0.01, gyr_cov=0.01, det_range=60.0, extrinsic_est_en=True, filter_size_surf=0.5, filter_size_map=0.5, - scan_bodyframe_pub_en=False, ), G1HighLevelDdsSdk.blueprint(), unitree_g1_vis, From e5621fb3df8618ef497f15f0009d3df07c477249 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 02:06:05 -0700 Subject: [PATCH 154/185] fastlio2 pcap_to_db: open the .rrd in rerun + get_data fallback for --pcap - After writing .rrd, open it in rerun (subprocess), unless --no-gui. - A --pcap that isn't a local file is resolved via get_data (LFS), mirroring the --db fallback, so a get_data-style pcap path works directly. --- .../lidar/fastlio2/tools/pcap_to_db.py | 24 +++++++++++++++---- 1 file changed, 19 insertions(+), 5 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index 735d12d1a1..f838b961c9 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -31,8 +31,7 @@ python -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db --db "$DB" --pcap "$PCAP_PATH" # A quick-look .rrd (aggregated world lidar + pose path) is written next - # to the db automatically. View it with: - rerun "${DB%.db}.rrd" + # to the db and opened in rerun automatically (--no-gui to skip opening). One coordinator runs three autoconnected modules: a ``VirtualMid360`` replays the pcap over the Livox wire (aliasing the host/lidar IPs onto a dummy interface on @@ -47,6 +46,7 @@ import argparse from pathlib import Path import sqlite3 +import subprocess import sys import time from typing import TYPE_CHECKING, Any @@ -332,10 +332,19 @@ def _load_overrides(config: str) -> dict[str, Any]: def _run(args: argparse.Namespace) -> int: from dimos.core.coordination.module_coordinator import ModuleCoordinator - pcap_path = Path(args.pcap).expanduser().resolve() + pcap_path = Path(args.pcap).expanduser() if not pcap_path.exists(): - print(f"[pcap_to_db] missing pcap: {pcap_path}", file=sys.stderr) - return 2 + try: + from dimos.utils.data import get_data + + pcap_path = get_data(args.pcap) + except (FileNotFoundError, RuntimeError, OSError) as exc: + print( + f"[pcap_to_db] pcap not found locally or via get_data: {args.pcap} ({exc})", + file=sys.stderr, + ) + return 2 + pcap_path = pcap_path.resolve() args.pcap_path = pcap_path db_path = _resolve_db_path(args, pcap_path) db_path.parent.mkdir(parents=True, exist_ok=True) @@ -381,6 +390,8 @@ def _run(args: argparse.Namespace) -> int: rrd = _write_rrd(db_path, args.odom_stream_name, args.lidar_stream_name, args.voxel) if rrd is not None: print(f"[pcap_to_db] wrote {rrd.name} (aggregated lidar + pose path)", flush=True) + if not args.no_gui: + subprocess.Popen(["rerun", str(rrd)]) except Exception as exc: # viz is a non-fatal bonus print(f"[pcap_to_db] .rrd generation skipped ({exc})", file=sys.stderr, flush=True) return 0 @@ -419,6 +430,9 @@ def main(argv: list[str]) -> int: action="store_true", help="skip writing the .rrd quick-look (aggregated world lidar + pose path)", ) + parser.add_argument( + "--no-gui", action="store_true", help="write the .rrd but don't open it in rerun" + ) parser.add_argument( "--voxel", type=float, default=0.2, help="voxel size (m) for the .rrd aggregated map" ) From 9f13b1624f36f8c6f7a9cb02d15f2a535ba57ca7 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 02:30:06 -0700 Subject: [PATCH 155/185] blueprints: use default FastLio2 config in nav; stash FlowBase mid360 mount - mobile / alfred_nav / unitree_g1_onboard drop their FastLio2 tuning overrides (acc_cov/gyr_cov/det_range/extrinsic_est_en/filter_size) and use the module defaults. - preserve the removed FlowBase Mid-360 mount pose as FLOWBASE_MID360_MOUNT in the new dimos/hardware/drive_trains/flowbase/config.py for later use. --- dimos/control/blueprints/mobile.py | 8 ------- .../hardware/drive_trains/flowbase/config.py | 22 +++++++++++++++++++ .../robot/diy/alfred/blueprints/alfred_nav.py | 8 ------- .../primitive/unitree_g1_onboard.py | 8 ------- 4 files changed, 22 insertions(+), 24 deletions(-) create mode 100644 dimos/hardware/drive_trains/flowbase/config.py diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py index 9f8145bf35..0a0611490c 100644 --- a/dimos/control/blueprints/mobile.py +++ b/dimos/control/blueprints/mobile.py @@ -122,14 +122,6 @@ def _flowbase_twist_base( FastLio2.blueprint( host_ip=os.getenv("LIDAR_HOST_IP", "192.168.1.5"), lidar_ip=os.getenv("LIDAR_IP", "192.168.1.189"), - # nav tuning (was config/default.yaml): tighter covariance, live - # extrinsic calibration, shorter range, 0.5 m IESKF voxel. - acc_cov=0.01, - gyr_cov=0.01, - det_range=60.0, - extrinsic_est_en=True, - filter_size_surf=0.5, - filter_size_map=0.5, ), create_nav_stack( planner="simple", diff --git a/dimos/hardware/drive_trains/flowbase/config.py b/dimos/hardware/drive_trains/flowbase/config.py new file mode 100644 index 0000000000..494247d6c7 --- /dev/null +++ b/dimos/hardware/drive_trains/flowbase/config.py @@ -0,0 +1,22 @@ +# 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. + +"""FlowBase platform constants.""" + +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 + +# Mid-360 mount pose on the FlowBase (position + orientation) in the base frame. +FLOWBASE_MID360_MOUNT = Pose(0.20, -0.20, 0.10, *Quaternion.from_euler(Vector3(0, 0, 0))) diff --git a/dimos/robot/diy/alfred/blueprints/alfred_nav.py b/dimos/robot/diy/alfred/blueprints/alfred_nav.py index be01ef7387..27f80cea65 100644 --- a/dimos/robot/diy/alfred/blueprints/alfred_nav.py +++ b/dimos/robot/diy/alfred/blueprints/alfred_nav.py @@ -54,14 +54,6 @@ FastLio2.blueprint( host_ip=os.getenv("LIDAR_HOST_IP", "192.168.1.5"), lidar_ip=os.getenv("LIDAR_IP", "192.168.1.189"), - # nav tuning (was config/default.yaml): tighter covariance, live - # extrinsic calibration, shorter range, 0.5 m IESKF voxel. - acc_cov=0.01, - gyr_cov=0.01, - det_range=60.0, - extrinsic_est_en=True, - filter_size_surf=0.5, - filter_size_map=0.5, ), create_nav_stack(**nav_config), MovementManager.blueprint(), diff --git a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py index 8882ac7003..e3ccbe6a36 100644 --- a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py +++ b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py @@ -27,14 +27,6 @@ FastLio2.blueprint( host_ip=os.getenv("LIDAR_HOST_IP", "192.168.123.164"), lidar_ip=os.getenv("LIDAR_IP", "192.168.123.120"), - # nav tuning (was config/default.yaml): tighter covariance, live extrinsic - # calibration, shorter range, 0.5 m IESKF voxel. - acc_cov=0.01, - gyr_cov=0.01, - det_range=60.0, - extrinsic_est_en=True, - filter_size_surf=0.5, - filter_size_map=0.5, ), G1HighLevelDdsSdk.blueprint(), unitree_g1_vis, From 244954f1234fef8d45b9d28458ca4ab2f10fa8ca Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 02:40:00 -0700 Subject: [PATCH 156/185] fastlio2 pcap_to_db: FastLio2 tuning as direct CLI flags MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add a 'FastLio2 tuning' arg group (--acc-cov, --filter-size-surf/map, --det-range, --blind, --fov-degree, --lidar-type, --extrinsic-est-en, --scan/--dense-publish-en, etc.) merged into the FastLio2Config overrides — they take precedence over the --config YAML doc. Only flags that are set override anything. --- .../lidar/fastlio2/tools/pcap_to_db.py | 54 ++++++++++++++++++- 1 file changed, 52 insertions(+), 2 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index f838b961c9..26f7a7f8a9 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -21,9 +21,9 @@ # gen .db from pcap (defaults to .db next to the pcap) python -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db --pcap "$PCAP_PATH" - # override any FastLio2Config field via a small YAML/JSON doc, e.g. {acc_cov: 0.1} + # override FastLio2Config tuning via direct flags (or a --config YAML doc) python -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db \ - --pcap "$PCAP_PATH" --config overrides.yaml + --pcap "$PCAP_PATH" --acc-cov 0.5 --filter-size-surf 0.3 --lidar-type livox # add to existing .db (a missing --db is fetched via get_data before falling # back to building from scratch) @@ -65,6 +65,23 @@ # Extra seconds past the pcap's own duration before auto-stopping, when no # explicit --max-sensor-sec is given. _DRAIN_MARGIN_SEC = 4.0 +# FastLio2Config fields exposed as direct CLI flags (merged into --config). +_TUNING_FIELDS = ( + "acc_cov", + "gyr_cov", + "b_acc_cov", + "b_gyr_cov", + "filter_size_surf", + "filter_size_map", + "det_range", + "blind", + "fov_degree", + "scan_line", + "lidar_type", + "extrinsic_est_en", + "scan_publish_en", + "dense_publish_en", +) # Max |Δts| to match a lidar frame to an odometry pose when aggregating the .rrd. _POSE_MATCH_TOL = 0.1 @@ -349,6 +366,8 @@ def _run(args: argparse.Namespace) -> int: db_path = _resolve_db_path(args, pcap_path) db_path.parent.mkdir(parents=True, exist_ok=True) overrides = _load_overrides(args.config) + # Direct --tuning flags override the --config doc. + overrides.update({f: getattr(args, f) for f in _TUNING_FIELDS if getattr(args, f) is not None}) # Default the stop bound to the pcap's own duration: FAST-LIO keeps # dead-reckoning (publishing at full rate) after the pcap drains, so the @@ -447,6 +466,37 @@ def main(argv: list[str]) -> int: default="", help="YAML/JSON doc of FastLio2Config field overrides (e.g. {acc_cov: 0.1})", ) + # FastLio2Config tuning as direct flags; these take precedence over --config. + tuning = parser.add_argument_group("FastLio2 tuning") + tuning.add_argument("--acc-cov", type=float, help="IMU accel covariance") + tuning.add_argument("--gyr-cov", type=float, help="IMU gyro covariance") + tuning.add_argument("--b-acc-cov", type=float, help="IMU accel bias covariance") + tuning.add_argument("--b-gyr-cov", type=float, help="IMU gyro bias covariance") + tuning.add_argument("--filter-size-surf", type=float, help="IESKF scan voxel leaf (m)") + tuning.add_argument("--filter-size-map", type=float, help="ikd-tree map voxel leaf (m)") + tuning.add_argument("--det-range", type=float, help="max detection range (m)") + tuning.add_argument("--blind", type=float, help="spherical min range (m)") + tuning.add_argument("--fov-degree", type=int, help="sensor FOV (deg)") + tuning.add_argument("--scan-line", type=int, help="lidar scan lines") + tuning.add_argument("--lidar-type", choices=("livox", "velodyne", "ouster")) + tuning.add_argument( + "--extrinsic-est-en", + action=argparse.BooleanOptionalAction, + default=None, + help="online IMU-LiDAR extrinsic estimation", + ) + tuning.add_argument( + "--scan-publish-en", + action=argparse.BooleanOptionalAction, + default=None, + help="publish the lidar cloud", + ) + tuning.add_argument( + "--dense-publish-en", + action=argparse.BooleanOptionalAction, + default=None, + help="publish the full (vs voxel-downsampled) cloud", + ) parser.add_argument( "--odom-stream-name", default="fastlio_odometry", From 6bcaaa3671fd2bf6a48868cb645d2e1263922255 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 03:06:34 -0700 Subject: [PATCH 157/185] =?UTF-8?q?fastlio2:=20no=20YAML=20=E2=80=94=20pas?= =?UTF-8?q?s=20FAST-LIO=20params=20as=20plain=20CLI=20args?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The vendored core (dimos-module-fastlio2 @ a32c9f5) now takes a FastLioParams struct instead of loading a YAML, so drop yaml-cpp (flake + CMake). module.py no longer generates a throwaway YAML / config_path; the tuning fields are emitted as plain CLI args (lidar_type/timestamp_unit as strings, extrinsics as comma lists), and main.cpp reads them into FastLioParams. Also wire dense_publish_en to the core's get_body_cloud_down (IESKF-downsampled scan) instead of a PCL voxel in main.cpp. --- .../sensors/lidar/fastlio2/cpp/CMakeLists.txt | 4 -- .../sensors/lidar/fastlio2/cpp/README.md | 12 ++-- .../sensors/lidar/fastlio2/cpp/flake.lock | 6 +- .../sensors/lidar/fastlio2/cpp/flake.nix | 1 - .../sensors/lidar/fastlio2/cpp/main.cpp | 67 ++++++++++++------ .../hardware/sensors/lidar/fastlio2/module.py | 69 ++----------------- 6 files changed, 58 insertions(+), 101 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt index 81e7a3512d..a0dd5f72d8 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt @@ -57,9 +57,6 @@ find_package(Eigen3 REQUIRED) # PCL (only components we need — avoid full PCL which drags in VTK via io) find_package(PCL 1.8 REQUIRED COMPONENTS common filters) -# yaml-cpp (FAST-LIO config parsing — standard YAML format) -find_package(yaml-cpp REQUIRED) - # Livox SDK2 (from nix or /usr/local fallback) find_library(LIVOX_SDK livox_lidar_sdk_shared) if(NOT LIVOX_SDK) @@ -96,7 +93,6 @@ target_link_libraries(fastlio2_native PRIVATE ${LCM_LIBRARIES} ${LIVOX_SDK} ${PCL_LIBRARIES} - yaml-cpp::yaml-cpp ) if(OpenMP_CXX_FOUND) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md b/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md index 2ce86ecfe1..5359a0bc39 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md @@ -26,7 +26,7 @@ Requires: - CMake >= 3.14 - [LCM](https://lcm-proj.github.io/) (`pacman -S lcm` or build from source) - [Livox SDK2](https://github.com/Livox-SDK/Livox-SDK2) installed to `/usr/local` -- Eigen3, PCL (common, filters), yaml-cpp, Boost, OpenMP +- Eigen3, PCL (common, filters), Boost, OpenMP - [FAST-LIO-NON-ROS](https://github.com/leshy/FAST-LIO-NON-ROS) checked out locally ```bash @@ -74,7 +74,7 @@ autoconnect( --odometry '/odometry#nav_msgs.Odometry' \ --host_ip 192.168.1.5 \ --lidar_ip 192.168.1.155 \ - --config_path /path/to/fastlio.yaml # FAST-LIO tuning; module.py generates this from FastLio2Config + --acc_cov 1.0 --filter_size_surf 0.1 --extrinsic_t -0.011,-0.02329,0.04412 ``` Topic strings must include the `#type` suffix -- this is the actual LCM channel @@ -92,10 +92,10 @@ lcm-spy ## Configuration -There are no checked-in config files. FAST-LIO2 tuning (filter sizes, EKF -covariance, extrinsics, point-cloud processing) lives on `FastLio2Config` in -`../module.py`; on `start()` the module renders those fields to a throwaway YAML -and passes it as `--config_path`. +There are no config files and no YAML. FAST-LIO2 tuning (filter sizes, EKF +covariance, extrinsics, …) lives on `FastLio2Config` in `../module.py` and is +passed to the binary as plain CLI args, which `main.cpp` reads into a +`FastLioParams` struct. ## File overview diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock index 4802e3f830..34bf2f67bf 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock @@ -37,11 +37,11 @@ "fast-lio": { "flake": false, "locked": { - "lastModified": 1781771749, - "narHash": "sha256-/H47XVgjNtajgqbt+cJBHCpap0nPt5jWVTCSIrMcgqY=", + "lastModified": 1781776629, + "narHash": "sha256-Ik3OwjSUZza/C545iPC4G/fzfJfFsdIo2GvplgN45hA=", "owner": "dimensionalOS", "repo": "dimos-module-fastlio2", - "rev": "50367cb1b99efc97c69defc43832063e780f2ccf", + "rev": "a32c9f599940a94595aa72868e2e4ab436a44b75", "type": "github" }, "original": { diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix index d9267b7749..c7c4319440 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix @@ -80,7 +80,6 @@ pkgs.glib pkgs.eigen pkgs.pcl - pkgs.yaml-cpp pkgs.boost pkgs.llvmPackages.openmp ]; diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index 2696319bcb..e0241658b3 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -12,7 +12,7 @@ // ./fastlio2_native \ // --lidar '/lidar#sensor_msgs.PointCloud2' \ // --odometry '/odometry#nav_msgs.Odometry' \ -// --config_path /path/to/fastlio.yaml \ # generated by module.py from FastLio2Config +// --acc_cov 1.0 --filter_size_surf 0.1 ... \ # tuning as plain CLI args // --host_ip 192.168.1.5 --lidar_ip 192.168.1.155 #include @@ -45,8 +45,6 @@ #include "fast_lio.hpp" #include "fast_lio_debug.hpp" -#include - using livox_common::GRAVITY_MS2; using livox_common::DATA_TYPE_IMU; using livox_common::DATA_TYPE_CARTESIAN_HIGH; @@ -86,15 +84,20 @@ static uint64_t get_timestamp_ns(const LivoxLidarEthernetPacket* pkt) { using dimos::time_from_seconds; using dimos::make_header; -// Leaf size (m) for the non-dense (dense_publish_en=false) downsampled output. -static constexpr float PUBLISH_VOXEL_LEAF = 0.1f; - -static PointCloudXYZI::Ptr voxel_downsample(const PointCloudXYZI::Ptr& cloud, float leaf) { - PointCloudXYZI::Ptr out(new PointCloudXYZI()); - pcl::VoxelGrid vg; - vg.setInputCloud(cloud); - vg.setLeafSize(leaf, leaf, leaf); - vg.filter(*out); +// Parse a comma-separated list of doubles (CLI vector args); empty on bad input. +static std::vector parse_doubles(const std::string& csv) { + std::vector out; + size_t i = 0; + while (i < csv.size()) { + size_t j = csv.find(',', i); + if (j == std::string::npos) j = csv.size(); + try { + out.push_back(std::stod(csv.substr(i, j - i))); + } catch (...) { + return {}; + } + i = j + 1; + } return out; } @@ -306,12 +309,30 @@ int main(int argc, char** argv) { return 1; } - // FAST-LIO config path - std::string config_path = mod.arg("config_path", ""); - if (config_path.empty()) { - fprintf(stderr, "Error: --config_path is required\n"); - return 1; - } + // FAST-LIO tuning, passed as CLI args by the dimos module (no YAML). + FastLioParams params; + params.acc_cov = mod.arg_float("acc_cov", params.acc_cov); + params.gyr_cov = mod.arg_float("gyr_cov", params.gyr_cov); + params.b_acc_cov = mod.arg_float("b_acc_cov", params.b_acc_cov); + params.b_gyr_cov = mod.arg_float("b_gyr_cov", params.b_gyr_cov); + params.filter_size_surf = mod.arg_float("filter_size_surf", params.filter_size_surf); + params.filter_size_map = mod.arg_float("filter_size_map", params.filter_size_map); + params.det_range = mod.arg_float("det_range", params.det_range); + params.blind = mod.arg_float("blind", params.blind); + params.time_offset_lidar_to_imu = + mod.arg_float("time_offset_lidar_to_imu", params.time_offset_lidar_to_imu); + params.fov_degree = mod.arg_int("fov_degree", params.fov_degree); + params.scan_line = mod.arg_int("scan_line", params.scan_line); + params.scan_rate = mod.arg_int("scan_rate", params.scan_rate); + params.time_sync_en = mod.arg_bool("time_sync_en", params.time_sync_en); + params.extrinsic_est_en = mod.arg_bool("extrinsic_est_en", params.extrinsic_est_en); + std::string lidar_type = mod.arg("lidar_type", "livox"); + params.lidar_type = lidar_type == "velodyne" ? 2 : lidar_type == "ouster" ? 3 : 1; + std::string ts_unit = mod.arg("timestamp_unit", "microsecond"); + params.timestamp_unit = + ts_unit == "second" ? 0 : ts_unit == "millisecond" ? 1 : ts_unit == "nanosecond" ? 3 : 2; + if (auto et = parse_doubles(mod.arg("extrinsic_t", "")); !et.empty()) params.extrinsic_T = et; + if (auto er = parse_doubles(mod.arg("extrinsic_r", "")); !er.empty()) params.extrinsic_R = er; // FAST-LIO internal processing rates double msr_freq = mod.arg_float("msr_freq", 50.0f); @@ -356,7 +377,8 @@ int main(int argc, char** argv) { g_lidar_topic.empty() ? "(disabled)" : g_lidar_topic.c_str()); printf("[fastlio2] odometry topic: %s\n", g_odometry_topic.empty() ? "(disabled)" : g_odometry_topic.c_str()); - printf("[fastlio2] config: %s\n", config_path.c_str()); + printf("[fastlio2] acc_cov: %.3f filter_size_surf: %.3f\n", + params.acc_cov, params.filter_size_surf); printf("[fastlio2] host_ip: %s lidar_ip: %s frequency: %.1f Hz\n", host_ip.c_str(), lidar_ip.c_str(), g_frequency); printf("[fastlio2] pointcloud_freq: %.1f Hz odom_freq: %.1f Hz\n", @@ -377,7 +399,7 @@ int main(int argc, char** argv) { // Init FAST-LIO with config if (debug) printf("[fastlio2] Initializing FAST-LIO...\n"); - FastLio fast_lio(config_path, msr_freq, main_freq); + FastLio fast_lio(params, msr_freq, main_freq); g_fastlio = &fast_lio; if (debug) printf("[fastlio2] FAST-LIO initialized.\n"); @@ -441,9 +463,10 @@ int main(int argc, char** argv) { if (scan_publish_en && !g_lidar_topic.empty() && now - last_pc_publish >= pc_interval) { // Sensor/body-frame cloud; register downstream via the odom pose. - auto cloud = fast_lio.get_body_cloud(); + // dense_publish_en false -> FAST-LIO's IESKF-downsampled scan. + auto cloud = dense_publish_en ? fast_lio.get_body_cloud() + : fast_lio.get_body_cloud_down(); if (cloud && !cloud->empty()) { - if (!dense_publish_en) cloud = voxel_downsample(cloud, PUBLISH_VOXEL_LEAF); publish_lidar(cloud, ts, g_child_frame_id); } last_pc_publish = now; diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 18dbe72388..b2bdc394e2 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -18,22 +18,18 @@ sensor/body-frame point clouds (register via the odometry pose) and odometry with covariance. -FAST-LIO tuning lives directly on ``FastLio2Config`` (no YAML files). On -``start()`` the fields are rendered to a throwaway YAML that the C++ binary -reads via ``--config_path``. +FAST-LIO tuning lives directly on ``FastLio2Config`` and is passed to the C++ +binary as plain CLI args (no YAML). """ from __future__ import annotations import os -from pathlib import Path -import tempfile import time -from typing import TYPE_CHECKING, Any, Literal +from typing import TYPE_CHECKING, Literal from pydantic import Field from reactivex.disposable import Disposable -import yaml from dimos.core.core import rpc from dimos.core.native_module import NativeModule, NativeModuleConfig @@ -59,36 +55,9 @@ from dimos.navigation.nav_stack.frames import FRAME_BODY, FRAME_ODOM from dimos.spec import perception -# FAST-LIO encodes these as ints/codes; expose human-readable names and translate. +# Human-readable enums; the C++ binary maps these strings to FAST-LIO's int codes. LidarType = Literal["livox", "velodyne", "ouster"] -_LIDAR_TYPE_CODE = {"livox": 1, "velodyne": 2, "ouster": 3} - TimestampUnit = Literal["second", "millisecond", "microsecond", "nanosecond"] -_TIMESTAMP_UNIT_CODE = {"second": 0, "millisecond": 1, "microsecond": 2, "nanosecond": 3} - -# Field name -> FAST-LIO YAML (section, key) for the keys the C++ core reads. -# The publish flags aren't here — the core ignores them; they're passed to the -# dimos binary as CLI args (see main.cpp). -_YAML_LAYOUT: dict[str, tuple[str, str]] = { - "time_sync_en": ("common", "time_sync_en"), - "time_offset_lidar_to_imu": ("common", "time_offset_lidar_to_imu"), - "lidar_type": ("preprocess", "lidar_type"), - "scan_line": ("preprocess", "scan_line"), - "scan_rate": ("preprocess", "scan_rate"), - "timestamp_unit": ("preprocess", "timestamp_unit"), - "blind": ("preprocess", "blind"), - "acc_cov": ("mapping", "acc_cov"), - "gyr_cov": ("mapping", "gyr_cov"), - "b_acc_cov": ("mapping", "b_acc_cov"), - "b_gyr_cov": ("mapping", "b_gyr_cov"), - "filter_size_surf": ("mapping", "filter_size_surf"), - "filter_size_map": ("mapping", "filter_size_map"), - "fov_degree": ("mapping", "fov_degree"), - "det_range": ("mapping", "det_range"), - "extrinsic_est_en": ("mapping", "extrinsic_est_en"), - "extrinsic_t": ("mapping", "extrinsic_T"), - "extrinsic_r": ("mapping", "extrinsic_R"), -} class FastLio2Config(NativeModuleConfig): @@ -159,22 +128,6 @@ class FastLio2Config(NativeModuleConfig): host_imu_data_port: int = SDK_HOST_IMU_DATA_PORT host_log_data_port: int = SDK_HOST_LOG_DATA_PORT - # Set in start() to the generated YAML; passed as --config_path to the binary. - config_path: str | None = None - - # FAST-LIO tuning fields feed the generated YAML, not CLI args. - cli_exclude: frozenset[str] = frozenset(_YAML_LAYOUT) - - def render_config_yaml(self) -> str: - """Render the FAST-LIO tuning fields to YAML text the C++ binary reads.""" - doc: dict[str, dict[str, Any]] = {} - for field, (section, key) in _YAML_LAYOUT.items(): - doc.setdefault(section, {})[key] = getattr(self, field) - # Enum-like strings -> FAST-LIO int codes. - doc["preprocess"]["lidar_type"] = _LIDAR_TYPE_CODE[self.lidar_type] - doc["preprocess"]["timestamp_unit"] = _TIMESTAMP_UNIT_CODE[self.timestamp_unit] - return yaml.safe_dump(doc, sort_keys=False, default_flow_style=False) - class FastLio2(NativeModule, perception.Lidar, perception.Odometry): config: FastLio2Config @@ -182,25 +135,14 @@ class FastLio2(NativeModule, perception.Lidar, perception.Odometry): lidar: Out[PointCloud2] odometry: Out[Odometry] - _config_file: str | None = None - @rpc def start(self) -> None: self._validate_network() - self._write_config() super().start() self.register_disposable( Disposable(self.odometry.transport.subscribe(self._on_odom_for_tf, self.odometry)) ) - def _write_config(self) -> None: - """Render the config fields to a temp YAML and point the binary at it.""" - fd, path = tempfile.mkstemp(prefix="fastlio2_", suffix=".yaml") - with os.fdopen(fd, "w") as f: - f.write(self.config.render_config_yaml()) - self._config_file = path - self.config.config_path = path - def _on_odom_for_tf(self, msg: Odometry) -> None: self.tf.publish( Transform( @@ -224,9 +166,6 @@ def _on_odom_for_tf(self, msg: Odometry) -> None: @rpc def stop(self) -> None: super().stop() - if self._config_file is not None: - Path(self._config_file).unlink(missing_ok=True) - self._config_file = None def _validate_network(self) -> None: lidar_ip = self.config.lidar_ip From 5e88c5423f8b6c30a4095f70bc2b9111d5c7ce10 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 03:11:35 -0700 Subject: [PATCH 158/185] fastlio2 main.cpp: python-style formatting (de-align = and comments) Single space around assignment (was column-aligned, lisp-style) and collapse over-aligned inline comments. --- .../sensors/lidar/fastlio2/cpp/main.cpp | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index e0241658b3..7a8f20be43 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -63,8 +63,8 @@ static double get_publish_ts() { static std::string g_lidar_topic; static std::string g_odometry_topic; -static std::string g_frame_id; // required via --frame_id -static std::string g_child_frame_id; // required via --child_frame_id +static std::string g_frame_id; // required via --frame_id +static std::string g_child_frame_id; // required via --child_frame_id static float g_frequency = 10.0f; // Frame accumulator (Livox SDK raw → CustomMsg) @@ -207,7 +207,7 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ auto* pts = reinterpret_cast(data->data); for (uint16_t i = 0; i < dot_num; ++i) { custom_messages::CustomPoint cp; - cp.x = static_cast(pts[i].x) / 1000.0; // mm → m + cp.x = static_cast(pts[i].x) / 1000.0; // mm → m cp.y = static_cast(pts[i].y) / 1000.0; cp.z = static_cast(pts[i].z) / 1000.0; cp.reflectivity = pts[i].reflectivity; @@ -220,7 +220,7 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ auto* pts = reinterpret_cast(data->data); for (uint16_t i = 0; i < dot_num; ++i) { custom_messages::CustomPoint cp; - cp.x = static_cast(pts[i].x) / 100.0; // cm → m + cp.x = static_cast(pts[i].x) / 100.0; // cm → m cp.y = static_cast(pts[i].y) / 100.0; cp.z = static_cast(pts[i].z) / 100.0; cp.reflectivity = pts[i].reflectivity; @@ -360,16 +360,16 @@ int main(int argc, char** argv) { // SDK network ports (defaults from SdkPorts struct in livox_sdk_config.hpp) livox_common::SdkPorts ports; const livox_common::SdkPorts port_defaults; - ports.cmd_data = mod.arg_int("cmd_data_port", port_defaults.cmd_data); - ports.push_msg = mod.arg_int("push_msg_port", port_defaults.push_msg); - ports.point_data = mod.arg_int("point_data_port", port_defaults.point_data); - ports.imu_data = mod.arg_int("imu_data_port", port_defaults.imu_data); - ports.log_data = mod.arg_int("log_data_port", port_defaults.log_data); - ports.host_cmd_data = mod.arg_int("host_cmd_data_port", port_defaults.host_cmd_data); - ports.host_push_msg = mod.arg_int("host_push_msg_port", port_defaults.host_push_msg); + ports.cmd_data = mod.arg_int("cmd_data_port", port_defaults.cmd_data); + ports.push_msg = mod.arg_int("push_msg_port", port_defaults.push_msg); + ports.point_data = mod.arg_int("point_data_port", port_defaults.point_data); + ports.imu_data = mod.arg_int("imu_data_port", port_defaults.imu_data); + ports.log_data = mod.arg_int("log_data_port", port_defaults.log_data); + ports.host_cmd_data = mod.arg_int("host_cmd_data_port", port_defaults.host_cmd_data); + ports.host_push_msg = mod.arg_int("host_push_msg_port", port_defaults.host_push_msg); ports.host_point_data = mod.arg_int("host_point_data_port", port_defaults.host_point_data); - ports.host_imu_data = mod.arg_int("host_imu_data_port", port_defaults.host_imu_data); - ports.host_log_data = mod.arg_int("host_log_data_port", port_defaults.host_log_data); + ports.host_imu_data = mod.arg_int("host_imu_data_port", port_defaults.host_imu_data); + ports.host_log_data = mod.arg_int("host_log_data_port", port_defaults.host_log_data); if (debug) { printf("[fastlio2] Starting FAST-LIO2 + Livox Mid-360 native module\n"); From d35224ae5cd5b7d1f462e466edd17047d81f1e81 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 03:15:52 -0700 Subject: [PATCH 159/185] fastlio2 main.cpp: inline run_main_iter into the main loop It was a [&]-capturing lambda called from exactly one place (a leftover of the old replay/virtual-clock design). Inline the body into the while loop and drop the now-redundant check_now copy. --- .../sensors/lidar/fastlio2/cpp/main.cpp | 48 +++++++++---------- 1 file changed, 22 insertions(+), 26 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index 7a8f20be43..285957ff10 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -403,8 +403,7 @@ int main(int argc, char** argv) { g_fastlio = &fast_lio; if (debug) printf("[fastlio2] FAST-LIO initialized.\n"); - // Main-loop state. The body lives in `run_main_iter` below, driven by the - // wall-clock-paced main thread. + // Main-loop rate-limit state (consumed by the loop below). auto frame_interval = std::chrono::microseconds( static_cast(1e6 / g_frequency)); const double process_period_ms = 1000.0 / main_freq; @@ -419,7 +418,25 @@ int main(int argc, char** argv) { auto last_pc_publish = last_emit; auto last_odom_publish = last_emit; - auto run_main_iter = [&](std::chrono::steady_clock::time_point now) { + // The Livox SDK opens UDP sockets and dispatches via its own callback + // threads; the main loop below consumes what's queued. + if (!livox_common::init_livox_sdk(host_ip, lidar_ip, ports, debug)) { + return 1; + } + SetLivoxLidarPointCloudCallBack(on_point_cloud, nullptr); + SetLivoxLidarImuDataCallback(on_imu_data, nullptr); + SetLivoxLidarInfoChangeCallback(on_info_change, nullptr); + if (!LivoxLidarSdkStart()) { + fprintf(stderr, "Error: LivoxLidarSdkStart failed\n"); + LivoxLidarSdkUninit(); + return 1; + } + if (debug) printf("[fastlio2] SDK started, waiting for device...\n"); + + while (g_running.load()) { + auto loop_start = std::chrono::high_resolution_clock::now(); + auto now = std::chrono::steady_clock::now(); + // At frame rate, drain accumulated raw points into a CustomMsg and feed // FAST-LIO. Hold g_pc_mutex across the rate-limit check + swap so a // callback can't slip a packet in between the decision and the swap. @@ -427,14 +444,13 @@ int main(int argc, char** argv) { uint64_t frame_start = 0; { std::lock_guard lock(g_pc_mutex); - auto check_now = now; - if (check_now - last_emit >= frame_interval) { + if (now - last_emit >= frame_interval) { if (!g_accumulated_points.empty()) { points.swap(g_accumulated_points); frame_start = g_frame_start_ns; g_frame_has_timestamp = false; } - last_emit = check_now; + last_emit = now; } } if (!points.empty()) { @@ -478,26 +494,6 @@ int main(int argc, char** argv) { last_odom_publish = now; } } - }; - - // The Livox SDK opens UDP sockets and dispatches via its own callback - // threads; the main thread drives run_main_iter, consuming what's queued. - if (!livox_common::init_livox_sdk(host_ip, lidar_ip, ports, debug)) { - return 1; - } - SetLivoxLidarPointCloudCallBack(on_point_cloud, nullptr); - SetLivoxLidarImuDataCallback(on_imu_data, nullptr); - SetLivoxLidarInfoChangeCallback(on_info_change, nullptr); - if (!LivoxLidarSdkStart()) { - fprintf(stderr, "Error: LivoxLidarSdkStart failed\n"); - LivoxLidarSdkUninit(); - return 1; - } - if (debug) printf("[fastlio2] SDK started, waiting for device...\n"); - - while (g_running.load()) { - auto loop_start = std::chrono::high_resolution_clock::now(); - run_main_iter(std::chrono::steady_clock::now()); // Drain LCM messages. lcm.handleTimeout(0); From 404d604f5e9140a8a2183e7ba41e9dd0537dc308 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 03:31:12 -0700 Subject: [PATCH 160/185] fastlio2 main.cpp: run_main_iter as a free function (time as arg, not a lambda) Extract the per-iteration body into a plain static function that takes the time point + the rate-limit bookmarks/intervals/flags as explicit args, instead of a [&]-capturing lambda. --- .../sensors/lidar/fastlio2/cpp/main.cpp | 131 ++++++++++-------- 1 file changed, 73 insertions(+), 58 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index 285957ff10..9a571a5a60 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -297,6 +297,76 @@ static void signal_handler(int /*sig*/) { // Main +// One iteration of the main loop: drain accumulated points into a CustomMsg, +// run a FAST-LIO step, and publish results (rate-limited by the bookmarks). +static void run_main_iter(std::chrono::steady_clock::time_point now, + FastLio& fast_lio, + std::chrono::steady_clock::time_point& last_emit, + std::chrono::steady_clock::time_point& last_pc_publish, + std::chrono::steady_clock::time_point& last_odom_publish, + std::chrono::microseconds frame_interval, + std::chrono::microseconds pc_interval, + std::chrono::microseconds odom_interval, + bool scan_publish_en, bool dense_publish_en) { + // At frame rate, drain accumulated raw points into a CustomMsg and feed + // FAST-LIO. Hold g_pc_mutex across the rate-limit check + swap so a + // callback can't slip a packet in between the decision and the swap. + std::vector points; + uint64_t frame_start = 0; + { + std::lock_guard lock(g_pc_mutex); + if (now - last_emit >= frame_interval) { + if (!g_accumulated_points.empty()) { + points.swap(g_accumulated_points); + frame_start = g_frame_start_ns; + g_frame_has_timestamp = false; + } + last_emit = now; + } + } + if (!points.empty()) { + // Build CustomMsg + auto lidar_msg = boost::make_shared(); + lidar_msg->header.seq = 0; + lidar_msg->header.stamp = custom_messages::Time().fromSec( + static_cast(frame_start) / 1e9); + lidar_msg->header.frame_id = "livox_frame"; + lidar_msg->timebase = frame_start; + lidar_msg->lidar_id = 0; + for (int i = 0; i < 3; i++) lidar_msg->rsvd[i] = 0; + lidar_msg->point_num = static_cast(points.size()); + lidar_msg->points = std::move(points); + fast_lio.feed_lidar(lidar_msg); + } + + // Run one FAST-LIO IESKF step. Cheap when the IMU/lidar queues + // are empty; the heavy work happens after a feed_lidar above. + fast_lio.process(); + + // Check for new SLAM results and publish (rate-limited). + auto pose = fast_lio.get_pose(); + if (!pose.empty() && (pose[0] != 0.0 || pose[1] != 0.0 || pose[2] != 0.0)) { + double ts = get_publish_ts(); + if (scan_publish_en && !g_lidar_topic.empty() + && now - last_pc_publish >= pc_interval) { + // Sensor/body-frame cloud; register downstream via the odom pose. + // dense_publish_en false -> FAST-LIO's IESKF-downsampled scan. + auto cloud = dense_publish_en ? fast_lio.get_body_cloud() + : fast_lio.get_body_cloud_down(); + if (cloud && !cloud->empty()) { + publish_lidar(cloud, ts, g_child_frame_id); + } + last_pc_publish = now; + } + + // Pose + covariance, rate-limited to odom_freq. + if (!g_odometry_topic.empty() && now - last_odom_publish >= odom_interval) { + publish_odometry(fast_lio.get_odometry(), ts); + last_odom_publish = now; + } + } +} + int main(int argc, char** argv) { dimos::NativeModule mod(argc, argv); @@ -436,64 +506,9 @@ int main(int argc, char** argv) { while (g_running.load()) { auto loop_start = std::chrono::high_resolution_clock::now(); auto now = std::chrono::steady_clock::now(); - - // At frame rate, drain accumulated raw points into a CustomMsg and feed - // FAST-LIO. Hold g_pc_mutex across the rate-limit check + swap so a - // callback can't slip a packet in between the decision and the swap. - std::vector points; - uint64_t frame_start = 0; - { - std::lock_guard lock(g_pc_mutex); - if (now - last_emit >= frame_interval) { - if (!g_accumulated_points.empty()) { - points.swap(g_accumulated_points); - frame_start = g_frame_start_ns; - g_frame_has_timestamp = false; - } - last_emit = now; - } - } - if (!points.empty()) { - // Build CustomMsg - auto lidar_msg = boost::make_shared(); - lidar_msg->header.seq = 0; - lidar_msg->header.stamp = custom_messages::Time().fromSec( - static_cast(frame_start) / 1e9); - lidar_msg->header.frame_id = "livox_frame"; - lidar_msg->timebase = frame_start; - lidar_msg->lidar_id = 0; - for (int i = 0; i < 3; i++) lidar_msg->rsvd[i] = 0; - lidar_msg->point_num = static_cast(points.size()); - lidar_msg->points = std::move(points); - fast_lio.feed_lidar(lidar_msg); - } - - // Run one FAST-LIO IESKF step. Cheap when the IMU/lidar queues - // are empty; the heavy work happens after a feed_lidar above. - fast_lio.process(); - - // Check for new SLAM results and publish (rate-limited). - auto pose = fast_lio.get_pose(); - if (!pose.empty() && (pose[0] != 0.0 || pose[1] != 0.0 || pose[2] != 0.0)) { - double ts = get_publish_ts(); - if (scan_publish_en && !g_lidar_topic.empty() - && now - last_pc_publish >= pc_interval) { - // Sensor/body-frame cloud; register downstream via the odom pose. - // dense_publish_en false -> FAST-LIO's IESKF-downsampled scan. - auto cloud = dense_publish_en ? fast_lio.get_body_cloud() - : fast_lio.get_body_cloud_down(); - if (cloud && !cloud->empty()) { - publish_lidar(cloud, ts, g_child_frame_id); - } - last_pc_publish = now; - } - - // Pose + covariance, rate-limited to odom_freq. - if (!g_odometry_topic.empty() && now - last_odom_publish >= odom_interval) { - publish_odometry(fast_lio.get_odometry(), ts); - last_odom_publish = now; - } - } + run_main_iter(now, fast_lio, last_emit, last_pc_publish, last_odom_publish, + frame_interval, pc_interval, odom_interval, + scan_publish_en, dense_publish_en); // Drain LCM messages. lcm.handleTimeout(0); From fa1ee7bd9e463184340686b8c914e54bc0106e6f Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 03:36:06 -0700 Subject: [PATCH 161/185] fastlio2/pointlio recorders: record at msg.ts; drop ts-alignment machinery MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Remove _existing_min_ts / _resolve_offset / _resolve_ts / time_offset and the EPS tie-breaker. Records use the base Recorder's ts (msg.ts) directly — the native modules always stamp a real epoch ts, so no re-basing is needed. Drop the now-unused --time-offset from both pcap_to_db tools. --- .../sensors/lidar/fastlio2/recorder.py | 86 ++----------------- .../lidar/fastlio2/tools/pcap_to_db.py | 7 -- .../sensors/lidar/pointlio/recorder.py | 80 +---------------- .../lidar/pointlio/scripts/pcap_to_db.py | 7 -- 4 files changed, 9 insertions(+), 171 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/recorder.py b/dimos/hardware/sensors/lidar/fastlio2/recorder.py index 8bed8849f8..5d7e775306 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/recorder.py +++ b/dimos/hardware/sensors/lidar/fastlio2/recorder.py @@ -15,22 +15,16 @@ """Record FAST-LIO odometry + lidar into a memory2 SQLite db. A ``TfRecorder`` whose ``odometry`` / ``lidar`` In ports auto-connect to a -FastLio2's same-named outputs. Beyond the base recorder it: records under -configurable stream names, re-bases timestamps onto the db's existing clock so a -run can be appended and compared on one timeline, and replaces only its own -streams when appending (``force``). Poses come straight from the odometry stream -(``@pose_setter_for``): each lidar frame is stamped with the latest odometry pose -so ``fastlio_lidar`` carries the trajectory and ``dimos map global`` can register -it. +FastLio2's same-named outputs. It records them under configurable stream names, +replacing only its own streams when appending (``force``). Poses come straight +from the odometry stream (``@pose_setter_for``): each lidar frame is stamped with +the latest odometry pose so ``fastlio_lidar`` carries the trajectory and ``dimos +map global`` can register it. """ from __future__ import annotations -import math from pathlib import Path -import sqlite3 -import time -from typing import Any from dimos.core.stream import In from dimos.memory2.module import OnExisting @@ -39,48 +33,13 @@ from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -# Below this the db's existing timestamps are sensor-boot seconds, not unix time. -_SENSOR_CLOCK_MAX = 1e8 -# Strictly-increasing tie-breaker so two samples never collide on ts. -_EPS = 1e-9 - - -def _existing_min_ts(db_path: Path) -> float: - """Min ts across the db's existing streams, or -1.0 if none/absent.""" - if not db_path.exists(): - return -1.0 - con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) - try: - tables = [ - row[0] - for row in con.execute("SELECT name FROM sqlite_master WHERE type='table'").fetchall() - ] - best: float | None = None - for table in tables: - if table.startswith("_") or table.startswith("sqlite_"): - continue - try: - cols = [col[1] for col in con.execute(f"PRAGMA table_info('{table}')").fetchall()] - if "ts" not in cols: - continue - row = con.execute(f"SELECT MIN(ts) FROM '{table}'").fetchone() - except sqlite3.OperationalError: - continue - if row and row[0] is not None: - best = row[0] if best is None else min(best, row[0]) - return best if best is not None else -1.0 - finally: - con.close() - class FastLio2RecorderConfig(TfRecorderConfig): - """Target db + timing conversion for the FAST-LIO recorder.""" + """Target db + stream names for the FAST-LIO recorder.""" # db stream/table names the FastLio2 outputs are recorded under. odom_stream_name: str = "fastlio_odometry" lidar_stream_name: str = "fastlio_lidar" - # Explicit offset override; NaN means auto-derive from the db's earliest ts. - time_offset: float = float("nan") # Drop pre-existing odom/lidar streams instead of refusing to overwrite. force: bool = False # Append into a populated db (keep other streams); replace only our two. @@ -93,10 +52,6 @@ class FastLio2Recorder(TfRecorder): odometry: In[Odometry] lidar: In[PointCloud2] - _offset: float | None = None - _ref_start_ts: float = -1.0 - _last_odom_ts: float = 0.0 - _last_lidar_ts: float = 0.0 _last_odom_pose: Pose | None = None def _stream_name(self, port_name: str) -> str: @@ -117,35 +72,6 @@ def _prepare_streams(self) -> None: ) for name in existing: self.store.delete_stream(name) - # Reference is the db's earliest ts *after* dropping our own old streams, - # so only the data we're aligning against counts. - self._ref_start_ts = _existing_min_ts(Path(cfg.db_path)) - - def _resolve_offset(self, first_ts: float) -> float: - override = self.config.time_offset - if not math.isnan(override): - return override - ref = self._ref_start_ts - if ref < 0.0 or ref < _SENSOR_CLOCK_MAX: - # Empty db, or db already on the sensor clock -> exact alignment. - return 0.0 - # db on wall-clock time -> start-align FastLio2 onto the db's earliest ts. - return ref - first_ts - - def _resolve_ts(self, name: str, msg: Any) -> float: - # `is not None`, not `or`: a real sensor ts of 0.0 must not fall back to - # wall time (would misclassify the stream's clock in _resolve_offset). - raw = getattr(msg, "ts", None) - raw = raw if raw is not None else time.time() - if self._offset is None: - self._offset = self._resolve_offset(raw) - last = self._last_odom_ts if name == "odometry" else self._last_lidar_ts - ts = max(raw + self._offset, last + _EPS) - if name == "odometry": - self._last_odom_ts = ts - else: - self._last_lidar_ts = ts - return ts @pose_setter_for("odometry") def _odom_pose(self, msg: Odometry) -> Pose | None: diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index 26f7a7f8a9..9274ffb22e 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -255,7 +255,6 @@ def _build_blueprint( db_path=str(db_path), odom_stream_name=args.odom_stream_name, lidar_stream_name=args.lidar_stream_name, - time_offset=float("nan") if args.time_offset is None else args.time_offset, force=args.force, ), ).global_config(n_workers=4, robot_model="mid360_fastlio_pcap_to_db") @@ -437,12 +436,6 @@ def main(argv: list[str]) -> int: default=0.0, help="stop after N sensor seconds (0 = whole pcap)", ) - parser.add_argument( - "--time-offset", - type=float, - default=None, - help="seconds added to every output ts (auto if omitted)", - ) parser.add_argument("--force", action="store_true", help="overwrite existing fastlio streams") parser.add_argument( "--no-rrd", diff --git a/dimos/hardware/sensors/lidar/pointlio/recorder.py b/dimos/hardware/sensors/lidar/pointlio/recorder.py index d2cf6f86e7..af94a7530b 100644 --- a/dimos/hardware/sensors/lidar/pointlio/recorder.py +++ b/dimos/hardware/sensors/lidar/pointlio/recorder.py @@ -15,10 +15,8 @@ """Record Point-LIO odometry + lidar into a memory2 SQLite db. A ``TfRecorder`` whose ``odometry`` / ``lidar`` In ports auto-connect to a -PointLio's same-named outputs. Beyond the base recorder it: records under -configurable stream names, re-bases timestamps onto the db's existing clock so a -run can be appended (e.g. onto a fastlio replay) and compared on one timeline, -and replaces only its own streams when appending (``force``). Poses come straight +PointLio's same-named outputs. It records them under configurable stream names, +replacing only its own streams when appending (``force``). Poses come straight from the odometry stream (``@pose_setter_for``): each lidar frame is stamped with the latest odometry pose so ``pointlio_lidar`` carries the trajectory and ``dimos map global`` can register the body-frame cloud directly (no ``pose-fill`` pass). @@ -26,11 +24,7 @@ from __future__ import annotations -import math from pathlib import Path -import sqlite3 -import time -from typing import Any from dimos.core.stream import In from dimos.memory2.module import OnExisting @@ -39,48 +33,13 @@ from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -# Below this the db's existing timestamps are sensor-boot seconds, not unix time. -_SENSOR_CLOCK_MAX = 1e8 -# Strictly-increasing tie-breaker so two samples never collide on ts. -_EPS = 1e-9 - - -def _existing_min_ts(db_path: Path) -> float: - """Min ts across the db's existing streams, or -1.0 if none/absent.""" - if not db_path.exists(): - return -1.0 - con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) - try: - tables = [ - row[0] - for row in con.execute("SELECT name FROM sqlite_master WHERE type='table'").fetchall() - ] - best: float | None = None - for table in tables: - if table.startswith("_") or table.startswith("sqlite_"): - continue - try: - cols = [col[1] for col in con.execute(f"PRAGMA table_info('{table}')").fetchall()] - if "ts" not in cols: - continue - row = con.execute(f"SELECT MIN(ts) FROM '{table}'").fetchone() - except sqlite3.OperationalError: - continue - if row and row[0] is not None: - best = row[0] if best is None else min(best, row[0]) - return best if best is not None else -1.0 - finally: - con.close() - class PointlioRecorderConfig(TfRecorderConfig): - """Target db + timing conversion for the Point-LIO recorder.""" + """Target db + stream names for the Point-LIO recorder.""" # db stream/table names the Point-LIO outputs are recorded under. odom_stream_name: str = "pointlio_odometry" lidar_stream_name: str = "pointlio_lidar" - # Explicit offset override; NaN means auto-derive from the db's earliest ts. - time_offset: float = float("nan") # Drop pre-existing odom/lidar streams instead of refusing to overwrite. force: bool = False # Append into a populated db (keep other streams); replace only our two. @@ -93,10 +52,6 @@ class PointlioRecorder(TfRecorder): odometry: In[Odometry] lidar: In[PointCloud2] - _offset: float | None = None - _ref_start_ts: float = -1.0 - _last_odom_ts: float = 0.0 - _last_lidar_ts: float = 0.0 _last_odom_pose: Pose | None = None def _stream_name(self, port_name: str) -> str: @@ -117,35 +72,6 @@ def _prepare_streams(self) -> None: ) for name in existing: self.store.delete_stream(name) - # Reference is the db's earliest ts *after* dropping our own old streams, - # so only the data we're aligning against (e.g. a fastlio replay) counts. - self._ref_start_ts = _existing_min_ts(Path(cfg.db_path)) - - def _resolve_offset(self, first_ts: float) -> float: - override = self.config.time_offset - if not math.isnan(override): - return override - ref = self._ref_start_ts - if ref < 0.0 or ref < _SENSOR_CLOCK_MAX: - # Empty db, or db already on the sensor clock -> exact alignment. - return 0.0 - # db on wall-clock time -> start-align Point-LIO onto the db's earliest ts. - return ref - first_ts - - def _resolve_ts(self, name: str, msg: Any) -> float: - # `is not None`, not `or`: a real sensor ts of 0.0 must not fall back to - # wall time (would misclassify the stream's clock in _resolve_offset). - raw = getattr(msg, "ts", None) - raw = raw if raw is not None else time.time() - if self._offset is None: - self._offset = self._resolve_offset(raw) - last = self._last_odom_ts if name == "odometry" else self._last_lidar_ts - ts = max(raw + self._offset, last + _EPS) - if name == "odometry": - self._last_odom_ts = ts - else: - self._last_lidar_ts = ts - return ts @pose_setter_for("odometry") def _odom_pose(self, msg: Odometry) -> Pose | None: diff --git a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py index ed84b4fe75..2640d56986 100644 --- a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py @@ -237,7 +237,6 @@ def _build_blueprint( db_path=str(db_path), odom_stream_name=args.odom_stream_name, lidar_stream_name=args.lidar_stream_name, - time_offset=float("nan") if args.time_offset is None else args.time_offset, force=args.force, ), ).global_config(n_workers=4, robot_model="mid360_pointlio_pcap_to_db") @@ -382,12 +381,6 @@ def main(argv: list[str]) -> int: default=0.0, help="stop after N sensor seconds (0 = whole pcap)", ) - parser.add_argument( - "--time-offset", - type=float, - default=None, - help="seconds added to every output ts (auto if omitted)", - ) parser.add_argument("--force", action="store_true", help="overwrite existing pointlio streams") parser.add_argument( "--no-rrd", From eb7891e7bd47121ee60a10cee9bfa8898472993c Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 03:39:37 -0700 Subject: [PATCH 162/185] fastlio2: delete cpp/README.md; fix stale 'rendered to YAML' comment The README was redundant; the tuning comment referenced the removed YAML/_YAML_LAYOUT (it's plain CLI args now). --- .../sensors/lidar/fastlio2/cpp/README.md | 108 ------------------ .../hardware/sensors/lidar/fastlio2/module.py | 2 +- 2 files changed, 1 insertion(+), 109 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/fastlio2/cpp/README.md diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md b/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md deleted file mode 100644 index 5359a0bc39..0000000000 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/README.md +++ /dev/null @@ -1,108 +0,0 @@ -# FAST-LIO2 Native Module (C++) - -Real-time LiDAR SLAM using FAST-LIO2 with integrated Livox Mid-360 driver. -Binds Livox SDK2 directly into FAST-LIO-NON-ROS: SDK callbacks feed -CustomMsg/Imu to FastLio, which performs EKF-LOAM SLAM. Sensor/body-frame -point clouds and odometry are published on LCM. - -## Build - -### Nix (recommended) - -```bash -cd dimos/hardware/sensors/lidar/fastlio2/cpp -nix build .#fastlio2_native -``` - -Binary lands at `result/bin/fastlio2_native`. - -The flake pulls Livox SDK2 from the livox sub-flake and -[FAST-LIO-NON-ROS](https://github.com/leshy/FAST-LIO-NON-ROS) from GitHub -automatically. - -### Native (CMake) - -Requires: -- CMake >= 3.14 -- [LCM](https://lcm-proj.github.io/) (`pacman -S lcm` or build from source) -- [Livox SDK2](https://github.com/Livox-SDK/Livox-SDK2) installed to `/usr/local` -- Eigen3, PCL (common, filters), Boost, OpenMP -- [FAST-LIO-NON-ROS](https://github.com/leshy/FAST-LIO-NON-ROS) checked out locally - -```bash -cd dimos/hardware/sensors/lidar/fastlio2/cpp -cmake -B build -DFASTLIO_DIR=$HOME/coding/FAST-LIO-NON-ROS -cmake --build build -j$(nproc) -cmake --install build -``` - -Binary lands at `result/bin/fastlio2_native` (same location as nix). - -If `-DFASTLIO_DIR` is omitted, CMake auto-fetches FAST-LIO-NON-ROS from GitHub. - -## Network setup - -The Mid-360 communicates over USB ethernet. Configure the interface: - -```bash -sudo nmcli con add type ethernet ifname usbeth0 con-name livox-mid360 \ - ipv4.addresses 192.168.1.5/24 ipv4.method manual -sudo nmcli con up livox-mid360 -``` - -This persists across reboots. The lidar defaults to `192.168.1.155`. - -## Usage - -Normally launched by `FastLio2` via the NativeModule framework: - -```python -from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 -from dimos.core.coordination.blueprints import autoconnect - -autoconnect( - FastLio2.blueprint(host_ip="192.168.1.5"), - SomeConsumer.blueprint(), -).build().loop() -``` - -### Manual invocation (for debugging) - -```bash -./result/bin/fastlio2_native \ - --lidar '/pointcloud#sensor_msgs.PointCloud2' \ - --odometry '/odometry#nav_msgs.Odometry' \ - --host_ip 192.168.1.5 \ - --lidar_ip 192.168.1.155 \ - --acc_cov 1.0 --filter_size_surf 0.1 --extrinsic_t -0.011,-0.02329,0.04412 -``` - -Topic strings must include the `#type` suffix -- this is the actual LCM channel -name used by dimos subscribers. - -For full vis: -```sh -rerun-bridge -``` - -For LCM traffic: -```sh -lcm-spy -``` - -## Configuration - -There are no config files and no YAML. FAST-LIO2 tuning (filter sizes, EKF -covariance, extrinsics, …) lives on `FastLio2Config` in `../module.py` and is -passed to the binary as plain CLI args, which `main.cpp` reads into a -`FastLioParams` struct. - -## File overview - -| File | Description | -|---------------------------|--------------------------------------------------------------| -| `main.cpp` | Livox SDK2 + FAST-LIO2 integration, EKF SLAM, LCM publishing | -| `dimos_native_module.hpp` | Reusable header for parsing NativeModule CLI args | -| `flake.nix` | Nix flake for hermetic builds | -| `CMakeLists.txt` | Build config, fetches dimos-lcm headers automatically | -| `../module.py` | Python NativeModule wrapper (`FastLio2`) | diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index b2bdc394e2..e2743eea7e 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -86,7 +86,7 @@ class FastLio2Config(NativeModuleConfig): debug: bool = False - # FAST-LIO tuning, rendered to the generated YAML (see _YAML_LAYOUT). + # FAST-LIO tuning, passed to the binary as plain CLI args (read in main.cpp). # common time_sync_en: bool = False time_offset_lidar_to_imu: float = 0.0 From a2aa3cefdffcdae2b8824238a49d00edd843d208 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 03:42:02 -0700 Subject: [PATCH 163/185] fastlio2: reword acc_cov comment (no jhist/go2 references) Drop the jhist pointer and Go2-specific wording; describe acc_cov generically (high vs low IMU-accel trust). --- dimos/hardware/sensors/lidar/fastlio2/module.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index e2743eea7e..7d05e7af16 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -97,8 +97,8 @@ class FastLio2Config(NativeModuleConfig): timestamp_unit: TimestampUnit = "microsecond" # velodyne/ouster time field unit blind: float = 0.5 # spherical min range (m) # mapping - # acc_cov down-weights the IMU accel prediction; upstream 0.1 lets Go2 odom - # diverge to km/s, 1.0 holds bounded. See jhist dimos-fastlio-velocity-spike.md. + # acc_cov down-weights the IMU accel prediction. 0.01 is high trust (fine for + # drones); 1.0 is low trust (good for robot dogs that go up/down stairs). acc_cov: float = 1.0 gyr_cov: float = 0.1 b_acc_cov: float = 0.0001 From e30afd9f57b257d9e98a6af91e13152beb3ba8c5 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 03:47:29 -0700 Subject: [PATCH 164/185] fastlio2/pointlio recorders: drop the force flag; always replace own streams _prepare_streams is now 3 lines: delete the recorder's own two streams if present (keeping any other streams), then record. Removes the force config + the refuse- to-overwrite raise, and --force from both pcap_to_db tools. --- .../sensors/lidar/fastlio2/recorder.py | 18 ++++-------------- .../sensors/lidar/fastlio2/tools/pcap_to_db.py | 2 -- .../sensors/lidar/pointlio/recorder.py | 18 ++++-------------- .../lidar/pointlio/scripts/pcap_to_db.py | 2 -- 4 files changed, 8 insertions(+), 32 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/recorder.py b/dimos/hardware/sensors/lidar/fastlio2/recorder.py index 5d7e775306..7eb4fd1fea 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/recorder.py +++ b/dimos/hardware/sensors/lidar/fastlio2/recorder.py @@ -24,8 +24,6 @@ from __future__ import annotations -from pathlib import Path - from dimos.core.stream import In from dimos.memory2.module import OnExisting from dimos.memory2.tf_recorder import TfRecorder, TfRecorderConfig, pose_setter_for @@ -40,8 +38,6 @@ class FastLio2RecorderConfig(TfRecorderConfig): # db stream/table names the FastLio2 outputs are recorded under. odom_stream_name: str = "fastlio_odometry" lidar_stream_name: str = "fastlio_lidar" - # Drop pre-existing odom/lidar streams instead of refusing to overwrite. - force: bool = False # Append into a populated db (keep other streams); replace only our two. on_existing: OnExisting = OnExisting.APPEND @@ -62,16 +58,10 @@ def _stream_name(self, port_name: str) -> str: return port_name def _prepare_streams(self) -> None: - cfg = self.config - names = (cfg.odom_stream_name, cfg.lidar_stream_name) - existing = sorted(set(self.store.list_streams()) & set(names)) - if existing and not cfg.force: - raise RuntimeError( - f"FastLio2Recorder: {Path(cfg.db_path).name} already has {existing}; " - "set force=True to overwrite" - ) - for name in existing: - self.store.delete_stream(name) + # Replace only our own streams (keep anything else in the db). + for name in (self.config.odom_stream_name, self.config.lidar_stream_name): + if name in self.store.list_streams(): + self.store.delete_stream(name) @pose_setter_for("odometry") def _odom_pose(self, msg: Odometry) -> Pose | None: diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index 9274ffb22e..a992db8ca6 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -255,7 +255,6 @@ def _build_blueprint( db_path=str(db_path), odom_stream_name=args.odom_stream_name, lidar_stream_name=args.lidar_stream_name, - force=args.force, ), ).global_config(n_workers=4, robot_model="mid360_fastlio_pcap_to_db") @@ -436,7 +435,6 @@ def main(argv: list[str]) -> int: default=0.0, help="stop after N sensor seconds (0 = whole pcap)", ) - parser.add_argument("--force", action="store_true", help="overwrite existing fastlio streams") parser.add_argument( "--no-rrd", action="store_true", diff --git a/dimos/hardware/sensors/lidar/pointlio/recorder.py b/dimos/hardware/sensors/lidar/pointlio/recorder.py index af94a7530b..834b8d2ebe 100644 --- a/dimos/hardware/sensors/lidar/pointlio/recorder.py +++ b/dimos/hardware/sensors/lidar/pointlio/recorder.py @@ -24,8 +24,6 @@ from __future__ import annotations -from pathlib import Path - from dimos.core.stream import In from dimos.memory2.module import OnExisting from dimos.memory2.tf_recorder import TfRecorder, TfRecorderConfig, pose_setter_for @@ -40,8 +38,6 @@ class PointlioRecorderConfig(TfRecorderConfig): # db stream/table names the Point-LIO outputs are recorded under. odom_stream_name: str = "pointlio_odometry" lidar_stream_name: str = "pointlio_lidar" - # Drop pre-existing odom/lidar streams instead of refusing to overwrite. - force: bool = False # Append into a populated db (keep other streams); replace only our two. on_existing: OnExisting = OnExisting.APPEND @@ -62,16 +58,10 @@ def _stream_name(self, port_name: str) -> str: return port_name def _prepare_streams(self) -> None: - cfg = self.config - names = (cfg.odom_stream_name, cfg.lidar_stream_name) - existing = sorted(set(self.store.list_streams()) & set(names)) - if existing and not cfg.force: - raise RuntimeError( - f"PointlioRecorder: {Path(cfg.db_path).name} already has {existing}; " - "set force=True to overwrite" - ) - for name in existing: - self.store.delete_stream(name) + # Replace only our own streams (keep anything else in the db). + for name in (self.config.odom_stream_name, self.config.lidar_stream_name): + if name in self.store.list_streams(): + self.store.delete_stream(name) @pose_setter_for("odometry") def _odom_pose(self, msg: Odometry) -> Pose | None: diff --git a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py index 2640d56986..e43aaa6272 100644 --- a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py @@ -237,7 +237,6 @@ def _build_blueprint( db_path=str(db_path), odom_stream_name=args.odom_stream_name, lidar_stream_name=args.lidar_stream_name, - force=args.force, ), ).global_config(n_workers=4, robot_model="mid360_pointlio_pcap_to_db") @@ -381,7 +380,6 @@ def main(argv: list[str]) -> int: default=0.0, help="stop after N sensor seconds (0 = whole pcap)", ) - parser.add_argument("--force", action="store_true", help="overwrite existing pointlio streams") parser.add_argument( "--no-rrd", action="store_true", From 2eb1ac4514d713c1b247a6bbf4d725d112f66f02 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 03:56:24 -0700 Subject: [PATCH 165/185] fastlio2/pointlio recorders: rename via ports + .remappings, drop _stream_name MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Name the In ports as the db tables (fastlio_/pointlio_odometry/lidar) and wire them to the module's odometry/lidar outputs with .remappings() in pcap_to_db — matching the base Recorder convention (stream = port name). Removes _stream_name and the odom/lidar_stream_name config; _prepare_streams just replaces our own ports' streams. pcap_to_db drops the now-fixed --odom/lidar-stream-name args. --- .../sensors/lidar/fastlio2/recorder.py | 32 +++------ .../lidar/fastlio2/tools/pcap_to_db.py | 71 +++++++++---------- .../sensors/lidar/pointlio/recorder.py | 32 +++------ .../lidar/pointlio/scripts/pcap_to_db.py | 64 ++++++++--------- 4 files changed, 84 insertions(+), 115 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/recorder.py b/dimos/hardware/sensors/lidar/fastlio2/recorder.py index 7eb4fd1fea..39a06c4db1 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/recorder.py +++ b/dimos/hardware/sensors/lidar/fastlio2/recorder.py @@ -14,9 +14,9 @@ """Record FAST-LIO odometry + lidar into a memory2 SQLite db. -A ``TfRecorder`` whose ``odometry`` / ``lidar`` In ports auto-connect to a -FastLio2's same-named outputs. It records them under configurable stream names, -replacing only its own streams when appending (``force``). Poses come straight +A ``TfRecorder`` that records its In ports under their own names +(``fastlio_odometry`` / ``fastlio_lidar``) — wire them to FastLio2's +``odometry`` / ``lidar`` outputs with ``.remappings()``. Poses come straight from the odometry stream (``@pose_setter_for``): each lidar frame is stamped with the latest odometry pose so ``fastlio_lidar`` carries the trajectory and ``dimos map global`` can register it. @@ -33,43 +33,31 @@ class FastLio2RecorderConfig(TfRecorderConfig): - """Target db + stream names for the FAST-LIO recorder.""" - - # db stream/table names the FastLio2 outputs are recorded under. - odom_stream_name: str = "fastlio_odometry" - lidar_stream_name: str = "fastlio_lidar" - # Append into a populated db (keep other streams); replace only our two. + # Append into a populated db (keep other streams); replace only our own. on_existing: OnExisting = OnExisting.APPEND class FastLio2Recorder(TfRecorder): config: FastLio2RecorderConfig - odometry: In[Odometry] - lidar: In[PointCloud2] + fastlio_odometry: In[Odometry] + fastlio_lidar: In[PointCloud2] _last_odom_pose: Pose | None = None - def _stream_name(self, port_name: str) -> str: - if port_name == "odometry": - return self.config.odom_stream_name - if port_name == "lidar": - return self.config.lidar_stream_name - return port_name - def _prepare_streams(self) -> None: - # Replace only our own streams (keep anything else in the db). - for name in (self.config.odom_stream_name, self.config.lidar_stream_name): + # Replace our own streams (keep anything else in the db). + for name in self.inputs: if name in self.store.list_streams(): self.store.delete_stream(name) - @pose_setter_for("odometry") + @pose_setter_for("fastlio_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("lidar") + @pose_setter_for("fastlio_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. diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index a992db8ca6..794c24a327 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -84,6 +84,9 @@ ) # Max |Δts| to match a lidar frame to an odometry pose when aggregating the .rrd. _POSE_MATCH_TOL = 0.1 +# db stream/table names (= the recorder's In-port names). +_ODOM_STREAM = "fastlio_odometry" +_LIDAR_STREAM = "fastlio_lidar" def _quat_to_rot(qx: float, qy: float, qz: float, qw: float) -> Any: @@ -224,9 +227,10 @@ def _build_blueprint( ) -> Blueprint: """autoconnect(VirtualMid360 + FastLio2 + FastLio2Recorder). - FastLio2's ``odometry``/``lidar`` outputs auto-wire to the recorder's - same-named inputs. VirtualMid360 carries no dimos streams — it speaks the - Livox wire protocol, reached by host_ip/lidar_ip, and sets up the NIC itself. + The recorder's ``fastlio_odometry``/``fastlio_lidar`` In ports (which name + the db streams) are remapped to FastLio2's ``odometry``/``lidar`` outputs. + VirtualMid360 carries no dimos streams — it speaks the Livox wire protocol, + reached by host_ip/lidar_ip, and sets up the NIC itself. """ from dimos.core.coordination.blueprints import autoconnect from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 @@ -238,25 +242,30 @@ def _build_blueprint( ) fastlio_kwargs.update(overrides) - return autoconnect( - VirtualMid360.blueprint( - pcap=str(args.pcap_path), - rate=args.rate, - delay=args.warmup_sec, # hold streaming until FastLio2's SDK is up - host_ip=args.host_ip, - lidar_ip=args.lidar_ip, - alias_iface=args.alias_iface, - # When the NIC is provisioned by hand, skip the module's own sudo - # (it runs in a tty-less worker where a password prompt can't appear). - setup_network=not args.no_network_setup, - ), - FastLio2.blueprint(**fastlio_kwargs), - FastLio2Recorder.blueprint( - db_path=str(db_path), - odom_stream_name=args.odom_stream_name, - lidar_stream_name=args.lidar_stream_name, - ), - ).global_config(n_workers=4, robot_model="mid360_fastlio_pcap_to_db") + return ( + autoconnect( + VirtualMid360.blueprint( + pcap=str(args.pcap_path), + rate=args.rate, + delay=args.warmup_sec, # hold streaming until FastLio2's SDK is up + host_ip=args.host_ip, + lidar_ip=args.lidar_ip, + alias_iface=args.alias_iface, + # When the NIC is provisioned by hand, skip the module's own sudo + # (it runs in a tty-less worker where a password prompt can't appear). + setup_network=not args.no_network_setup, + ), + FastLio2.blueprint(**fastlio_kwargs), + FastLio2Recorder.blueprint(db_path=str(db_path)), + ) + .remappings( + [ + (FastLio2Recorder, "fastlio_odometry", "odometry"), + (FastLio2Recorder, "fastlio_lidar", "lidar"), + ] + ) + .global_config(n_workers=4, robot_model="mid360_fastlio_pcap_to_db") + ) def _poll_until_drained( @@ -387,14 +396,12 @@ def _run(args: argparse.Namespace) -> int: coord = None try: coord = ModuleCoordinator.build(_build_blueprint(args, db_path, overrides)) - drained = _poll_until_drained( - db_path, args.odom_stream_name, args.lidar_stream_name, max_sensor_sec - ) + drained = _poll_until_drained(db_path, _ODOM_STREAM, _LIDAR_STREAM, max_sensor_sec) finally: if coord is not None: coord.stop() - o_cnt, o_min, o_max = _odom_stats(db_path, args.odom_stream_name) + o_cnt, o_min, o_max = _odom_stats(db_path, _ODOM_STREAM) if o_cnt == 0 or not drained: print("[pcap_to_db] no odometry recorded — check the run above", file=sys.stderr) return 1 @@ -404,7 +411,7 @@ def _run(args: argparse.Namespace) -> int: ) if not args.no_rrd: try: - rrd = _write_rrd(db_path, args.odom_stream_name, args.lidar_stream_name, args.voxel) + rrd = _write_rrd(db_path, _ODOM_STREAM, _LIDAR_STREAM, args.voxel) if rrd is not None: print(f"[pcap_to_db] wrote {rrd.name} (aggregated lidar + pose path)", flush=True) if not args.no_gui: @@ -488,16 +495,6 @@ def main(argv: list[str]) -> int: default=None, help="publish the full (vs voxel-downsampled) cloud", ) - parser.add_argument( - "--odom-stream-name", - default="fastlio_odometry", - help="db stream/table name for the recorded odometry", - ) - parser.add_argument( - "--lidar-stream-name", - default="fastlio_lidar", - help="db stream/table name for the recorded point cloud", - ) # Addressing knobs (override to run two replays at once). parser.add_argument("--host-ip", default="192.168.1.5") parser.add_argument("--lidar-ip", default="192.168.1.155") diff --git a/dimos/hardware/sensors/lidar/pointlio/recorder.py b/dimos/hardware/sensors/lidar/pointlio/recorder.py index 834b8d2ebe..31e504616e 100644 --- a/dimos/hardware/sensors/lidar/pointlio/recorder.py +++ b/dimos/hardware/sensors/lidar/pointlio/recorder.py @@ -14,9 +14,9 @@ """Record Point-LIO odometry + lidar into a memory2 SQLite db. -A ``TfRecorder`` whose ``odometry`` / ``lidar`` In ports auto-connect to a -PointLio's same-named outputs. It records them under configurable stream names, -replacing only its own streams when appending (``force``). Poses come straight +A ``TfRecorder`` that records its In ports under their own names +(``pointlio_odometry`` / ``pointlio_lidar``) — wire them to PointLio's +``odometry`` / ``lidar`` outputs with ``.remappings()``. Poses come straight from the odometry stream (``@pose_setter_for``): each lidar frame is stamped with the latest odometry pose so ``pointlio_lidar`` carries the trajectory and ``dimos map global`` can register the body-frame cloud directly (no ``pose-fill`` pass). @@ -33,43 +33,31 @@ class PointlioRecorderConfig(TfRecorderConfig): - """Target db + stream names for the Point-LIO recorder.""" - - # db stream/table names the Point-LIO outputs are recorded under. - odom_stream_name: str = "pointlio_odometry" - lidar_stream_name: str = "pointlio_lidar" - # Append into a populated db (keep other streams); replace only our two. + # Append into a populated db (keep other streams); replace only our own. on_existing: OnExisting = OnExisting.APPEND class PointlioRecorder(TfRecorder): config: PointlioRecorderConfig - odometry: In[Odometry] - lidar: In[PointCloud2] + pointlio_odometry: In[Odometry] + pointlio_lidar: In[PointCloud2] _last_odom_pose: Pose | None = None - def _stream_name(self, port_name: str) -> str: - if port_name == "odometry": - return self.config.odom_stream_name - if port_name == "lidar": - return self.config.lidar_stream_name - return port_name - def _prepare_streams(self) -> None: - # Replace only our own streams (keep anything else in the db). - for name in (self.config.odom_stream_name, self.config.lidar_stream_name): + # Replace our own streams (keep anything else in the db). + for name in self.inputs: if name in self.store.list_streams(): self.store.delete_stream(name) - @pose_setter_for("odometry") + @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("lidar") + @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. diff --git a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py index e43aaa6272..b842634e33 100644 --- a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py @@ -63,6 +63,9 @@ _STARTUP_TIMEOUT_SEC = 60.0 # Max |Δts| to match a lidar frame to an odometry pose when aggregating the .rrd. _POSE_MATCH_TOL = 0.1 +# db stream/table names (= the recorder's In-port names). +_ODOM_STREAM = "pointlio_odometry" +_LIDAR_STREAM = "pointlio_lidar" # Extra seconds past the pcap's own duration before auto-stopping, when no # explicit --max-sensor-sec is given. _DRAIN_MARGIN_SEC = 4.0 @@ -220,25 +223,30 @@ def _build_blueprint( ) pointlio_kwargs.update(overrides) - return autoconnect( - VirtualMid360.blueprint( - pcap=str(args.pcap_path), - rate=args.rate, - delay=args.warmup_sec, # hold streaming until PointLio's SDK is up - host_ip=args.host_ip, - lidar_ip=args.lidar_ip, - alias_iface=args.alias_iface, - # When the NIC is provisioned by hand, skip the module's own sudo - # (it runs in a tty-less worker where a password prompt can't appear). - setup_network=not args.no_network_setup, - ), - PointLio.blueprint(**pointlio_kwargs), - PointlioRecorder.blueprint( - db_path=str(db_path), - odom_stream_name=args.odom_stream_name, - lidar_stream_name=args.lidar_stream_name, - ), - ).global_config(n_workers=4, robot_model="mid360_pointlio_pcap_to_db") + return ( + autoconnect( + VirtualMid360.blueprint( + pcap=str(args.pcap_path), + rate=args.rate, + delay=args.warmup_sec, # hold streaming until PointLio's SDK is up + host_ip=args.host_ip, + lidar_ip=args.lidar_ip, + alias_iface=args.alias_iface, + # When the NIC is provisioned by hand, skip the module's own sudo + # (it runs in a tty-less worker where a password prompt can't appear). + setup_network=not args.no_network_setup, + ), + PointLio.blueprint(**pointlio_kwargs), + PointlioRecorder.blueprint(db_path=str(db_path)), + ) + .remappings( + [ + (PointlioRecorder, "pointlio_odometry", "odometry"), + (PointlioRecorder, "pointlio_lidar", "lidar"), + ] + ) + .global_config(n_workers=4, robot_model="mid360_pointlio_pcap_to_db") + ) def _poll_until_drained( @@ -334,14 +342,12 @@ def _run(args: argparse.Namespace) -> int: coord = None try: coord = ModuleCoordinator.build(_build_blueprint(args, db_path, overrides)) - drained = _poll_until_drained( - db_path, args.odom_stream_name, args.lidar_stream_name, max_sensor_sec - ) + drained = _poll_until_drained(db_path, _ODOM_STREAM, _LIDAR_STREAM, max_sensor_sec) finally: if coord is not None: coord.stop() - o_cnt, o_min, o_max = _odom_stats(db_path, args.odom_stream_name) + o_cnt, o_min, o_max = _odom_stats(db_path, _ODOM_STREAM) if o_cnt == 0 or not drained: print("[pcap_to_db] no odometry recorded — check the run above", file=sys.stderr) return 1 @@ -351,7 +357,7 @@ def _run(args: argparse.Namespace) -> int: ) if not args.no_rrd: try: - rrd = _write_rrd(db_path, args.odom_stream_name, args.lidar_stream_name, args.voxel) + rrd = _write_rrd(db_path, _ODOM_STREAM, _LIDAR_STREAM, args.voxel) if rrd is not None: print(f"[pcap_to_db] wrote {rrd.name} (aggregated lidar + pose path)", flush=True) except Exception as exc: # viz is a non-fatal bonus @@ -399,16 +405,6 @@ def main(argv: list[str]) -> int: default="", help="YAML/JSON doc of PointLioConfig field overrides (e.g. {acc_cov_input: 0.3})", ) - parser.add_argument( - "--odom-stream-name", - default="pointlio_odometry", - help="db stream/table name for the recorded odometry", - ) - parser.add_argument( - "--lidar-stream-name", - default="pointlio_lidar", - help="db stream/table name for the recorded point cloud", - ) # Addressing knobs (override to run two replays at once). parser.add_argument("--host-ip", default="192.168.1.5") parser.add_argument("--lidar-ip", default="192.168.1.155") From 9183d7e10712fbcc4bfe1d05cafeff46e9c84949 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 04:13:10 -0700 Subject: [PATCH 166/185] memory2: fold TfRecorder into base Recorder (all Recorders record tf) Move record_tf + @pose_setter_for into Recorder/RecorderConfig and delete tf_recorder.py. Every Recorder now records the live tf stream by default and supports per-stream pose setters; fastlio/pointlio recorders subclass Recorder directly. Drop the now-redundant tf-recorder blueprint entry. --- .../sensors/lidar/fastlio2/recorder.py | 9 +- .../sensors/lidar/pointlio/recorder.py | 9 +- dimos/memory2/module.py | 74 ++++++++++- dimos/memory2/tf_recorder.py | 115 ------------------ dimos/robot/all_blueprints.py | 1 - 5 files changed, 78 insertions(+), 130 deletions(-) delete mode 100644 dimos/memory2/tf_recorder.py diff --git a/dimos/hardware/sensors/lidar/fastlio2/recorder.py b/dimos/hardware/sensors/lidar/fastlio2/recorder.py index 39a06c4db1..c5df2b961b 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/recorder.py +++ b/dimos/hardware/sensors/lidar/fastlio2/recorder.py @@ -14,7 +14,7 @@ """Record FAST-LIO odometry + lidar into a memory2 SQLite db. -A ``TfRecorder`` that records its In ports under their own names +A ``Recorder`` that records its In ports under their own names (``fastlio_odometry`` / ``fastlio_lidar``) — wire them to FastLio2's ``odometry`` / ``lidar`` outputs with ``.remappings()``. Poses come straight from the odometry stream (``@pose_setter_for``): each lidar frame is stamped with @@ -25,19 +25,18 @@ from __future__ import annotations from dimos.core.stream import In -from dimos.memory2.module import OnExisting -from dimos.memory2.tf_recorder import TfRecorder, TfRecorderConfig, pose_setter_for +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 FastLio2RecorderConfig(TfRecorderConfig): +class FastLio2RecorderConfig(RecorderConfig): # Append into a populated db (keep other streams); replace only our own. on_existing: OnExisting = OnExisting.APPEND -class FastLio2Recorder(TfRecorder): +class FastLio2Recorder(Recorder): config: FastLio2RecorderConfig fastlio_odometry: In[Odometry] diff --git a/dimos/hardware/sensors/lidar/pointlio/recorder.py b/dimos/hardware/sensors/lidar/pointlio/recorder.py index 31e504616e..741b091c2c 100644 --- a/dimos/hardware/sensors/lidar/pointlio/recorder.py +++ b/dimos/hardware/sensors/lidar/pointlio/recorder.py @@ -14,7 +14,7 @@ """Record Point-LIO odometry + lidar into a memory2 SQLite db. -A ``TfRecorder`` that records its In ports under their own names +A ``Recorder`` that records its In ports under their own names (``pointlio_odometry`` / ``pointlio_lidar``) — wire them to PointLio's ``odometry`` / ``lidar`` outputs with ``.remappings()``. Poses come straight from the odometry stream (``@pose_setter_for``): each lidar frame is stamped with @@ -25,19 +25,18 @@ from __future__ import annotations from dimos.core.stream import In -from dimos.memory2.module import OnExisting -from dimos.memory2.tf_recorder import TfRecorder, TfRecorderConfig, pose_setter_for +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 PointlioRecorderConfig(TfRecorderConfig): +class PointlioRecorderConfig(RecorderConfig): # Append into a populated db (keep other streams); replace only our own. on_existing: OnExisting = OnExisting.APPEND -class PointlioRecorder(TfRecorder): +class PointlioRecorder(Recorder): config: PointlioRecorderConfig pointlio_odometry: In[Odometry] diff --git a/dimos/memory2/module.py b/dimos/memory2/module.py index 37c80c0c80..3a542a6a38 100644 --- a/dimos/memory2/module.py +++ b/dimos/memory2/module.py @@ -14,10 +14,12 @@ from __future__ import annotations +from collections.abc import Callable import enum import inspect import os from pathlib import Path +import sqlite3 import time from typing import TYPE_CHECKING, Any, Generic, TypeVar, cast @@ -37,6 +39,7 @@ from dimos.models.embedding.base import EmbeddingModel from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.utils.data import backup_file from dimos.utils.logging_config import setup_logger @@ -263,10 +266,27 @@ class RecorderConfig(MemoryModuleConfig): default_frame_id: str = "base_link" tf_tolerance: float = 0.5 db_path: str | Path = "recording.db" + # Also record the live tf stream (under "tf") alongside the In ports. + record_tf: bool = True + + +PoseSetter = Callable[[Any], "Pose | None"] + + +def pose_setter_for(*stream_names: str) -> Callable[[Any], Any]: + """Mark a method ``(self, msg) -> Pose | None`` as the pose setter for the + given recorded stream(s). Streams without a setter fall back to the tf-based + ``world <- frame_id`` lookup.""" + + def decorate(fn: Any) -> Any: + fn._pose_setter_for = tuple(stream_names) + return fn + + return decorate class Recorder(MemoryModule): - """Records all ``In`` ports to a memory2 SQLite database. + """Records all ``In`` ports to a memory2 SQLite database, plus the live tf tree. Subclass with the topics you want to record:: @@ -275,10 +295,20 @@ class MyRecorder(Recorder): lidar: In[PointCloud2] blueprint.add(MyRecorder, db_path="session.db") + + Each stream's pose defaults to a ``world <- frame_id`` tf lookup; decorate a + method with ``@pose_setter_for("stream")`` to source it elsewhere (e.g. from + an odometry stream):: + + @pose_setter_for("lidar") + def _lidar_pose(self, msg): + return self._last_odom_pose """ config: RecorderConfig + _pose_setters: dict[str, Any] = {} + @rpc def start(self) -> None: super().start() @@ -289,6 +319,8 @@ def start(self) -> None: ) return + self._pose_setters = self._collect_pose_setters() + # TODO: store reset API/logic is not implemented yet. This module # shouldn't need to know about files (SqliteStore specific), and # .live() subs need to know how to re-sub in case of a restart of @@ -311,7 +343,7 @@ def start(self) -> None: self._prepare_streams() - if not self.inputs: + if not self.inputs and not self.config.record_tf: logger.warning("Recorder has no In ports — nothing to record, subclass the Recorder") return @@ -322,6 +354,9 @@ def start(self) -> None: "Recording %s -> %s (%s)", name, self._stream_name(name), port.type.__name__ ) + if self.config.record_tf: + self._record_tf() + def _port_to_stream(self, name: str, input_topic: In[Any], stream: Stream[Any]) -> None: """Append each message from *input_topic* to *stream*, attaching world pose via tf. @@ -360,10 +395,41 @@ def _resolve_ts(self, name: str, msg: Any) -> float: return getattr(msg, "ts", None) or time.time() def _resolve_pose(self, name: str, msg: Any, ts: float) -> Pose | None: - """Pose to anchor *msg* with. Default: world<-frame_id via tf. Override to - source poses elsewhere (e.g. from an odometry stream).""" + """Pose to anchor *msg* with. Dispatches to the stream's + ``@pose_setter_for`` if one is defined, else falls back to a + ``world <- frame_id`` tf lookup.""" + setter = self._pose_setters.get(name) + if setter is not None: + return setter(msg) frame_id = getattr(msg, "frame_id", None) or self.config.default_frame_id transform = self.tf.get( "world", frame_id, time_point=ts, time_tolerance=self.config.tf_tolerance ) return transform.to_pose() if transform is not None else None + + def _collect_pose_setters(self) -> dict[str, PoseSetter]: + """Map stream name -> bound ``@pose_setter_for`` method.""" + setters: dict[str, PoseSetter] = {} + for attr_name in dir(type(self)): + fn = getattr(type(self), attr_name, None) + for stream in getattr(fn, "_pose_setter_for", ()): + setters[stream] = getattr(self, attr_name) + return setters + + def _record_tf(self) -> None: + """Record the live tf stream under "tf".""" + topic = getattr(self.tf.config, "topic", None) + if not topic: + logger.warning("Recorder: no tf topic configured — not recording tf") + return + tf_stream = self.store.stream("tf", TFMessage) + + def on_tf(msg: TFMessage, _topic: Any) -> None: + try: + for transform in msg.transforms: + tf_stream.append(TFMessage(transform), ts=transform.ts, pose=None) + except sqlite3.ProgrammingError: + # A late LCM callback raced teardown and hit the closed store. + pass + + self.register_disposable(Disposable(self.tf.pubsub.subscribe(topic, on_tf))) diff --git a/dimos/memory2/tf_recorder.py b/dimos/memory2/tf_recorder.py deleted file mode 100644 index 3ea9f7ba26..0000000000 --- a/dimos/memory2/tf_recorder.py +++ /dev/null @@ -1,115 +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. - -"""A Recorder that also records the tf tree and lets subclasses set per-stream poses. - -Decorate a method with ``@pose_setter_for("stream")`` to control how a recorded -stream's pose is resolved; streams without a setter fall back to the base -Recorder's tf-based ``world <- frame_id`` lookup:: - - class MyRecorder(TfRecorder): - odometry: In[Odometry] - lidar: In[PointCloud2] - - @pose_setter_for("odometry") - def _odom_pose(self, msg): - self._last_pose = msg.pose.pose - return self._last_pose - - @pose_setter_for("lidar") - def _lidar_pose(self, msg): - return self._last_pose # stamp the cloud with the latest odom pose -""" - -from __future__ import annotations - -from collections.abc import Callable -import sqlite3 -from typing import Any - -from reactivex.disposable import Disposable - -from dimos.core.core import rpc -from dimos.memory2.module import Recorder, RecorderConfig -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - -PoseSetter = Callable[[Any], "Pose | None"] - - -def pose_setter_for(*stream_names: str) -> Callable[[Any], Any]: - """Mark a method ``(self, msg) -> Pose | None`` as the pose setter for the - given recorded stream(s).""" - - def decorate(fn: Any) -> Any: - fn._pose_setter_for = tuple(stream_names) - return fn - - return decorate - - -class TfRecorderConfig(RecorderConfig): - # Also record the live tf stream alongside the In ports. - record_tf: bool = True - - -class TfRecorder(Recorder): - config: TfRecorderConfig - - _pose_setters: dict[str, PoseSetter] = {} - - @rpc - def start(self) -> None: - self._pose_setters = self._collect_pose_setters() - super().start() - if self.config.g.replay: - return - if self.config.record_tf: - self._record_tf() - - def _collect_pose_setters(self) -> dict[str, PoseSetter]: - """Map stream name -> bound @pose_setter_for method.""" - setters: dict[str, PoseSetter] = {} - for attr_name in dir(type(self)): - fn = getattr(type(self), attr_name, None) - for stream in getattr(fn, "_pose_setter_for", ()): - setters[stream] = getattr(self, attr_name) - return setters - - def _resolve_pose(self, name: str, msg: Any, ts: float) -> Pose | None: - """Dispatch to the stream's @pose_setter_for, else the base tf lookup.""" - setter = self._pose_setters.get(name) - if setter is not None: - return setter(msg) - return super()._resolve_pose(name, msg, ts) - - def _record_tf(self) -> None: - topic = getattr(self.tf.config, "topic", None) - if not topic: - logger.warning("TfRecorder: no tf topic configured — not recording tf") - return - tf_stream = self.store.stream("tf", TFMessage) - - def on_tf(msg: TFMessage, _topic: Any) -> None: - try: - for transform in msg.transforms: - tf_stream.append(TFMessage(transform), ts=transform.ts, pose=None) - except sqlite3.ProgrammingError: - # A late LCM callback raced teardown and hit the closed store. - pass - - self.register_disposable(Disposable(self.tf.pubsub.subscribe(topic, on_tf))) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 0ba446a1fe..0b35c6e75e 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -236,7 +236,6 @@ "temporal-memory": "dimos.perception.experimental.temporal_memory.temporal_memory.TemporalMemory", "terrain-analysis": "dimos.navigation.nav_stack.modules.terrain_analysis.terrain_analysis.TerrainAnalysis", "terrain-map-ext": "dimos.navigation.nav_stack.modules.terrain_map_ext.terrain_map_ext.TerrainMapExt", - "tf-recorder": "dimos.memory2.tf_recorder.TfRecorder", "twist-teleop-module": "dimos.teleop.quest.quest_extensions.TwistTeleopModule", "unitree-g1-skill-container": "dimos.robot.unitree.g1.skill_container.UnitreeG1SkillContainer", "unitree-skill-container": "dimos.robot.unitree.unitree_skill_container.UnitreeSkillContainer", From 0ab7ab1d04de342c21e92fdc26e2fe97bc3ebb64 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 04:36:35 -0700 Subject: [PATCH 167/185] pointlio: drop YAML config, pass tuning as plain CLI args Mirror the fastlio no-yaml change: PointLioConfig tuning fields are passed to the binary as CLI args (read into a PointLioParams struct in main.cpp) instead of being rendered to a throwaway YAML read via --config_path. Removes the yaml-cpp dependency from the flake + CMake and the config-file plumbing from the module. Requires the matching dimos-module-fastlio2 pointlio-branch change (flake.lock bumped to 288e357). --- .../sensors/lidar/pointlio/cpp/CMakeLists.txt | 3 - .../sensors/lidar/pointlio/cpp/flake.lock | 6 +- .../sensors/lidar/pointlio/cpp/flake.nix | 1 - .../sensors/lidar/pointlio/cpp/main.cpp | 90 +++++++++++++-- .../hardware/sensors/lidar/pointlio/module.py | 109 ++---------------- 5 files changed, 95 insertions(+), 114 deletions(-) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt index 27ad294a3b..9bce7246f2 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt @@ -53,8 +53,6 @@ find_package(Eigen3 REQUIRED) # PCL (only components we need — avoid full PCL which drags in VTK via io) find_package(PCL 1.8 REQUIRED COMPONENTS common filters) -find_package(yaml-cpp REQUIRED) - # glog (iVox map backend — include/ivox/ivox3d.h needs glog) find_package(glog REQUIRED) @@ -94,7 +92,6 @@ target_link_libraries(pointlio_native PRIVATE ${LCM_LIBRARIES} ${LIVOX_SDK} ${PCL_LIBRARIES} - yaml-cpp::yaml-cpp glog::glog ) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock index 531b9d7a15..3cb06c2284 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock @@ -37,11 +37,11 @@ "fast-lio": { "flake": false, "locked": { - "lastModified": 1781514593, - "narHash": "sha256-85zc03F2Ztw/pO1nUtVHnC+4yGzVi5z9pKk9lZcFnE8=", + "lastModified": 1781782101, + "narHash": "sha256-2phOAdagFal8BTBEKxEbl3LDSx/7SNGVTFu0zYEXB1g=", "owner": "dimensionalOS", "repo": "dimos-module-fastlio2", - "rev": "286c530db7661ca6874cd8c1381357adf83cd19f", + "rev": "288e357e5457723c1cce4d4060f76ed7f85b10d4", "type": "github" }, "original": { diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix index 303a5aa093..0ef30ba768 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix @@ -89,7 +89,6 @@ pkgs.glib pkgs.eigen pkgs.pcl - pkgs.yaml-cpp pkgs.glog pkgs.boost pkgs.llvmPackages.openmp diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 8caef55333..36f6b3c403 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -11,7 +11,7 @@ // ./pointlio_native \ // --lidar '/lidar#sensor_msgs.PointCloud2' \ // --odometry '/odometry#nav_msgs.Odometry' \ -// --config_path /path/to/pointlio.yaml \ # generated by module.py from PointLioConfig +// --filter_size_surf 0.2 --ivox_grid_resolution 2.0 ... \ # tuning as plain CLI args // --host_ip 192.168.1.5 --lidar_ip 192.168.1.155 #include @@ -57,6 +57,23 @@ static double get_publish_ts() { return std::chrono::duration( std::chrono::system_clock::now().time_since_epoch()).count(); } +// Parse a comma-separated list of doubles (CLI vector args); empty on bad input. +static std::vector parse_doubles(const std::string& csv) { + std::vector out; + size_t i = 0; + while (i < csv.size()) { + size_t j = csv.find(',', i); + if (j == std::string::npos) j = csv.size(); + try { + out.push_back(std::stod(csv.substr(i, j - i))); + } catch (...) { + return {}; + } + i = j + 1; + } + return out; +} + static std::string g_lidar_topic; static std::string g_odometry_topic; static std::string g_frame_id; // required via --frame_id @@ -321,11 +338,68 @@ int main(int argc, char** argv) { return 1; } - std::string config_path = mod.arg("config_path", ""); - if (config_path.empty()) { - fprintf(stderr, "Error: --config_path is required\n"); - return 1; - } + // Point-LIO tuning, passed as CLI args by the dimos module (no YAML). + PointLioParams params; + // common + params.con_frame = mod.arg_bool("con_frame", params.con_frame); + params.con_frame_num = mod.arg_int("con_frame_num", params.con_frame_num); + params.cut_frame = mod.arg_bool("cut_frame", params.cut_frame); + params.cut_frame_time_interval = mod.arg_float("cut_frame_time_interval", params.cut_frame_time_interval); + params.time_lag_imu_to_lidar = mod.arg_float("time_lag_imu_to_lidar", params.time_lag_imu_to_lidar); + // preprocess + params.scan_line = mod.arg_int("scan_line", params.scan_line); + params.scan_rate = mod.arg_int("scan_rate", params.scan_rate); + params.blind = mod.arg_float("blind", params.blind); + params.point_filter_num = mod.arg_int("point_filter_num", params.point_filter_num); + std::string lidar_type = mod.arg("lidar_type", "avia"); + params.lidar_type = lidar_type == "velodyne" ? 2 : lidar_type == "ouster" ? 3 : + lidar_type == "hesai" ? 4 : lidar_type == "unilidar" ? 5 : 1; + std::string ts_unit = mod.arg("timestamp_unit", "nanosecond"); + params.timestamp_unit = ts_unit == "second" ? 0 : ts_unit == "millisecond" ? 1 : + ts_unit == "microsecond" ? 2 : 3; + // mapping + params.use_imu_as_input = mod.arg_bool("use_imu_as_input", params.use_imu_as_input); + params.prop_at_freq_of_imu = mod.arg_bool("prop_at_freq_of_imu", params.prop_at_freq_of_imu); + params.check_satu = mod.arg_bool("check_satu", params.check_satu); + params.init_map_size = mod.arg_int("init_map_size", params.init_map_size); + params.space_down_sample = mod.arg_bool("space_down_sample", params.space_down_sample); + params.satu_acc = mod.arg_float("satu_acc", params.satu_acc); + params.satu_gyro = mod.arg_float("satu_gyro", params.satu_gyro); + params.acc_norm = mod.arg_float("acc_norm", params.acc_norm); + params.plane_thr = mod.arg_float("plane_thr", params.plane_thr); + params.filter_size_surf = mod.arg_float("filter_size_surf", params.filter_size_surf); + params.filter_size_map = mod.arg_float("filter_size_map", params.filter_size_map); + params.ivox_grid_resolution = mod.arg_float("ivox_grid_resolution", params.ivox_grid_resolution); + std::string ivox_nearby = mod.arg("ivox_nearby_type", "nearby6"); + params.ivox_nearby_type = ivox_nearby == "center" ? 0 : ivox_nearby == "nearby18" ? 18 : + ivox_nearby == "nearby26" ? 26 : 6; + params.cube_side_length = mod.arg_float("cube_side_length", params.cube_side_length); + params.det_range = mod.arg_float("det_range", params.det_range); + params.fov_degree = mod.arg_float("fov_degree", params.fov_degree); + params.imu_en = mod.arg_bool("imu_en", params.imu_en); + params.start_in_aggressive_motion = mod.arg_bool("start_in_aggressive_motion", params.start_in_aggressive_motion); + params.extrinsic_est_en = mod.arg_bool("extrinsic_est_en", params.extrinsic_est_en); + params.imu_time_inte = mod.arg_float("imu_time_inte", params.imu_time_inte); + params.lidar_meas_cov = mod.arg_float("lidar_meas_cov", params.lidar_meas_cov); + params.acc_cov_input = mod.arg_float("acc_cov_input", params.acc_cov_input); + params.vel_cov = mod.arg_float("vel_cov", params.vel_cov); + params.gyr_cov_input = mod.arg_float("gyr_cov_input", params.gyr_cov_input); + params.gyr_cov_output = mod.arg_float("gyr_cov_output", params.gyr_cov_output); + params.acc_cov_output = mod.arg_float("acc_cov_output", params.acc_cov_output); + params.b_gyr_cov = mod.arg_float("b_gyr_cov", params.b_gyr_cov); + params.b_acc_cov = mod.arg_float("b_acc_cov", params.b_acc_cov); + params.imu_meas_acc_cov = mod.arg_float("imu_meas_acc_cov", params.imu_meas_acc_cov); + params.imu_meas_omg_cov = mod.arg_float("imu_meas_omg_cov", params.imu_meas_omg_cov); + params.match_s = mod.arg_float("match_s", params.match_s); + params.gravity_align = mod.arg_bool("gravity_align", params.gravity_align); + if (auto g = parse_doubles(mod.arg("gravity", "")); !g.empty()) params.gravity = g; + if (auto gi = parse_doubles(mod.arg("gravity_init", "")); !gi.empty()) params.gravity_init = gi; + if (auto et = parse_doubles(mod.arg("extrinsic_t", "")); !et.empty()) params.extrinsic_T = et; + if (auto er = parse_doubles(mod.arg("extrinsic_r", "")); !er.empty()) params.extrinsic_R = er; + // odometry + params.publish_odometry_without_downsample = + mod.arg_bool("publish_odometry_without_downsample", params.publish_odometry_without_downsample); + params.odom_only = mod.arg_bool("odom_only", params.odom_only); // Point-LIO internal processing rates double msr_freq = mod.arg_float("msr_freq", 50.0f); @@ -362,7 +436,7 @@ int main(int argc, char** argv) { printf("[pointlio] Starting Point-LIO + Livox Mid-360 native module\n"); printf("[pointlio] lidar topic: %s\n", g_lidar_topic.empty() ? "(disabled)" : g_lidar_topic.c_str()); printf("[pointlio] odometry topic: %s\n", g_odometry_topic.empty() ? "(disabled)" : g_odometry_topic.c_str()); - printf("[pointlio] config: %s\n", config_path.c_str()); + printf("[pointlio] tuning: filter_size_surf=%.3f ivox_res=%.3f lidar_type=%d\n", params.filter_size_surf, params.ivox_grid_resolution, params.lidar_type); printf("[pointlio] host_ip: %s lidar_ip: %s frequency: %.1f Hz\n", host_ip.c_str(), lidar_ip.c_str(), g_frequency); printf("[pointlio] pointcloud_freq: %.1f Hz odom_freq: %.1f Hz\n", pointcloud_freq, odom_freq); } @@ -378,7 +452,7 @@ int main(int argc, char** argv) { g_lcm = &lcm; if (debug) { printf("[pointlio] Initializing Point-LIO...\n"); } - PointLio point_lio(config_path, msr_freq, main_freq); + PointLio point_lio(params, msr_freq, main_freq); g_point_lio = &point_lio; if (debug) { printf("[pointlio] Point-LIO initialized.\n"); } diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index 01e6454947..328fdcbbe9 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -25,21 +25,17 @@ SomeConsumer.blueprint(), )).loop() -Point-LIO tuning lives directly on ``PointLioConfig`` (no YAML files). On -``start()`` the fields are rendered to a throwaway YAML that the C++ binary reads -via ``--config_path``. +Point-LIO tuning lives directly on ``PointLioConfig`` and is passed to the C++ +binary as plain CLI args (no YAML). """ from __future__ import annotations import os -from pathlib import Path -import tempfile -from typing import TYPE_CHECKING, Any, Literal +from typing import TYPE_CHECKING, Literal from pydantic import Field from reactivex.disposable import Disposable -import yaml from dimos.core.core import rpc from dimos.core.native_module import NativeModule, NativeModuleConfig @@ -65,72 +61,14 @@ from dimos.navigation.nav_stack.frames import FRAME_ODOM from dimos.spec import perception -# Point-LIO encodes these as ints/codes; expose human-readable names and translate. -# LID_TYPE enum (Point-LIO src/preprocess.h). 1 = AVIA selects the Livox branch +# Human-readable enums; the C++ binary (main.cpp) maps these strings to +# Point-LIO's int codes. +# LID_TYPE enum (Point-LIO src/preprocess.h). avia = 1 selects the Livox branch # the Mid-360 emits. LidarType = Literal["avia", "velodyne", "ouster", "hesai", "unilidar"] -_LIDAR_TYPE_CODE = {"avia": 1, "velodyne": 2, "ouster": 3, "hesai": 4, "unilidar": 5} - TimestampUnit = Literal["second", "millisecond", "microsecond", "nanosecond"] -_TIMESTAMP_UNIT_CODE = {"second": 0, "millisecond": 1, "microsecond": 2, "nanosecond": 3} - # iVox local-map neighbour stencil. IvoxNearbyType = Literal["center", "nearby6", "nearby18", "nearby26"] -_IVOX_NEARBY_CODE = {"center": 0, "nearby6": 6, "nearby18": 18, "nearby26": 26} - -# Field name -> Point-LIO YAML (section, key). Only these fields are rendered into -# the generated config; everything else on PointLioConfig is module plumbing. -_YAML_LAYOUT: dict[str, tuple[str, str]] = { - "con_frame": ("common", "con_frame"), - "con_frame_num": ("common", "con_frame_num"), - "cut_frame": ("common", "cut_frame"), - "cut_frame_time_interval": ("common", "cut_frame_time_interval"), - "time_lag_imu_to_lidar": ("common", "time_lag_imu_to_lidar"), - "lidar_type": ("preprocess", "lidar_type"), - "scan_line": ("preprocess", "scan_line"), - "scan_rate": ("preprocess", "scan_rate"), - "timestamp_unit": ("preprocess", "timestamp_unit"), - "blind": ("preprocess", "blind"), - "point_filter_num": ("preprocess", "point_filter_num"), - "use_imu_as_input": ("mapping", "use_imu_as_input"), - "prop_at_freq_of_imu": ("mapping", "prop_at_freq_of_imu"), - "check_satu": ("mapping", "check_satu"), - "init_map_size": ("mapping", "init_map_size"), - "space_down_sample": ("mapping", "space_down_sample"), - "satu_acc": ("mapping", "satu_acc"), - "satu_gyro": ("mapping", "satu_gyro"), - "acc_norm": ("mapping", "acc_norm"), - "plane_thr": ("mapping", "plane_thr"), - "filter_size_surf": ("mapping", "filter_size_surf"), - "filter_size_map": ("mapping", "filter_size_map"), - "ivox_grid_resolution": ("mapping", "ivox_grid_resolution"), - "ivox_nearby_type": ("mapping", "ivox_nearby_type"), - "cube_side_length": ("mapping", "cube_side_length"), - "det_range": ("mapping", "det_range"), - "fov_degree": ("mapping", "fov_degree"), - "imu_en": ("mapping", "imu_en"), - "start_in_aggressive_motion": ("mapping", "start_in_aggressive_motion"), - "extrinsic_est_en": ("mapping", "extrinsic_est_en"), - "imu_time_inte": ("mapping", "imu_time_inte"), - "lidar_meas_cov": ("mapping", "lidar_meas_cov"), - "acc_cov_input": ("mapping", "acc_cov_input"), - "vel_cov": ("mapping", "vel_cov"), - "gyr_cov_input": ("mapping", "gyr_cov_input"), - "gyr_cov_output": ("mapping", "gyr_cov_output"), - "acc_cov_output": ("mapping", "acc_cov_output"), - "b_gyr_cov": ("mapping", "b_gyr_cov"), - "b_acc_cov": ("mapping", "b_acc_cov"), - "imu_meas_acc_cov": ("mapping", "imu_meas_acc_cov"), - "imu_meas_omg_cov": ("mapping", "imu_meas_omg_cov"), - "match_s": ("mapping", "match_s"), - "gravity_align": ("mapping", "gravity_align"), - "gravity": ("mapping", "gravity"), - "gravity_init": ("mapping", "gravity_init"), - "extrinsic_t": ("mapping", "extrinsic_T"), - "extrinsic_r": ("mapping", "extrinsic_R"), - "publish_odometry_without_downsample": ("odometry", "publish_odometry_without_downsample"), - "odom_only": ("odometry", "odom_only"), -} class PointLioConfig(NativeModuleConfig): @@ -158,7 +96,7 @@ class PointLioConfig(NativeModuleConfig): debug: bool = False - # Point-LIO tuning, rendered to the generated YAML (see _YAML_LAYOUT). + # Point-LIO tuning, passed to the binary as plain CLI args (read in main.cpp). # common con_frame: bool = False con_frame_num: int = 1 @@ -227,22 +165,9 @@ class PointLioConfig(NativeModuleConfig): host_imu_data_port: int = SDK_HOST_IMU_DATA_PORT host_log_data_port: int = SDK_HOST_LOG_DATA_PORT - # Set in start() to the generated YAML; passed as --config_path to the binary. - config_path: str | None = None - - # Point-LIO tuning fields feed the generated YAML, not CLI args. - cli_exclude: frozenset[str] = frozenset({"body_start_frame_id", *_YAML_LAYOUT}) - - def render_config_yaml(self) -> str: - """Render the Point-LIO tuning fields to YAML text the C++ binary reads.""" - doc: dict[str, dict[str, Any]] = {} - for field, (section, key) in _YAML_LAYOUT.items(): - doc.setdefault(section, {})[key] = getattr(self, field) - # Enum-like strings -> Point-LIO int codes. - doc["preprocess"]["lidar_type"] = _LIDAR_TYPE_CODE[self.lidar_type] - doc["preprocess"]["timestamp_unit"] = _TIMESTAMP_UNIT_CODE[self.timestamp_unit] - doc["mapping"]["ivox_nearby_type"] = _IVOX_NEARBY_CODE[self.ivox_nearby_type] - return yaml.safe_dump(doc, sort_keys=False, default_flow_style=False) + # body_start_frame_id anchors the published TF in Python only; it's not a + # binary arg. Everything else (tuning included) renders to a CLI arg. + cli_exclude: frozenset[str] = frozenset({"body_start_frame_id"}) class PointLio(NativeModule, perception.Lidar, perception.Odometry): @@ -251,25 +176,14 @@ class PointLio(NativeModule, perception.Lidar, perception.Odometry): lidar: Out[PointCloud2] odometry: Out[Odometry] - _config_file: str | None = None - @rpc def start(self) -> None: self._validate_network() - self._write_config() super().start() self.register_disposable( Disposable(self.odometry.transport.subscribe(self._on_odom_for_tf, self.odometry)) ) - def _write_config(self) -> None: - """Render the config fields to a temp YAML and point the binary at it.""" - fd, path = tempfile.mkstemp(prefix="pointlio_", suffix=".yaml") - with os.fdopen(fd, "w") as f: - f.write(self.config.render_config_yaml()) - self._config_file = path - self.config.config_path = path - def _on_odom_for_tf(self, msg: Odometry) -> None: self.tf.publish( Transform( @@ -295,9 +209,6 @@ def _on_odom_for_tf(self, msg: Odometry) -> None: @rpc def stop(self) -> None: super().stop() - if self._config_file is not None: - Path(self._config_file).unlink(missing_ok=True) - self._config_file = None def _validate_network(self) -> None: lidar_ip = self.config.lidar_ip From fba90db29b7f5a86b69f4f6a45f967b51d9dd53d Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 04:36:45 -0700 Subject: [PATCH 168/185] memory2: add Recorder.stream_remapping config for per-stream renames MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit {port_name: db_stream_name} to control the recorded stream/table name without subclassing — conceptually what .remappings() expresses, but the active remappings aren't readily accessible from inside the module. --- dimos/memory2/module.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/dimos/memory2/module.py b/dimos/memory2/module.py index 3a542a6a38..f140b0dfdf 100644 --- a/dimos/memory2/module.py +++ b/dimos/memory2/module.py @@ -255,8 +255,6 @@ class OnExisting(str, enum.Enum): OVERWRITE = "overwrite" ERROR = "error" BACKUP = "backup" - # Leave the db untouched; subclasses replace only their own streams in - # ``_prepare_streams``. APPEND = "append" @@ -268,6 +266,11 @@ class RecorderConfig(MemoryModuleConfig): db_path: str | Path = "recording.db" # Also record the live tf stream (under "tf") alongside the In ports. record_tf: bool = True + # Rename recorded streams: {port_name: db_stream_name}. Conceptually this is + # what the wiring layer's .remappings() expresses, but there's no easy way to + # read the active remappings from inside the module (AFAIK), so this config + # arg does the per-stream rename directly. + stream_remapping: dict[str, str] = Field(default_factory=dict) PoseSetter = Callable[[Any], "Pose | None"] @@ -383,8 +386,9 @@ def on_msg(msg: Any) -> None: self.register_disposable(Disposable(input_topic.subscribe(on_msg))) def _stream_name(self, port_name: str) -> str: - """db stream/table name to record *port_name* under. Override to rename.""" - return port_name + """db stream/table name to record *port_name* under. Renamed via + ``config.stream_remapping``; override for fancier mapping.""" + return self.config.stream_remapping.get(port_name, port_name) def _prepare_streams(self) -> None: """Hook run after the on_existing check, before ports are wired. Override From 43d25f5ae547c91e856434676768d1152243c264 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 04:48:28 -0700 Subject: [PATCH 169/185] fastlio/pointlio: consistent frame_id scheme, stamp cloud with sensor frame MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Both modules now expose the same three frames: frame_id (fixed odom, the odometry header + TF parent), child_frame_id (moving body, the odometry child + TF child), and sensor_frame_id (the lidar's own frame, stamped on the published point cloud). get_body_cloud is the undistorted scan in the sensor frame, so the cloud was previously mislabeled — fastlio stamped it with the body frame and pointlio reused frame_id for both the cloud and the odometry header. pointlio's body_start_frame_id/body_frame_id are folded into this scheme. Drops a stale PGO comment. --- .../sensors/lidar/fastlio2/cpp/main.cpp | 6 +++-- .../hardware/sensors/lidar/fastlio2/module.py | 9 +++++--- .../sensors/lidar/pointlio/cpp/main.cpp | 10 +++++---- .../hardware/sensors/lidar/pointlio/module.py | 22 +++++++++---------- 4 files changed, 26 insertions(+), 21 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index 9a571a5a60..1437c9e9a6 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -65,6 +65,7 @@ static std::string g_lidar_topic; static std::string g_odometry_topic; static std::string g_frame_id; // required via --frame_id static std::string g_child_frame_id; // required via --child_frame_id +static std::string g_sensor_frame_id; // required via --sensor_frame_id static float g_frequency = 10.0f; // Frame accumulator (Livox SDK raw → CustomMsg) @@ -349,12 +350,12 @@ static void run_main_iter(std::chrono::steady_clock::time_point now, double ts = get_publish_ts(); if (scan_publish_en && !g_lidar_topic.empty() && now - last_pc_publish >= pc_interval) { - // Sensor/body-frame cloud; register downstream via the odom pose. + // Sensor-frame cloud; register downstream via the odom pose. // dense_publish_en false -> FAST-LIO's IESKF-downsampled scan. auto cloud = dense_publish_en ? fast_lio.get_body_cloud() : fast_lio.get_body_cloud_down(); if (cloud && !cloud->empty()) { - publish_lidar(cloud, ts, g_child_frame_id); + publish_lidar(cloud, ts, g_sensor_frame_id); } last_pc_publish = now; } @@ -414,6 +415,7 @@ int main(int argc, char** argv) { g_frequency = mod.arg_float("frequency", 10.0f); g_frame_id = mod.arg_required("frame_id"); g_child_frame_id = mod.arg_required("child_frame_id"); + g_sensor_frame_id = mod.arg_required("sensor_frame_id"); float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f); float odom_freq = mod.arg_float("odom_freq", 50.0f); diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 7d05e7af16..248082324e 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -52,7 +52,7 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.navigation.nav_stack.frames import FRAME_BODY, FRAME_ODOM +from dimos.navigation.nav_stack.frames import FRAME_BODY, FRAME_ODOM, FRAME_SENSOR from dimos.spec import perception # Human-readable enums; the C++ binary maps these strings to FAST-LIO's int codes. @@ -71,10 +71,13 @@ class FastLio2Config(NativeModuleConfig): lidar_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_FASTLIO_LIDAR_IP")) frequency: float = 10.0 - # "odom" frame: FastLio2 gives smooth continuous odometry; PGO publishes the - # map→odom correction via TF. + # Odometry is published as frame_id (fixed) -> child_frame_id (moving body), + # and also broadcast on TF. The point cloud is stamped with sensor_frame_id + # (the lidar's own frame — get_body_cloud is the undistorted scan, not yet + # transformed into the body frame). frame_id: str = FRAME_ODOM child_frame_id: str = FRAME_BODY + sensor_frame_id: str = FRAME_SENSOR # FAST-LIO internal processing rates msr_freq: float = 50.0 diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 36f6b3c403..b261f14992 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -5,7 +5,7 @@ // // Binds Livox SDK2 directly into the Point-LIO core: SDK callbacks feed // CustomMsg/Imu to the IESKF estimator, which performs LiDAR-inertial SLAM. -// Sensor-frame (mid360_link) point clouds and odometry are published on LCM. +// Sensor-frame point clouds and odometry are published on LCM. // // Usage: // ./pointlio_native \ @@ -78,6 +78,7 @@ static std::string g_lidar_topic; static std::string g_odometry_topic; static std::string g_frame_id; // required via --frame_id static std::string g_child_frame_id; // required via --child_frame_id +static std::string g_sensor_frame_id; // required via --sensor_frame_id static float g_frequency = 10.0f; // Frame accumulator (Livox SDK raw → CustomMsg) @@ -102,7 +103,7 @@ static uint64_t get_timestamp_ns(const LivoxLidarEthernetPacket* pkt) { using dimos::time_from_seconds; using dimos::make_header; -// Publish the lidar point cloud in the sensor body frame (g_frame_id). +// Publish the lidar point cloud in the sensor frame (g_sensor_frame_id). // `cloud` is Point-LIO's undistorted scan in the sensor's own frame // (get_body_cloud), so points are published as-is with no world registration. static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, const std::string& topic = "") { @@ -112,7 +113,7 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, const std int num_points = static_cast(cloud->size()); sensor_msgs::PointCloud2 pc; - pc.header = make_header(g_frame_id, timestamp); + pc.header = make_header(g_sensor_frame_id, timestamp); pc.height = 1; pc.width = num_points; pc.is_bigendian = 0; @@ -410,7 +411,8 @@ int main(int argc, char** argv) { std::string lidar_ip = mod.arg("lidar_ip", "192.168.1.155"); g_frequency = mod.arg_float("frequency", 10.0f); g_frame_id = mod.arg_required("frame_id"); - g_child_frame_id = mod.arg_required("body_frame_id"); + g_child_frame_id = mod.arg_required("child_frame_id"); + g_sensor_frame_id = mod.arg_required("sensor_frame_id"); float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f); float odom_freq = mod.arg_float("odom_freq", 50.0f); diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index 328fdcbbe9..a8dbe03b94 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -58,7 +58,7 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.navigation.nav_stack.frames import FRAME_ODOM +from dimos.navigation.nav_stack.frames import FRAME_BODY, FRAME_ODOM, FRAME_SENSOR from dimos.spec import perception # Human-readable enums; the C++ binary (main.cpp) maps these strings to @@ -81,11 +81,13 @@ class PointLioConfig(NativeModuleConfig): lidar_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_POINTLIO_LIDAR_IP")) frequency: float = 10.0 - # Sensor frame for the cloud + odometry headers. - frame_id: str = "mid360_link" - # Published TF: body_start_frame_id -> body_frame_id. - body_start_frame_id: str = FRAME_ODOM - body_frame_id: str = "base_link" + # Odometry is published as frame_id (fixed) -> child_frame_id (moving body), + # and also broadcast on TF. The point cloud is stamped with sensor_frame_id + # (the lidar's own frame — get_body_cloud is the undistorted scan, not yet + # transformed into the body frame). + frame_id: str = FRAME_ODOM + child_frame_id: str = FRAME_BODY + sensor_frame_id: str = FRAME_SENSOR # Point-LIO internal processing rates (Hz) msr_freq: float = 50.0 @@ -165,10 +167,6 @@ class PointLioConfig(NativeModuleConfig): host_imu_data_port: int = SDK_HOST_IMU_DATA_PORT host_log_data_port: int = SDK_HOST_LOG_DATA_PORT - # body_start_frame_id anchors the published TF in Python only; it's not a - # binary arg. Everything else (tuning included) renders to a CLI arg. - cli_exclude: frozenset[str] = frozenset({"body_start_frame_id"}) - class PointLio(NativeModule, perception.Lidar, perception.Odometry): config: PointLioConfig @@ -187,8 +185,8 @@ def start(self) -> None: def _on_odom_for_tf(self, msg: Odometry) -> None: self.tf.publish( Transform( - frame_id=self.config.body_start_frame_id, - child_frame_id=self.config.body_frame_id, + frame_id=self.frame_id, + child_frame_id=self.config.child_frame_id, translation=Vector3( msg.pose.position.x, msg.pose.position.y, From 33c70415f8c8c4b33a039becbf2be695b8ba45e8 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 05:11:04 -0700 Subject: [PATCH 170/185] fastlio/pointlio: default sensor_frame_id to mid360_link Use the lidar's concrete frame name rather than the generic FRAME_SENSOR. --- dimos/hardware/sensors/lidar/fastlio2/module.py | 4 ++-- dimos/hardware/sensors/lidar/pointlio/module.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index 248082324e..32150a6eb2 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -52,7 +52,7 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.navigation.nav_stack.frames import FRAME_BODY, FRAME_ODOM, FRAME_SENSOR +from dimos.navigation.nav_stack.frames import FRAME_BODY, FRAME_ODOM from dimos.spec import perception # Human-readable enums; the C++ binary maps these strings to FAST-LIO's int codes. @@ -77,7 +77,7 @@ class FastLio2Config(NativeModuleConfig): # transformed into the body frame). frame_id: str = FRAME_ODOM child_frame_id: str = FRAME_BODY - sensor_frame_id: str = FRAME_SENSOR + sensor_frame_id: str = "mid360_link" # FAST-LIO internal processing rates msr_freq: float = 50.0 diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index a8dbe03b94..679d678b18 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -58,7 +58,7 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.navigation.nav_stack.frames import FRAME_BODY, FRAME_ODOM, FRAME_SENSOR +from dimos.navigation.nav_stack.frames import FRAME_BODY, FRAME_ODOM from dimos.spec import perception # Human-readable enums; the C++ binary (main.cpp) maps these strings to @@ -87,7 +87,7 @@ class PointLioConfig(NativeModuleConfig): # transformed into the body frame). frame_id: str = FRAME_ODOM child_frame_id: str = FRAME_BODY - sensor_frame_id: str = FRAME_SENSOR + sensor_frame_id: str = "mid360_link" # Point-LIO internal processing rates (Hz) msr_freq: float = 50.0 From c76339de71d3525889c290d828dd3538ec916074 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 06:42:05 -0700 Subject: [PATCH 171/185] memory2/recorders: fix APPEND re-run duplicating tf + ignoring stream_remapping Move the replace-on-append logic into the base Recorder so it drops exactly the streams it is about to rewrite: the remapped In-port streams (via _stream_name) AND the tf stream. Previously each recorder's _prepare_streams deleted only the raw In-port names, so re-running into an existing db appended a second full copy of the tf tree and would orphan remapped streams. Also guard _record_tf for non-pubsub tf backends, and drop the now-dead world/global_map_fastlio rerun overrides (FastLio2's global_map port was removed in this PR). --- .../sensors/lidar/fastlio2/recorder.py | 6 ------ .../sensors/lidar/pointlio/recorder.py | 6 ------ dimos/memory2/module.py | 21 +++++++++++++------ dimos/navigation/nav_stack/main.py | 1 - .../g1/blueprints/primitive/unitree_g1_vis.py | 1 - 5 files changed, 15 insertions(+), 20 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/recorder.py b/dimos/hardware/sensors/lidar/fastlio2/recorder.py index c5df2b961b..08784cfe89 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/recorder.py +++ b/dimos/hardware/sensors/lidar/fastlio2/recorder.py @@ -44,12 +44,6 @@ class FastLio2Recorder(Recorder): _last_odom_pose: Pose | None = None - def _prepare_streams(self) -> None: - # Replace our own streams (keep anything else in the db). - for name in self.inputs: - if name in self.store.list_streams(): - self.store.delete_stream(name) - @pose_setter_for("fastlio_odometry") def _odom_pose(self, msg: Odometry) -> Pose | None: pose = getattr(msg, "pose", None) diff --git a/dimos/hardware/sensors/lidar/pointlio/recorder.py b/dimos/hardware/sensors/lidar/pointlio/recorder.py index 741b091c2c..ba9a3230f6 100644 --- a/dimos/hardware/sensors/lidar/pointlio/recorder.py +++ b/dimos/hardware/sensors/lidar/pointlio/recorder.py @@ -44,12 +44,6 @@ class PointlioRecorder(Recorder): _last_odom_pose: Pose | None = None - def _prepare_streams(self) -> None: - # Replace our own streams (keep anything else in the db). - for name in self.inputs: - if name in self.store.list_streams(): - self.store.delete_stream(name) - @pose_setter_for("pointlio_odometry") def _odom_pose(self, msg: Odometry) -> Pose | None: pose = getattr(msg, "pose", None) diff --git a/dimos/memory2/module.py b/dimos/memory2/module.py index f140b0dfdf..1d162e2f72 100644 --- a/dimos/memory2/module.py +++ b/dimos/memory2/module.py @@ -391,8 +391,16 @@ def _stream_name(self, port_name: str) -> str: return self.config.stream_remapping.get(port_name, port_name) def _prepare_streams(self) -> None: - """Hook run after the on_existing check, before ports are wired. Override - to replace specific streams when appending into a populated db.""" + """On APPEND, drop the streams this recorder is about to (re)write — the + remapped In-port streams plus ``tf`` — so a re-run replaces them instead + of duplicating, while leaving any other streams in the db untouched.""" + if self.config.on_existing is not OnExisting.APPEND: + return + targets = {self._stream_name(name) for name in self.inputs} + if self.config.record_tf: + targets.add("tf") + for stream in targets.intersection(self.store.list_streams()): + self.store.delete_stream(stream) def _resolve_ts(self, name: str, msg: Any) -> float: """Timestamp to record *msg* at. Override to re-base onto another clock.""" @@ -421,10 +429,11 @@ def _collect_pose_setters(self) -> dict[str, PoseSetter]: return setters def _record_tf(self) -> None: - """Record the live tf stream under "tf".""" + """Record the live tf stream under "tf" (no-op without a pubsub tf).""" topic = getattr(self.tf.config, "topic", None) - if not topic: - logger.warning("Recorder: no tf topic configured — not recording tf") + pubsub = getattr(self.tf, "pubsub", None) + if not topic or pubsub is None: + logger.warning("Recorder: no pubsub tf available — not recording tf") return tf_stream = self.store.stream("tf", TFMessage) @@ -436,4 +445,4 @@ def on_tf(msg: TFMessage, _topic: Any) -> None: # A late LCM callback raced teardown and hit the closed store. pass - self.register_disposable(Disposable(self.tf.pubsub.subscribe(topic, on_tf))) + self.register_disposable(Disposable(pubsub.subscribe(topic, on_tf))) diff --git a/dimos/navigation/nav_stack/main.py b/dimos/navigation/nav_stack/main.py index 4729233507..8d3392a55e 100644 --- a/dimos/navigation/nav_stack/main.py +++ b/dimos/navigation/nav_stack/main.py @@ -217,7 +217,6 @@ def nav_stack_rerun_config( visual_override.setdefault("world/terrain_map_ext", _terrain_map_colors) visual_override.setdefault("world/global_map", _global_map_colors) visual_override.setdefault("world/global_map_pgo", _global_map_colors) - visual_override.setdefault("world/global_map_fastlio", _global_map_colors) visual_override.setdefault( "world/registered_scan", _registered_scan_colors if show_registered_scan else _hide ) diff --git a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_vis.py b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_vis.py index 64dec5e4bb..bdbe24b7ae 100644 --- a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_vis.py +++ b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_vis.py @@ -47,7 +47,6 @@ def _g1_path_colors(path: Path) -> Any: "world/odometry": g1_odometry_tf_override, "world/lidar": None, "world/local_map": None, - "world/global_map_fastlio": None, "world/global_costmap": g1_costmap, "world/path": _g1_path_colors, }, From 1e3e9b11527e4f6c5694a69079d084dcfce14b80 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 09:30:50 -0700 Subject: [PATCH 172/185] fastlio main.cpp: match pointlio formatting; inline get_publish_ts + _stream_name Braces on every if/for/while (inline single-statement bodies), collapse the awkwardly-wrapped calls/signatures to one line, and break the long run_main_iter def/call with the closing paren on its own line. Inline get_publish_ts (and the Recorder._stream_name helper, now config.stream_remapping.get at the use sites); add the missing braces to pointlio's parse_doubles too. --- .../sensors/lidar/fastlio2/cpp/main.cpp | 137 ++++++++---------- .../sensors/lidar/pointlio/cpp/main.cpp | 2 +- dimos/memory2/module.py | 14 +- 3 files changed, 66 insertions(+), 87 deletions(-) diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index 1437c9e9a6..94294ec312 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -56,11 +56,6 @@ static std::atomic g_running{true}; static lcm::LCM* g_lcm = nullptr; static FastLio* g_fastlio = nullptr; -static double get_publish_ts() { - return std::chrono::duration( - std::chrono::system_clock::now().time_since_epoch()).count(); -} - static std::string g_lidar_topic; static std::string g_odometry_topic; static std::string g_frame_id; // required via --frame_id @@ -91,7 +86,7 @@ static std::vector parse_doubles(const std::string& csv) { size_t i = 0; while (i < csv.size()) { size_t j = csv.find(',', i); - if (j == std::string::npos) j = csv.size(); + if (j == std::string::npos) { j = csv.size(); } try { out.push_back(std::stod(csv.substr(i, j - i))); } catch (...) { @@ -104,10 +99,9 @@ static std::vector parse_doubles(const std::string& csv) { // Publish a lidar point cloud, stamped with `frame_id`. -static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, - const std::string& frame_id, const std::string& topic = "") { +static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, const std::string& frame_id, const std::string& topic = "") { const std::string& chan = topic.empty() ? g_lidar_topic : topic; - if (!g_lcm || !cloud || cloud->empty() || chan.empty()) return; + if (!g_lcm || !cloud || cloud->empty() || chan.empty()) { return; } int num_points = static_cast(cloud->size()); @@ -156,7 +150,7 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, // Publish odometry static void publish_odometry(const custom_messages::Odometry& odom, double timestamp) { - if (!g_lcm) return; + if (!g_lcm) { return; } nav_msgs::Odometry msg; msg.header = make_header(g_frame_id, timestamp); @@ -190,9 +184,8 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times // Livox SDK callbacks -static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/, - LivoxLidarEthernetPacket* data, void* /*client_data*/) { - if (!g_running.load() || data == nullptr) return; +static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/, LivoxLidarEthernetPacket* data, void* /*client_data*/) { + if (!g_running.load() || data == nullptr) { return; } uint64_t ts_ns = get_timestamp_ns(data); uint16_t dot_num = data->dot_num; @@ -233,9 +226,8 @@ static void on_point_cloud(const uint32_t /*handle*/, const uint8_t /*dev_type*/ } } -static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, - LivoxLidarEthernetPacket* data, void* /*client_data*/) { - if (!g_running.load() || data == nullptr || !g_fastlio) return; +static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, LivoxLidarEthernetPacket* data, void* /*client_data*/) { + if (!g_running.load() || data == nullptr || !g_fastlio) { return; } uint64_t pkt_ts_ns = get_timestamp_ns(data); @@ -253,28 +245,24 @@ static void on_imu_data(const uint32_t /*handle*/, const uint8_t /*dev_type*/, imu_msg->orientation.y = 0.0; imu_msg->orientation.z = 0.0; imu_msg->orientation.w = 1.0; - for (int j = 0; j < 9; ++j) - imu_msg->orientation_covariance[j] = 0.0; + for (int j = 0; j < 9; ++j) { imu_msg->orientation_covariance[j] = 0.0; } imu_msg->angular_velocity.x = static_cast(imu_pts[i].gyro_x); imu_msg->angular_velocity.y = static_cast(imu_pts[i].gyro_y); imu_msg->angular_velocity.z = static_cast(imu_pts[i].gyro_z); - for (int j = 0; j < 9; ++j) - imu_msg->angular_velocity_covariance[j] = 0.0; + for (int j = 0; j < 9; ++j) { imu_msg->angular_velocity_covariance[j] = 0.0; } imu_msg->linear_acceleration.x = static_cast(imu_pts[i].acc_x) * GRAVITY_MS2; imu_msg->linear_acceleration.y = static_cast(imu_pts[i].acc_y) * GRAVITY_MS2; imu_msg->linear_acceleration.z = static_cast(imu_pts[i].acc_z) * GRAVITY_MS2; - for (int j = 0; j < 9; ++j) - imu_msg->linear_acceleration_covariance[j] = 0.0; + for (int j = 0; j < 9; ++j) { imu_msg->linear_acceleration_covariance[j] = 0.0; } g_fastlio->feed_imu(imu_msg); } } -static void on_info_change(const uint32_t handle, const LivoxLidarInfo* info, - void* /*client_data*/) { - if (info == nullptr) return; +static void on_info_change(const uint32_t handle, const LivoxLidarInfo* info, void* /*client_data*/) { + if (info == nullptr) { return; } char sn[17] = {}; std::memcpy(sn, info->sn, 16); @@ -282,8 +270,7 @@ static void on_info_change(const uint32_t handle, const LivoxLidarInfo* info, std::memcpy(ip, info->lidar_ip, 16); if (fastlio_debug) { - printf("[fastlio2] Device connected: handle=%u type=%u sn=%s ip=%s\n", - handle, info->dev_type, sn, ip); + printf("[fastlio2] Device connected: handle=%u type=%u sn=%s ip=%s\n", handle, info->dev_type, sn, ip); } SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, nullptr, nullptr); @@ -300,15 +287,18 @@ static void signal_handler(int /*sig*/) { // One iteration of the main loop: drain accumulated points into a CustomMsg, // run a FAST-LIO step, and publish results (rate-limited by the bookmarks). -static void run_main_iter(std::chrono::steady_clock::time_point now, - FastLio& fast_lio, - std::chrono::steady_clock::time_point& last_emit, - std::chrono::steady_clock::time_point& last_pc_publish, - std::chrono::steady_clock::time_point& last_odom_publish, - std::chrono::microseconds frame_interval, - std::chrono::microseconds pc_interval, - std::chrono::microseconds odom_interval, - bool scan_publish_en, bool dense_publish_en) { +static void run_main_iter( + std::chrono::steady_clock::time_point now, + FastLio& fast_lio, + std::chrono::steady_clock::time_point& last_emit, + std::chrono::steady_clock::time_point& last_pc_publish, + std::chrono::steady_clock::time_point& last_odom_publish, + std::chrono::microseconds frame_interval, + std::chrono::microseconds pc_interval, + std::chrono::microseconds odom_interval, + bool scan_publish_en, + bool dense_publish_en +) { // At frame rate, drain accumulated raw points into a CustomMsg and feed // FAST-LIO. Hold g_pc_mutex across the rate-limit check + swap so a // callback can't slip a packet in between the decision and the swap. @@ -329,12 +319,11 @@ static void run_main_iter(std::chrono::steady_clock::time_point now, // Build CustomMsg auto lidar_msg = boost::make_shared(); lidar_msg->header.seq = 0; - lidar_msg->header.stamp = custom_messages::Time().fromSec( - static_cast(frame_start) / 1e9); + lidar_msg->header.stamp = custom_messages::Time().fromSec(static_cast(frame_start) / 1e9); lidar_msg->header.frame_id = "livox_frame"; lidar_msg->timebase = frame_start; lidar_msg->lidar_id = 0; - for (int i = 0; i < 3; i++) lidar_msg->rsvd[i] = 0; + for (int i = 0; i < 3; i++) { lidar_msg->rsvd[i] = 0; } lidar_msg->point_num = static_cast(points.size()); lidar_msg->points = std::move(points); fast_lio.feed_lidar(lidar_msg); @@ -347,13 +336,11 @@ static void run_main_iter(std::chrono::steady_clock::time_point now, // Check for new SLAM results and publish (rate-limited). auto pose = fast_lio.get_pose(); if (!pose.empty() && (pose[0] != 0.0 || pose[1] != 0.0 || pose[2] != 0.0)) { - double ts = get_publish_ts(); - if (scan_publish_en && !g_lidar_topic.empty() - && now - last_pc_publish >= pc_interval) { + double ts = std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count(); + if (scan_publish_en && !g_lidar_topic.empty() && now - last_pc_publish >= pc_interval) { // Sensor-frame cloud; register downstream via the odom pose. // dense_publish_en false -> FAST-LIO's IESKF-downsampled scan. - auto cloud = dense_publish_en ? fast_lio.get_body_cloud() - : fast_lio.get_body_cloud_down(); + auto cloud = dense_publish_en ? fast_lio.get_body_cloud() : fast_lio.get_body_cloud_down(); if (cloud && !cloud->empty()) { publish_lidar(cloud, ts, g_sensor_frame_id); } @@ -390,8 +377,7 @@ int main(int argc, char** argv) { params.filter_size_map = mod.arg_float("filter_size_map", params.filter_size_map); params.det_range = mod.arg_float("det_range", params.det_range); params.blind = mod.arg_float("blind", params.blind); - params.time_offset_lidar_to_imu = - mod.arg_float("time_offset_lidar_to_imu", params.time_offset_lidar_to_imu); + params.time_offset_lidar_to_imu = mod.arg_float("time_offset_lidar_to_imu", params.time_offset_lidar_to_imu); params.fov_degree = mod.arg_int("fov_degree", params.fov_degree); params.scan_line = mod.arg_int("scan_line", params.scan_line); params.scan_rate = mod.arg_int("scan_rate", params.scan_rate); @@ -400,10 +386,9 @@ int main(int argc, char** argv) { std::string lidar_type = mod.arg("lidar_type", "livox"); params.lidar_type = lidar_type == "velodyne" ? 2 : lidar_type == "ouster" ? 3 : 1; std::string ts_unit = mod.arg("timestamp_unit", "microsecond"); - params.timestamp_unit = - ts_unit == "second" ? 0 : ts_unit == "millisecond" ? 1 : ts_unit == "nanosecond" ? 3 : 2; - if (auto et = parse_doubles(mod.arg("extrinsic_t", "")); !et.empty()) params.extrinsic_T = et; - if (auto er = parse_doubles(mod.arg("extrinsic_r", "")); !er.empty()) params.extrinsic_R = er; + params.timestamp_unit = ts_unit == "second" ? 0 : ts_unit == "millisecond" ? 1 : ts_unit == "nanosecond" ? 3 : 2; + if (auto et = parse_doubles(mod.arg("extrinsic_t", "")); !et.empty()) { params.extrinsic_T = et; } + if (auto er = parse_doubles(mod.arg("extrinsic_r", "")); !er.empty()) { params.extrinsic_R = er; } // FAST-LIO internal processing rates double msr_freq = mod.arg_float("msr_freq", 50.0f); @@ -445,16 +430,11 @@ int main(int argc, char** argv) { if (debug) { printf("[fastlio2] Starting FAST-LIO2 + Livox Mid-360 native module\n"); - printf("[fastlio2] lidar topic: %s\n", - g_lidar_topic.empty() ? "(disabled)" : g_lidar_topic.c_str()); - printf("[fastlio2] odometry topic: %s\n", - g_odometry_topic.empty() ? "(disabled)" : g_odometry_topic.c_str()); - printf("[fastlio2] acc_cov: %.3f filter_size_surf: %.3f\n", - params.acc_cov, params.filter_size_surf); - printf("[fastlio2] host_ip: %s lidar_ip: %s frequency: %.1f Hz\n", - host_ip.c_str(), lidar_ip.c_str(), g_frequency); - printf("[fastlio2] pointcloud_freq: %.1f Hz odom_freq: %.1f Hz\n", - pointcloud_freq, odom_freq); + printf("[fastlio2] lidar topic: %s\n", g_lidar_topic.empty() ? "(disabled)" : g_lidar_topic.c_str()); + printf("[fastlio2] odometry topic: %s\n", g_odometry_topic.empty() ? "(disabled)" : g_odometry_topic.c_str()); + printf("[fastlio2] acc_cov: %.3f filter_size_surf: %.3f\n", params.acc_cov, params.filter_size_surf); + printf("[fastlio2] host_ip: %s lidar_ip: %s frequency: %.1f Hz\n", host_ip.c_str(), lidar_ip.c_str(), g_frequency); + printf("[fastlio2] pointcloud_freq: %.1f Hz odom_freq: %.1f Hz\n", pointcloud_freq, odom_freq); } // Signal handlers @@ -470,20 +450,17 @@ int main(int argc, char** argv) { g_lcm = &lcm; // Init FAST-LIO with config - if (debug) printf("[fastlio2] Initializing FAST-LIO...\n"); + if (debug) { printf("[fastlio2] Initializing FAST-LIO...\n"); } FastLio fast_lio(params, msr_freq, main_freq); g_fastlio = &fast_lio; - if (debug) printf("[fastlio2] FAST-LIO initialized.\n"); + if (debug) { printf("[fastlio2] FAST-LIO initialized.\n"); } // Main-loop rate-limit state (consumed by the loop below). - auto frame_interval = std::chrono::microseconds( - static_cast(1e6 / g_frequency)); + auto frame_interval = std::chrono::microseconds(static_cast(1e6 / g_frequency)); const double process_period_ms = 1000.0 / main_freq; - auto pc_interval = std::chrono::microseconds( - static_cast(1e6 / pointcloud_freq)); - auto odom_interval = std::chrono::microseconds( - static_cast(1e6 / odom_freq)); + auto pc_interval = std::chrono::microseconds(static_cast(1e6 / pointcloud_freq)); + auto odom_interval = std::chrono::microseconds(static_cast(1e6 / odom_freq)); // Rate-limit bookmarks, seeded to now so they don't all fire on iteration 1. auto last_emit = std::chrono::steady_clock::now(); @@ -503,14 +480,23 @@ int main(int argc, char** argv) { LivoxLidarSdkUninit(); return 1; } - if (debug) printf("[fastlio2] SDK started, waiting for device...\n"); + if (debug) { printf("[fastlio2] SDK started, waiting for device...\n"); } while (g_running.load()) { auto loop_start = std::chrono::high_resolution_clock::now(); auto now = std::chrono::steady_clock::now(); - run_main_iter(now, fast_lio, last_emit, last_pc_publish, last_odom_publish, - frame_interval, pc_interval, odom_interval, - scan_publish_en, dense_publish_en); + run_main_iter( + now, + fast_lio, + last_emit, + last_pc_publish, + last_odom_publish, + frame_interval, + pc_interval, + odom_interval, + scan_publish_en, + dense_publish_en + ); // Drain LCM messages. lcm.handleTimeout(0); @@ -519,19 +505,18 @@ int main(int argc, char** argv) { auto loop_end = std::chrono::high_resolution_clock::now(); auto elapsed_ms = std::chrono::duration(loop_end - loop_start).count(); if (elapsed_ms < process_period_ms) { - std::this_thread::sleep_for(std::chrono::microseconds( - static_cast((process_period_ms - elapsed_ms) * 1000))); + std::this_thread::sleep_for(std::chrono::microseconds(static_cast((process_period_ms - elapsed_ms) * 1000))); } } // Cleanup. Uninit the SDK (stops + joins its callback threads) BEFORE // clearing the globals the callbacks read, so a late on_point/on_imu can't // race the assignment and dereference a null g_fastlio / g_lcm. - if (debug) printf("[fastlio2] Shutting down...\n"); + if (debug) { printf("[fastlio2] Shutting down...\n"); } LivoxLidarSdkUninit(); g_fastlio = nullptr; g_lcm = nullptr; - if (debug) printf("[fastlio2] Done.\n"); + if (debug) { printf("[fastlio2] Done.\n"); } return 0; } diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index b261f14992..e3d53482ef 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -63,7 +63,7 @@ static std::vector parse_doubles(const std::string& csv) { size_t i = 0; while (i < csv.size()) { size_t j = csv.find(',', i); - if (j == std::string::npos) j = csv.size(); + if (j == std::string::npos) { j = csv.size(); } try { out.push_back(std::stod(csv.substr(i, j - i))); } catch (...) { diff --git a/dimos/memory2/module.py b/dimos/memory2/module.py index 1d162e2f72..416c2d77fb 100644 --- a/dimos/memory2/module.py +++ b/dimos/memory2/module.py @@ -351,11 +351,10 @@ def start(self) -> None: return for name, port in self.inputs.items(): - stream: Stream[Any] = self.store.stream(self._stream_name(name), port.type) + stream_name = self.config.stream_remapping.get(name, name) + stream: Stream[Any] = self.store.stream(stream_name, port.type) self._port_to_stream(name, port, stream) - logger.info( - "Recording %s -> %s (%s)", name, self._stream_name(name), port.type.__name__ - ) + logger.info("Recording %s -> %s (%s)", name, stream_name, port.type.__name__) if self.config.record_tf: self._record_tf() @@ -385,18 +384,13 @@ def on_msg(msg: Any) -> None: self.register_disposable(Disposable(input_topic.subscribe(on_msg))) - def _stream_name(self, port_name: str) -> str: - """db stream/table name to record *port_name* under. Renamed via - ``config.stream_remapping``; override for fancier mapping.""" - return self.config.stream_remapping.get(port_name, port_name) - def _prepare_streams(self) -> None: """On APPEND, drop the streams this recorder is about to (re)write — the remapped In-port streams plus ``tf`` — so a re-run replaces them instead of duplicating, while leaving any other streams in the db untouched.""" if self.config.on_existing is not OnExisting.APPEND: return - targets = {self._stream_name(name) for name in self.inputs} + targets = {self.config.stream_remapping.get(name, name) for name in self.inputs} if self.config.record_tf: targets.add("tf") for stream in targets.intersection(self.store.list_streams()): From c5a6ab944f61997a9e270589c7961fe69c098ff8 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 10:30:32 -0700 Subject: [PATCH 173/185] memory2: fix mypy no-any-return in Recorder._resolve_pose The pose-setter dict is typed dict[str, Any] (to avoid evaluating the Pose forward-ref at class-definition time), so cast its result back to Pose | None. --- dimos/memory2/module.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/memory2/module.py b/dimos/memory2/module.py index 416c2d77fb..600f82a592 100644 --- a/dimos/memory2/module.py +++ b/dimos/memory2/module.py @@ -406,7 +406,7 @@ def _resolve_pose(self, name: str, msg: Any, ts: float) -> Pose | None: ``world <- frame_id`` tf lookup.""" setter = self._pose_setters.get(name) if setter is not None: - return setter(msg) + return cast("Pose | None", setter(msg)) frame_id = getattr(msg, "frame_id", None) or self.config.default_frame_id transform = self.tf.get( "world", frame_id, time_point=ts, time_tolerance=self.config.tf_tolerance From 02600309d2939ffae79259eae30778cf5ac7524a Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 13:48:50 -0700 Subject: [PATCH 174/185] pointlio: revert module to origin/main (keep PR #2498 fastlio-only) The pointlio improvements (no-yaml config, Recorder-subclass rework, frame_id scheme) move to jeff/feat/pointlio_native via merge; this PR carries only the fastlio2 work + shared memory2 Recorder base. --- .../lidar/pointlio/config/default.yaml | 69 +++++++ .../sensors/lidar/pointlio/cpp/CMakeLists.txt | 3 + .../sensors/lidar/pointlio/cpp/flake.lock | 6 +- .../sensors/lidar/pointlio/cpp/flake.nix | 1 + .../sensors/lidar/pointlio/cpp/main.cpp | 100 ++-------- .../hardware/sensors/lidar/pointlio/module.py | 109 +++-------- .../sensors/lidar/pointlio/recorder.py | 173 +++++++++++++++--- .../lidar/pointlio/scripts/pcap_to_db.py | 122 ++++++------ 8 files changed, 330 insertions(+), 253 deletions(-) create mode 100644 dimos/hardware/sensors/lidar/pointlio/config/default.yaml diff --git a/dimos/hardware/sensors/lidar/pointlio/config/default.yaml b/dimos/hardware/sensors/lidar/pointlio/config/default.yaml new file mode 100644 index 0000000000..1fd09ec9fa --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/config/default.yaml @@ -0,0 +1,69 @@ +# Point-LIO config. Mid-360-specific values to retune for a different sensor: +# preprocess.lidar_type (Livox), blind/scan_line, mapping.extrinsic_T/R (Mid-360 +# IMU->lidar mount), det_range, fov_degree. +common: + con_frame: false + con_frame_num: 1 + cut_frame: false + cut_frame_time_interval: 0.1 + time_lag_imu_to_lidar: 0.0 + +preprocess: + # LID_TYPE enum (Point-LIO src/preprocess.h): + # 1 = AVIA (Livox), 2 = VELO16, 3 = OUST64, 4 = HESAIxt32, 5 = UNILIDAR + # 1 selects the Livox branch (preprocess.cpp avia_handler), which expects the + # Livox CustomMsg point layout the Mid-360 emits: + # https://github.com/Livox-SDK/livox_ros_driver2/blob/master/msg/CustomMsg.msg + lidar_type: 1 + scan_line: 4 + scan_rate: 10 + timestamp_unit: 3 # 3 = nanosecond + blind: 0.5 + # Pre-KF input decimation: keep every Nth raw point. 1 = keep all (disable). + point_filter_num: 3 + +mapping: + use_imu_as_input: false # false = IMU-as-output model (Point-LIO's robust path) + prop_at_freq_of_imu: true + check_satu: true + init_map_size: 10 + # Pre-KF voxel downsample of each scan before the filter. false = feed the + # full scan (disable). Leaf size is filter_size_surf below. + space_down_sample: true + satu_acc: 3.0 # g; accel >= this is treated as saturated (residual zeroed) to bound velocity + satu_gyro: 35.0 + acc_norm: 1.0 # IMU accel unit: g + plane_thr: 0.1 + filter_size_surf: 0.2 # pre-KF scan downsample leaf size (m), used iff space_down_sample + filter_size_map: 0.5 + ivox_grid_resolution: 2.0 # iVox local-map grid (m) + ivox_nearby_type: 6 # NEARBY6 + cube_side_length: 1000.0 + det_range: 100.0 + fov_degree: 360.0 + imu_en: true + start_in_aggressive_motion: false + extrinsic_est_en: false + imu_time_inte: 0.005 + lidar_meas_cov: 0.01 + acc_cov_input: 0.1 + vel_cov: 20.0 + gyr_cov_input: 0.01 + gyr_cov_output: 1000.0 + acc_cov_output: 500.0 + b_gyr_cov: 0.0001 + b_acc_cov: 0.0001 + imu_meas_acc_cov: 0.01 + imu_meas_omg_cov: 0.01 + match_s: 81.0 + gravity_align: true + gravity: [0.0, 0.0, -9.810] + gravity_init: [0.0, 0.0, -9.810] + extrinsic_T: [-0.011, -0.02329, 0.04412] # Mid-360 IMU->lidar offset (m) + extrinsic_R: [1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0] + +odometry: + publish_odometry_without_downsample: false + odom_only: false diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt index 9bce7246f2..27ad294a3b 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt @@ -53,6 +53,8 @@ find_package(Eigen3 REQUIRED) # PCL (only components we need — avoid full PCL which drags in VTK via io) find_package(PCL 1.8 REQUIRED COMPONENTS common filters) +find_package(yaml-cpp REQUIRED) + # glog (iVox map backend — include/ivox/ivox3d.h needs glog) find_package(glog REQUIRED) @@ -92,6 +94,7 @@ target_link_libraries(pointlio_native PRIVATE ${LCM_LIBRARIES} ${LIVOX_SDK} ${PCL_LIBRARIES} + yaml-cpp::yaml-cpp glog::glog ) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock index 3cb06c2284..531b9d7a15 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock @@ -37,11 +37,11 @@ "fast-lio": { "flake": false, "locked": { - "lastModified": 1781782101, - "narHash": "sha256-2phOAdagFal8BTBEKxEbl3LDSx/7SNGVTFu0zYEXB1g=", + "lastModified": 1781514593, + "narHash": "sha256-85zc03F2Ztw/pO1nUtVHnC+4yGzVi5z9pKk9lZcFnE8=", "owner": "dimensionalOS", "repo": "dimos-module-fastlio2", - "rev": "288e357e5457723c1cce4d4060f76ed7f85b10d4", + "rev": "286c530db7661ca6874cd8c1381357adf83cd19f", "type": "github" }, "original": { diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix index 0ef30ba768..303a5aa093 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix @@ -89,6 +89,7 @@ pkgs.glib pkgs.eigen pkgs.pcl + pkgs.yaml-cpp pkgs.glog pkgs.boost pkgs.llvmPackages.openmp diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index e3d53482ef..72e7a25094 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -5,13 +5,13 @@ // // Binds Livox SDK2 directly into the Point-LIO core: SDK callbacks feed // CustomMsg/Imu to the IESKF estimator, which performs LiDAR-inertial SLAM. -// Sensor-frame point clouds and odometry are published on LCM. +// Sensor-frame (mid360_link) point clouds and odometry are published on LCM. // // Usage: // ./pointlio_native \ // --lidar '/lidar#sensor_msgs.PointCloud2' \ // --odometry '/odometry#nav_msgs.Odometry' \ -// --filter_size_surf 0.2 --ivox_grid_resolution 2.0 ... \ # tuning as plain CLI args +// --config_path /path/to/default.yaml \ // --host_ip 192.168.1.5 --lidar_ip 192.168.1.155 #include @@ -57,28 +57,10 @@ static double get_publish_ts() { return std::chrono::duration( std::chrono::system_clock::now().time_since_epoch()).count(); } -// Parse a comma-separated list of doubles (CLI vector args); empty on bad input. -static std::vector parse_doubles(const std::string& csv) { - std::vector out; - size_t i = 0; - while (i < csv.size()) { - size_t j = csv.find(',', i); - if (j == std::string::npos) { j = csv.size(); } - try { - out.push_back(std::stod(csv.substr(i, j - i))); - } catch (...) { - return {}; - } - i = j + 1; - } - return out; -} - static std::string g_lidar_topic; static std::string g_odometry_topic; static std::string g_frame_id; // required via --frame_id static std::string g_child_frame_id; // required via --child_frame_id -static std::string g_sensor_frame_id; // required via --sensor_frame_id static float g_frequency = 10.0f; // Frame accumulator (Livox SDK raw → CustomMsg) @@ -103,7 +85,7 @@ static uint64_t get_timestamp_ns(const LivoxLidarEthernetPacket* pkt) { using dimos::time_from_seconds; using dimos::make_header; -// Publish the lidar point cloud in the sensor frame (g_sensor_frame_id). +// Publish the lidar point cloud in the sensor body frame (g_frame_id). // `cloud` is Point-LIO's undistorted scan in the sensor's own frame // (get_body_cloud), so points are published as-is with no world registration. static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, const std::string& topic = "") { @@ -113,7 +95,7 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, const std int num_points = static_cast(cloud->size()); sensor_msgs::PointCloud2 pc; - pc.header = make_header(g_sensor_frame_id, timestamp); + pc.header = make_header(g_frame_id, timestamp); pc.height = 1; pc.width = num_points; pc.is_bigendian = 0; @@ -339,68 +321,11 @@ int main(int argc, char** argv) { return 1; } - // Point-LIO tuning, passed as CLI args by the dimos module (no YAML). - PointLioParams params; - // common - params.con_frame = mod.arg_bool("con_frame", params.con_frame); - params.con_frame_num = mod.arg_int("con_frame_num", params.con_frame_num); - params.cut_frame = mod.arg_bool("cut_frame", params.cut_frame); - params.cut_frame_time_interval = mod.arg_float("cut_frame_time_interval", params.cut_frame_time_interval); - params.time_lag_imu_to_lidar = mod.arg_float("time_lag_imu_to_lidar", params.time_lag_imu_to_lidar); - // preprocess - params.scan_line = mod.arg_int("scan_line", params.scan_line); - params.scan_rate = mod.arg_int("scan_rate", params.scan_rate); - params.blind = mod.arg_float("blind", params.blind); - params.point_filter_num = mod.arg_int("point_filter_num", params.point_filter_num); - std::string lidar_type = mod.arg("lidar_type", "avia"); - params.lidar_type = lidar_type == "velodyne" ? 2 : lidar_type == "ouster" ? 3 : - lidar_type == "hesai" ? 4 : lidar_type == "unilidar" ? 5 : 1; - std::string ts_unit = mod.arg("timestamp_unit", "nanosecond"); - params.timestamp_unit = ts_unit == "second" ? 0 : ts_unit == "millisecond" ? 1 : - ts_unit == "microsecond" ? 2 : 3; - // mapping - params.use_imu_as_input = mod.arg_bool("use_imu_as_input", params.use_imu_as_input); - params.prop_at_freq_of_imu = mod.arg_bool("prop_at_freq_of_imu", params.prop_at_freq_of_imu); - params.check_satu = mod.arg_bool("check_satu", params.check_satu); - params.init_map_size = mod.arg_int("init_map_size", params.init_map_size); - params.space_down_sample = mod.arg_bool("space_down_sample", params.space_down_sample); - params.satu_acc = mod.arg_float("satu_acc", params.satu_acc); - params.satu_gyro = mod.arg_float("satu_gyro", params.satu_gyro); - params.acc_norm = mod.arg_float("acc_norm", params.acc_norm); - params.plane_thr = mod.arg_float("plane_thr", params.plane_thr); - params.filter_size_surf = mod.arg_float("filter_size_surf", params.filter_size_surf); - params.filter_size_map = mod.arg_float("filter_size_map", params.filter_size_map); - params.ivox_grid_resolution = mod.arg_float("ivox_grid_resolution", params.ivox_grid_resolution); - std::string ivox_nearby = mod.arg("ivox_nearby_type", "nearby6"); - params.ivox_nearby_type = ivox_nearby == "center" ? 0 : ivox_nearby == "nearby18" ? 18 : - ivox_nearby == "nearby26" ? 26 : 6; - params.cube_side_length = mod.arg_float("cube_side_length", params.cube_side_length); - params.det_range = mod.arg_float("det_range", params.det_range); - params.fov_degree = mod.arg_float("fov_degree", params.fov_degree); - params.imu_en = mod.arg_bool("imu_en", params.imu_en); - params.start_in_aggressive_motion = mod.arg_bool("start_in_aggressive_motion", params.start_in_aggressive_motion); - params.extrinsic_est_en = mod.arg_bool("extrinsic_est_en", params.extrinsic_est_en); - params.imu_time_inte = mod.arg_float("imu_time_inte", params.imu_time_inte); - params.lidar_meas_cov = mod.arg_float("lidar_meas_cov", params.lidar_meas_cov); - params.acc_cov_input = mod.arg_float("acc_cov_input", params.acc_cov_input); - params.vel_cov = mod.arg_float("vel_cov", params.vel_cov); - params.gyr_cov_input = mod.arg_float("gyr_cov_input", params.gyr_cov_input); - params.gyr_cov_output = mod.arg_float("gyr_cov_output", params.gyr_cov_output); - params.acc_cov_output = mod.arg_float("acc_cov_output", params.acc_cov_output); - params.b_gyr_cov = mod.arg_float("b_gyr_cov", params.b_gyr_cov); - params.b_acc_cov = mod.arg_float("b_acc_cov", params.b_acc_cov); - params.imu_meas_acc_cov = mod.arg_float("imu_meas_acc_cov", params.imu_meas_acc_cov); - params.imu_meas_omg_cov = mod.arg_float("imu_meas_omg_cov", params.imu_meas_omg_cov); - params.match_s = mod.arg_float("match_s", params.match_s); - params.gravity_align = mod.arg_bool("gravity_align", params.gravity_align); - if (auto g = parse_doubles(mod.arg("gravity", "")); !g.empty()) params.gravity = g; - if (auto gi = parse_doubles(mod.arg("gravity_init", "")); !gi.empty()) params.gravity_init = gi; - if (auto et = parse_doubles(mod.arg("extrinsic_t", "")); !et.empty()) params.extrinsic_T = et; - if (auto er = parse_doubles(mod.arg("extrinsic_r", "")); !er.empty()) params.extrinsic_R = er; - // odometry - params.publish_odometry_without_downsample = - mod.arg_bool("publish_odometry_without_downsample", params.publish_odometry_without_downsample); - params.odom_only = mod.arg_bool("odom_only", params.odom_only); + std::string config_path = mod.arg("config_path", ""); + if (config_path.empty()) { + fprintf(stderr, "Error: --config_path is required\n"); + return 1; + } // Point-LIO internal processing rates double msr_freq = mod.arg_float("msr_freq", 50.0f); @@ -411,8 +336,7 @@ int main(int argc, char** argv) { std::string lidar_ip = mod.arg("lidar_ip", "192.168.1.155"); g_frequency = mod.arg_float("frequency", 10.0f); g_frame_id = mod.arg_required("frame_id"); - g_child_frame_id = mod.arg_required("child_frame_id"); - g_sensor_frame_id = mod.arg_required("sensor_frame_id"); + g_child_frame_id = mod.arg_required("body_frame_id"); float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f); float odom_freq = mod.arg_float("odom_freq", 50.0f); @@ -438,7 +362,7 @@ int main(int argc, char** argv) { printf("[pointlio] Starting Point-LIO + Livox Mid-360 native module\n"); printf("[pointlio] lidar topic: %s\n", g_lidar_topic.empty() ? "(disabled)" : g_lidar_topic.c_str()); printf("[pointlio] odometry topic: %s\n", g_odometry_topic.empty() ? "(disabled)" : g_odometry_topic.c_str()); - printf("[pointlio] tuning: filter_size_surf=%.3f ivox_res=%.3f lidar_type=%d\n", params.filter_size_surf, params.ivox_grid_resolution, params.lidar_type); + printf("[pointlio] config: %s\n", config_path.c_str()); printf("[pointlio] host_ip: %s lidar_ip: %s frequency: %.1f Hz\n", host_ip.c_str(), lidar_ip.c_str(), g_frequency); printf("[pointlio] pointcloud_freq: %.1f Hz odom_freq: %.1f Hz\n", pointcloud_freq, odom_freq); } @@ -454,7 +378,7 @@ int main(int argc, char** argv) { g_lcm = &lcm; if (debug) { printf("[pointlio] Initializing Point-LIO...\n"); } - PointLio point_lio(params, msr_freq, main_freq); + PointLio point_lio(config_path, msr_freq, main_freq); g_point_lio = &point_lio; if (debug) { printf("[pointlio] Point-LIO initialized.\n"); } diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index 679d678b18..86cac95413 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -24,17 +24,16 @@ PointLio.blueprint(host_ip="192.168.1.5", lidar_ip="192.168.1.155"), SomeConsumer.blueprint(), )).loop() - -Point-LIO tuning lives directly on ``PointLioConfig`` and is passed to the C++ -binary as plain CLI args (no YAML). """ from __future__ import annotations import os -from typing import TYPE_CHECKING, Literal +from pathlib import Path +from typing import TYPE_CHECKING, Annotated from pydantic import Field +from pydantic.experimental.pipeline import validate_as from reactivex.disposable import Disposable from dimos.core.core import rpc @@ -58,17 +57,10 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.navigation.nav_stack.frames import FRAME_BODY, FRAME_ODOM +from dimos.navigation.nav_stack.frames import FRAME_ODOM from dimos.spec import perception -# Human-readable enums; the C++ binary (main.cpp) maps these strings to -# Point-LIO's int codes. -# LID_TYPE enum (Point-LIO src/preprocess.h). avia = 1 selects the Livox branch -# the Mid-360 emits. -LidarType = Literal["avia", "velodyne", "ouster", "hesai", "unilidar"] -TimestampUnit = Literal["second", "millisecond", "microsecond", "nanosecond"] -# iVox local-map neighbour stencil. -IvoxNearbyType = Literal["center", "nearby6", "nearby18", "nearby26"] +_CONFIG_DIR = Path(__file__).parent / "config" class PointLioConfig(NativeModuleConfig): @@ -81,13 +73,11 @@ class PointLioConfig(NativeModuleConfig): lidar_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_POINTLIO_LIDAR_IP")) frequency: float = 10.0 - # Odometry is published as frame_id (fixed) -> child_frame_id (moving body), - # and also broadcast on TF. The point cloud is stamped with sensor_frame_id - # (the lidar's own frame — get_body_cloud is the undistorted scan, not yet - # transformed into the body frame). - frame_id: str = FRAME_ODOM - child_frame_id: str = FRAME_BODY - sensor_frame_id: str = "mid360_link" + # Sensor frame for the cloud + odometry headers. + frame_id: str = "mid360_link" + # Published TF: body_start_frame_id -> body_frame_id. + body_start_frame_id: str = FRAME_ODOM + body_frame_id: str = "base_link" # Point-LIO internal processing rates (Hz) msr_freq: float = 50.0 @@ -96,64 +86,13 @@ class PointLioConfig(NativeModuleConfig): pointcloud_freq: float = 10.0 odom_freq: float = 30.0 - debug: bool = False + # Point-LIO YAML config (relative to config/ dir, or absolute path). + config: Annotated[ + Path, + validate_as(...).transform(lambda path: path if path.is_absolute() else _CONFIG_DIR / path), + ] = Path("default.yaml") - # Point-LIO tuning, passed to the binary as plain CLI args (read in main.cpp). - # common - con_frame: bool = False - con_frame_num: int = 1 - cut_frame: bool = False - cut_frame_time_interval: float = 0.1 - time_lag_imu_to_lidar: float = 0.0 - # preprocess - lidar_type: LidarType = "avia" # 1 = AVIA (Livox) branch the Mid-360 emits - scan_line: int = 4 - scan_rate: int = 10 - timestamp_unit: TimestampUnit = "nanosecond" - blind: float = 0.5 # spherical min range (m) - point_filter_num: int = 3 # pre-KF decimation: keep every Nth raw point (1 = all) - # mapping - use_imu_as_input: bool = False # false = IMU-as-output model (robust path) - prop_at_freq_of_imu: bool = True - check_satu: bool = True - init_map_size: int = 10 - space_down_sample: bool = True # pre-KF voxel downsample (leaf = filter_size_surf) - satu_acc: float = 3.0 # g; accel >= this is treated as saturated, bounding velocity - satu_gyro: float = 35.0 - acc_norm: float = 1.0 # IMU accel unit: g - plane_thr: float = 0.1 - filter_size_surf: float = 0.2 # pre-KF scan downsample leaf (m), iff space_down_sample - filter_size_map: float = 0.5 - ivox_grid_resolution: float = 2.0 # iVox local-map grid (m) - ivox_nearby_type: IvoxNearbyType = "nearby6" - cube_side_length: float = 1000.0 - det_range: float = 100.0 - fov_degree: float = 360.0 - imu_en: bool = True - start_in_aggressive_motion: bool = False - extrinsic_est_en: bool = False - imu_time_inte: float = 0.005 - lidar_meas_cov: float = 0.01 - acc_cov_input: float = 0.1 - vel_cov: float = 20.0 - gyr_cov_input: float = 0.01 - gyr_cov_output: float = 1000.0 - acc_cov_output: float = 500.0 - b_gyr_cov: float = 0.0001 - b_acc_cov: float = 0.0001 - imu_meas_acc_cov: float = 0.01 - imu_meas_omg_cov: float = 0.01 - match_s: float = 81.0 - gravity_align: bool = True - gravity: list[float] = Field(default_factory=lambda: [0.0, 0.0, -9.81]) - gravity_init: list[float] = Field(default_factory=lambda: [0.0, 0.0, -9.81]) - extrinsic_t: list[float] = Field(default_factory=lambda: [-0.011, -0.02329, 0.04412]) - extrinsic_r: list[float] = Field( - default_factory=lambda: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] - ) - # odometry - publish_odometry_without_downsample: bool = False - odom_only: bool = False + debug: bool = False # SDK port configuration (see livox/ports.py for defaults) cmd_data_port: int = SDK_CMD_DATA_PORT @@ -167,6 +106,18 @@ class PointLioConfig(NativeModuleConfig): host_imu_data_port: int = SDK_HOST_IMU_DATA_PORT host_log_data_port: int = SDK_HOST_LOG_DATA_PORT + # Resolved in __post_init__, passed as --config_path to the binary + config_path: str | None = None + + cli_exclude: frozenset[str] = frozenset({"config", "body_start_frame_id"}) + + def model_post_init(self, __context: object) -> None: + super().model_post_init(__context) + cfg = self.config + if not cfg.is_absolute(): + cfg = _CONFIG_DIR / cfg + self.config_path = str(cfg.resolve()) + class PointLio(NativeModule, perception.Lidar, perception.Odometry): config: PointLioConfig @@ -185,8 +136,8 @@ def start(self) -> None: def _on_odom_for_tf(self, msg: Odometry) -> None: self.tf.publish( Transform( - frame_id=self.frame_id, - child_frame_id=self.config.child_frame_id, + frame_id=self.config.body_start_frame_id, + child_frame_id=self.config.body_frame_id, translation=Vector3( msg.pose.position.x, msg.pose.position.y, diff --git a/dimos/hardware/sensors/lidar/pointlio/recorder.py b/dimos/hardware/sensors/lidar/pointlio/recorder.py index ba9a3230f6..e1afbd502c 100644 --- a/dimos/hardware/sensors/lidar/pointlio/recorder.py +++ b/dimos/hardware/sensors/lidar/pointlio/recorder.py @@ -14,44 +14,169 @@ """Record Point-LIO odometry + lidar into a memory2 SQLite db. -A ``Recorder`` that records its In ports under their own names -(``pointlio_odometry`` / ``pointlio_lidar``) — wire them to PointLio's -``odometry`` / ``lidar`` outputs with ``.remappings()``. Poses come straight -from the odometry stream (``@pose_setter_for``): each lidar frame is stamped with -the latest odometry pose so ``pointlio_lidar`` carries the trajectory and ``dimos -map global`` can register the body-frame cloud directly (no ``pose-fill`` pass). +Subscribes to a PointLio's ``odometry`` / ``lidar`` outputs (auto-connected by +matching stream name + type — no remappings needed) and appends them to a +memory2 store. Timestamps are converted onto the db's existing clock so a run +can be appended to an existing db (e.g. a fastlio replay) and compared on one +timeline. Owns the db lifecycle: refuses to clobber existing streams unless +``force``, and derives the alignment reference from whatever the db already holds. + +Each lidar frame is stamped with the latest odometry pose, so ``pointlio_lidar`` +carries the trajectory and ``dimos map global`` can register it directly (it +transforms the body-frame cloud by that pose) — no ``dimos map pose-fill`` pass. """ from __future__ import annotations +from collections.abc import AsyncIterator +import math +from pathlib import Path +import sqlite3 +import time + +from dimos.core.module import Module, ModuleConfig 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 +# Below this the db's existing timestamps are sensor-boot seconds, not unix time. +_SENSOR_CLOCK_MAX = 1e8 +# Strictly-increasing tie-breaker so two samples never collide on ts. +_EPS = 1e-9 +# Max sensor-ts gap to attach the latest odometry pose to a lidar frame, so +# pointlio_lidar carries the trajectory and `dimos map global` can register it +# (it transforms by the per-frame pose). Matches pose_fill's nearest-match window. +_POSE_MATCH_TOL = 0.1 -class PointlioRecorderConfig(RecorderConfig): - # Append into a populated db (keep other streams); replace only our own. - on_existing: OnExisting = OnExisting.APPEND +def _existing_min_ts(db_path: Path) -> float: + """Min ts across the db's existing streams, or -1.0 if none/absent.""" + if not db_path.exists(): + return -1.0 + con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) + try: + tables = [ + row[0] + for row in con.execute("SELECT name FROM sqlite_master WHERE type='table'").fetchall() + ] + best: float | None = None + for table in tables: + if table.startswith("_") or table.startswith("sqlite_"): + continue + try: + cols = [col[1] for col in con.execute(f"PRAGMA table_info('{table}')").fetchall()] + if "ts" not in cols: + continue + row = con.execute(f"SELECT MIN(ts) FROM '{table}'").fetchone() + except sqlite3.OperationalError: + continue + if row and row[0] is not None: + best = row[0] if best is None else min(best, row[0]) + return best if best is not None else -1.0 + finally: + con.close() + + +class PointlioRecorderConfig(ModuleConfig): + """Configures the recorder with the target db and timing conversion.""" + + db_path: str = "" + # db stream/table names the Point-LIO outputs are recorded under. + odom_stream_name: str = "pointlio_odometry" + lidar_stream_name: str = "pointlio_lidar" + # Explicit offset override; NaN means auto-derive from the db's earliest ts. + time_offset: float = float("nan") + # Drop pre-existing odom/lidar streams instead of refusing to overwrite. + force: bool = False -class PointlioRecorder(Recorder): - config: PointlioRecorderConfig - pointlio_odometry: In[Odometry] - pointlio_lidar: In[PointCloud2] +class PointlioRecorder(Module): + config: PointlioRecorderConfig + lidar: In[PointCloud2] + odometry: In[Odometry] + _offset: float | None = None + _ref_start_ts: float = -1.0 + _last_odom_ts: float = 0.0 + _last_lidar_ts: float = 0.0 + _odom_count: int = 0 + _lidar_count: int = 0 + # Latest odometry pose + its raw sensor ts, stamped onto each lidar frame so + # pointlio_lidar carries the trajectory (no separate pose-fill pass). _last_odom_pose: Pose | None = None + _last_odom_raw_ts: float = 0.0 + + async def main(self) -> AsyncIterator[None]: + # Deferred: the store is opened in the worker process that runs main(), + # not at module-scan/import time on the host. + from dimos.memory2.store.sqlite import SqliteStore + + cfg = self.config + self._store = SqliteStore(path=cfg.db_path) + names = (cfg.odom_stream_name, cfg.lidar_stream_name) + existing = sorted(set(self._store.list_streams()) & set(names)) + if existing and not cfg.force: + raise RuntimeError( + f"PointlioRecorder: {Path(cfg.db_path).name} already has {existing}; " + "set force=True to overwrite" + ) + for name in existing: + self._store.delete_stream(name) + # Reference is the db's earliest ts *after* dropping our own old streams, + # so only the data we're aligning against (e.g. a fastlio replay) counts. + self._ref_start_ts = _existing_min_ts(Path(cfg.db_path)) + self._os = self._store.stream(cfg.odom_stream_name, Odometry) + self._ls = self._store.stream(cfg.lidar_stream_name, PointCloud2) + yield + self._store.stop() - @pose_setter_for("pointlio_odometry") - def _odom_pose(self, msg: Odometry) -> Pose | None: + def _resolve_offset(self, first_ts: float) -> float: + override = self.config.time_offset + if not math.isnan(override): + return override + ref = self._ref_start_ts + if ref < 0.0 or ref < _SENSOR_CLOCK_MAX: + # Empty db, or db already on the sensor clock -> exact alignment. + return 0.0 + # db on wall-clock time -> start-align Point-LIO onto the db's earliest ts. + return ref - first_ts + + def _aligned_ts(self, raw_ts: float, last_ts: float) -> float: + """Convert a sensor ts onto the db clock, kept strictly above last_ts.""" + if self._offset is None: + self._offset = self._resolve_offset(raw_ts) + return max(raw_ts + self._offset, last_ts + _EPS) + + async def handle_odometry(self, msg: Odometry) -> None: + # `is not None`, not `or`: a real sensor ts of 0.0 must not fall back to + # wall time (would misclassify the stream's clock in _resolve_offset). + raw_ts_raw = getattr(msg, "ts", None) + raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() + ts = self._aligned_ts(raw_ts, self._last_odom_ts) + self._last_odom_ts = ts 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 + pose_inner = getattr(pose, "pose", None) if pose is not None else None + self._os.append(msg, ts=ts, pose=pose_inner) + self._last_odom_pose = pose_inner + self._last_odom_raw_ts = raw_ts + self._odom_count += 1 + + async def handle_lidar(self, msg: PointCloud2) -> None: + raw_ts_raw = getattr(msg, "ts", None) + raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() + ts = self._aligned_ts(raw_ts, self._last_lidar_ts) + self._last_lidar_ts = ts + # Stamp the latest odometry pose (within tolerance) onto the frame so + # pointlio_lidar carries the trajectory; map global transforms the + # body-frame cloud by it. Both Point-LIO outputs share a publish ts, so + # the nearest odometry is at most ~one odom period stale. Frames with no + # match (e.g. before the first odometry) get None and are map-skipped. + pose = ( + self._last_odom_pose + if self._last_odom_pose is not None + and abs(raw_ts - self._last_odom_raw_ts) <= _POSE_MATCH_TOL + else None + ) + self._ls.append(msg, ts=ts, pose=pose) + self._lidar_count += 1 diff --git a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py index b842634e33..a39678b549 100644 --- a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py @@ -18,12 +18,10 @@ # LFS archive, NOT the archive name) PCAP_PATH="$(python -c "from dimos.utils.data import get_data; print(get_data('mid360_shake_stairs/mid360_shake_stairs.pcap'))")" - # gen .db from pcap (defaults to .db next to the pcap) - python -m dimos.hardware.sensors.lidar.pointlio.scripts.pcap_to_db --pcap "$PCAP_PATH" - - # override any PointLioConfig field via a small YAML/JSON doc, e.g. {acc_cov_input: 0.3} + # gen .db from pcap with your config (defaults to .db next to the pcap) python -m dimos.hardware.sensors.lidar.pointlio.scripts.pcap_to_db \ - --pcap "$PCAP_PATH" --config overrides.yaml + --config dimos/hardware/sensors/lidar/pointlio/config/default.yaml \ + --pcap "$PCAP_PATH" # add to existing .db DB="mem2.db" @@ -63,9 +61,6 @@ _STARTUP_TIMEOUT_SEC = 60.0 # Max |Δts| to match a lidar frame to an odometry pose when aggregating the .rrd. _POSE_MATCH_TOL = 0.1 -# db stream/table names (= the recorder's In-port names). -_ODOM_STREAM = "pointlio_odometry" -_LIDAR_STREAM = "pointlio_lidar" # Extra seconds past the pcap's own duration before auto-stopping, when no # explicit --max-sensor-sec is given. _DRAIN_MARGIN_SEC = 4.0 @@ -204,9 +199,7 @@ def _write_rrd(db_path: Path, odom_stream: str, lidar_stream: str, voxel: float) store.stop() -def _build_blueprint( - args: argparse.Namespace, db_path: Path, overrides: dict[str, Any] -) -> Blueprint: +def _build_blueprint(args: argparse.Namespace, db_path: Path, config_path: str) -> Blueprint: """autoconnect(VirtualMid360 + PointLio + PointlioRecorder). PointLio's ``odometry``/``lidar`` outputs auto-wire to the recorder's @@ -218,35 +211,36 @@ def _build_blueprint( from dimos.hardware.sensors.lidar.pointlio.recorder import PointlioRecorder from dimos.hardware.sensors.lidar.virtual_mid360.module import VirtualMid360 - pointlio_kwargs: dict[str, Any] = dict( + # `config` (not `config_path`, which PointLioConfig derives itself); already + # an absolute path so it bypasses the config-dir-relative resolution. Omit + # when empty to keep the default.yaml. + pointlio_kwargs: dict[str, object] = dict( host_ip=args.host_ip, lidar_ip=args.lidar_ip, odom_freq=args.odom_freq, debug=False ) - pointlio_kwargs.update(overrides) - - return ( - autoconnect( - VirtualMid360.blueprint( - pcap=str(args.pcap_path), - rate=args.rate, - delay=args.warmup_sec, # hold streaming until PointLio's SDK is up - host_ip=args.host_ip, - lidar_ip=args.lidar_ip, - alias_iface=args.alias_iface, - # When the NIC is provisioned by hand, skip the module's own sudo - # (it runs in a tty-less worker where a password prompt can't appear). - setup_network=not args.no_network_setup, - ), - PointLio.blueprint(**pointlio_kwargs), - PointlioRecorder.blueprint(db_path=str(db_path)), - ) - .remappings( - [ - (PointlioRecorder, "pointlio_odometry", "odometry"), - (PointlioRecorder, "pointlio_lidar", "lidar"), - ] - ) - .global_config(n_workers=4, robot_model="mid360_pointlio_pcap_to_db") - ) + if config_path: + pointlio_kwargs["config"] = config_path + + return autoconnect( + VirtualMid360.blueprint( + pcap=str(args.pcap_path), + rate=args.rate, + delay=args.warmup_sec, # hold streaming until PointLio's SDK is up + host_ip=args.host_ip, + lidar_ip=args.lidar_ip, + alias_iface=args.alias_iface, + # When the NIC is provisioned by hand, skip the module's own sudo + # (it runs in a tty-less worker where a password prompt can't appear). + setup_network=not args.no_network_setup, + ), + PointLio.blueprint(**pointlio_kwargs), + PointlioRecorder.blueprint( + db_path=str(db_path), + odom_stream_name=args.odom_stream_name, + lidar_stream_name=args.lidar_stream_name, + time_offset=float("nan") if args.time_offset is None else args.time_offset, + force=args.force, + ), + ).global_config(n_workers=4, robot_model="mid360_pointlio_pcap_to_db") def _poll_until_drained( @@ -295,21 +289,6 @@ def _poll_until_drained( stagnant_since = None -def _load_overrides(config: str) -> dict[str, Any]: - """Load a YAML/JSON doc of PointLioConfig field overrides, e.g. {acc_cov_input: 0.3}.""" - if not config: - return {} - import yaml - - path = Path(config).expanduser().resolve() - if not path.exists(): - raise FileNotFoundError(f"--config not found: {path}") - data = yaml.safe_load(path.read_text()) or {} - if not isinstance(data, dict): - raise ValueError(f"--config must be a mapping of PointLioConfig fields, got {type(data)}") - return data - - def _run(args: argparse.Namespace) -> int: from dimos.core.coordination.module_coordinator import ModuleCoordinator @@ -320,7 +299,13 @@ def _run(args: argparse.Namespace) -> int: args.pcap_path = pcap_path db_path = Path(args.db).expanduser().resolve() if args.db else pcap_path.with_suffix(".db") db_path.parent.mkdir(parents=True, exist_ok=True) - overrides = _load_overrides(args.config) + # Resolve --config against the *invoking* cwd (pwd-relative) up front; the + # PointLio config field otherwise resolves a relative path against its own + # config/ dir, never the pwd. Absolute path passes through unchanged. + config_path = str(Path(args.config).expanduser().resolve()) if args.config else "" + if config_path and not Path(config_path).exists(): + print(f"[pcap_to_db] missing --config: {config_path}", file=sys.stderr) + return 2 # Default the stop bound to the pcap's own duration: Point-LIO keeps # dead-reckoning (publishing at full rate) after the pcap drains, so the @@ -341,13 +326,15 @@ def _run(args: argparse.Namespace) -> int: coord = None try: - coord = ModuleCoordinator.build(_build_blueprint(args, db_path, overrides)) - drained = _poll_until_drained(db_path, _ODOM_STREAM, _LIDAR_STREAM, max_sensor_sec) + coord = ModuleCoordinator.build(_build_blueprint(args, db_path, config_path)) + drained = _poll_until_drained( + db_path, args.odom_stream_name, args.lidar_stream_name, max_sensor_sec + ) finally: if coord is not None: coord.stop() - o_cnt, o_min, o_max = _odom_stats(db_path, _ODOM_STREAM) + o_cnt, o_min, o_max = _odom_stats(db_path, args.odom_stream_name) if o_cnt == 0 or not drained: print("[pcap_to_db] no odometry recorded — check the run above", file=sys.stderr) return 1 @@ -357,7 +344,7 @@ def _run(args: argparse.Namespace) -> int: ) if not args.no_rrd: try: - rrd = _write_rrd(db_path, _ODOM_STREAM, _LIDAR_STREAM, args.voxel) + rrd = _write_rrd(db_path, args.odom_stream_name, args.lidar_stream_name, args.voxel) if rrd is not None: print(f"[pcap_to_db] wrote {rrd.name} (aggregated lidar + pose path)", flush=True) except Exception as exc: # viz is a non-fatal bonus @@ -386,6 +373,13 @@ def main(argv: list[str]) -> int: default=0.0, help="stop after N sensor seconds (0 = whole pcap)", ) + parser.add_argument( + "--time-offset", + type=float, + default=None, + help="seconds added to every output ts (auto if omitted)", + ) + parser.add_argument("--force", action="store_true", help="overwrite existing pointlio streams") parser.add_argument( "--no-rrd", action="store_true", @@ -403,7 +397,17 @@ def main(argv: list[str]) -> int: parser.add_argument( "--config", default="", - help="YAML/JSON doc of PointLioConfig field overrides (e.g. {acc_cov_input: 0.3})", + help="Point-LIO YAML config (pwd-relative or absolute; default: module's default.yaml)", + ) + parser.add_argument( + "--odom-stream-name", + default="pointlio_odometry", + help="db stream/table name for the recorded odometry", + ) + parser.add_argument( + "--lidar-stream-name", + default="pointlio_lidar", + help="db stream/table name for the recorded point cloud", ) # Addressing knobs (override to run two replays at once). parser.add_argument("--host-ip", default="192.168.1.5") From 253cf7a50e1e07e43e873f3f18f57a715a81824d Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 18 Jun 2026 13:53:17 -0700 Subject: [PATCH 175/185] Revert "pointlio: revert module to origin/main (keep PR #2498 fastlio-only)" This reverts commit 02600309d2939ffae79259eae30778cf5ac7524a. --- .../lidar/pointlio/config/default.yaml | 69 ------- .../sensors/lidar/pointlio/cpp/CMakeLists.txt | 3 - .../sensors/lidar/pointlio/cpp/flake.lock | 6 +- .../sensors/lidar/pointlio/cpp/flake.nix | 1 - .../sensors/lidar/pointlio/cpp/main.cpp | 100 ++++++++-- .../hardware/sensors/lidar/pointlio/module.py | 109 ++++++++--- .../sensors/lidar/pointlio/recorder.py | 173 +++--------------- .../lidar/pointlio/scripts/pcap_to_db.py | 122 ++++++------ 8 files changed, 253 insertions(+), 330 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/pointlio/config/default.yaml diff --git a/dimos/hardware/sensors/lidar/pointlio/config/default.yaml b/dimos/hardware/sensors/lidar/pointlio/config/default.yaml deleted file mode 100644 index 1fd09ec9fa..0000000000 --- a/dimos/hardware/sensors/lidar/pointlio/config/default.yaml +++ /dev/null @@ -1,69 +0,0 @@ -# Point-LIO config. Mid-360-specific values to retune for a different sensor: -# preprocess.lidar_type (Livox), blind/scan_line, mapping.extrinsic_T/R (Mid-360 -# IMU->lidar mount), det_range, fov_degree. -common: - con_frame: false - con_frame_num: 1 - cut_frame: false - cut_frame_time_interval: 0.1 - time_lag_imu_to_lidar: 0.0 - -preprocess: - # LID_TYPE enum (Point-LIO src/preprocess.h): - # 1 = AVIA (Livox), 2 = VELO16, 3 = OUST64, 4 = HESAIxt32, 5 = UNILIDAR - # 1 selects the Livox branch (preprocess.cpp avia_handler), which expects the - # Livox CustomMsg point layout the Mid-360 emits: - # https://github.com/Livox-SDK/livox_ros_driver2/blob/master/msg/CustomMsg.msg - lidar_type: 1 - scan_line: 4 - scan_rate: 10 - timestamp_unit: 3 # 3 = nanosecond - blind: 0.5 - # Pre-KF input decimation: keep every Nth raw point. 1 = keep all (disable). - point_filter_num: 3 - -mapping: - use_imu_as_input: false # false = IMU-as-output model (Point-LIO's robust path) - prop_at_freq_of_imu: true - check_satu: true - init_map_size: 10 - # Pre-KF voxel downsample of each scan before the filter. false = feed the - # full scan (disable). Leaf size is filter_size_surf below. - space_down_sample: true - satu_acc: 3.0 # g; accel >= this is treated as saturated (residual zeroed) to bound velocity - satu_gyro: 35.0 - acc_norm: 1.0 # IMU accel unit: g - plane_thr: 0.1 - filter_size_surf: 0.2 # pre-KF scan downsample leaf size (m), used iff space_down_sample - filter_size_map: 0.5 - ivox_grid_resolution: 2.0 # iVox local-map grid (m) - ivox_nearby_type: 6 # NEARBY6 - cube_side_length: 1000.0 - det_range: 100.0 - fov_degree: 360.0 - imu_en: true - start_in_aggressive_motion: false - extrinsic_est_en: false - imu_time_inte: 0.005 - lidar_meas_cov: 0.01 - acc_cov_input: 0.1 - vel_cov: 20.0 - gyr_cov_input: 0.01 - gyr_cov_output: 1000.0 - acc_cov_output: 500.0 - b_gyr_cov: 0.0001 - b_acc_cov: 0.0001 - imu_meas_acc_cov: 0.01 - imu_meas_omg_cov: 0.01 - match_s: 81.0 - gravity_align: true - gravity: [0.0, 0.0, -9.810] - gravity_init: [0.0, 0.0, -9.810] - extrinsic_T: [-0.011, -0.02329, 0.04412] # Mid-360 IMU->lidar offset (m) - extrinsic_R: [1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0] - -odometry: - publish_odometry_without_downsample: false - odom_only: false diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt index 27ad294a3b..9bce7246f2 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt @@ -53,8 +53,6 @@ find_package(Eigen3 REQUIRED) # PCL (only components we need — avoid full PCL which drags in VTK via io) find_package(PCL 1.8 REQUIRED COMPONENTS common filters) -find_package(yaml-cpp REQUIRED) - # glog (iVox map backend — include/ivox/ivox3d.h needs glog) find_package(glog REQUIRED) @@ -94,7 +92,6 @@ target_link_libraries(pointlio_native PRIVATE ${LCM_LIBRARIES} ${LIVOX_SDK} ${PCL_LIBRARIES} - yaml-cpp::yaml-cpp glog::glog ) diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock index 531b9d7a15..3cb06c2284 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock @@ -37,11 +37,11 @@ "fast-lio": { "flake": false, "locked": { - "lastModified": 1781514593, - "narHash": "sha256-85zc03F2Ztw/pO1nUtVHnC+4yGzVi5z9pKk9lZcFnE8=", + "lastModified": 1781782101, + "narHash": "sha256-2phOAdagFal8BTBEKxEbl3LDSx/7SNGVTFu0zYEXB1g=", "owner": "dimensionalOS", "repo": "dimos-module-fastlio2", - "rev": "286c530db7661ca6874cd8c1381357adf83cd19f", + "rev": "288e357e5457723c1cce4d4060f76ed7f85b10d4", "type": "github" }, "original": { diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix index 303a5aa093..0ef30ba768 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix @@ -89,7 +89,6 @@ pkgs.glib pkgs.eigen pkgs.pcl - pkgs.yaml-cpp pkgs.glog pkgs.boost pkgs.llvmPackages.openmp diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 72e7a25094..e3d53482ef 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -5,13 +5,13 @@ // // Binds Livox SDK2 directly into the Point-LIO core: SDK callbacks feed // CustomMsg/Imu to the IESKF estimator, which performs LiDAR-inertial SLAM. -// Sensor-frame (mid360_link) point clouds and odometry are published on LCM. +// Sensor-frame point clouds and odometry are published on LCM. // // Usage: // ./pointlio_native \ // --lidar '/lidar#sensor_msgs.PointCloud2' \ // --odometry '/odometry#nav_msgs.Odometry' \ -// --config_path /path/to/default.yaml \ +// --filter_size_surf 0.2 --ivox_grid_resolution 2.0 ... \ # tuning as plain CLI args // --host_ip 192.168.1.5 --lidar_ip 192.168.1.155 #include @@ -57,10 +57,28 @@ static double get_publish_ts() { return std::chrono::duration( std::chrono::system_clock::now().time_since_epoch()).count(); } +// Parse a comma-separated list of doubles (CLI vector args); empty on bad input. +static std::vector parse_doubles(const std::string& csv) { + std::vector out; + size_t i = 0; + while (i < csv.size()) { + size_t j = csv.find(',', i); + if (j == std::string::npos) { j = csv.size(); } + try { + out.push_back(std::stod(csv.substr(i, j - i))); + } catch (...) { + return {}; + } + i = j + 1; + } + return out; +} + static std::string g_lidar_topic; static std::string g_odometry_topic; static std::string g_frame_id; // required via --frame_id static std::string g_child_frame_id; // required via --child_frame_id +static std::string g_sensor_frame_id; // required via --sensor_frame_id static float g_frequency = 10.0f; // Frame accumulator (Livox SDK raw → CustomMsg) @@ -85,7 +103,7 @@ static uint64_t get_timestamp_ns(const LivoxLidarEthernetPacket* pkt) { using dimos::time_from_seconds; using dimos::make_header; -// Publish the lidar point cloud in the sensor body frame (g_frame_id). +// Publish the lidar point cloud in the sensor frame (g_sensor_frame_id). // `cloud` is Point-LIO's undistorted scan in the sensor's own frame // (get_body_cloud), so points are published as-is with no world registration. static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, const std::string& topic = "") { @@ -95,7 +113,7 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, const std int num_points = static_cast(cloud->size()); sensor_msgs::PointCloud2 pc; - pc.header = make_header(g_frame_id, timestamp); + pc.header = make_header(g_sensor_frame_id, timestamp); pc.height = 1; pc.width = num_points; pc.is_bigendian = 0; @@ -321,11 +339,68 @@ int main(int argc, char** argv) { return 1; } - std::string config_path = mod.arg("config_path", ""); - if (config_path.empty()) { - fprintf(stderr, "Error: --config_path is required\n"); - return 1; - } + // Point-LIO tuning, passed as CLI args by the dimos module (no YAML). + PointLioParams params; + // common + params.con_frame = mod.arg_bool("con_frame", params.con_frame); + params.con_frame_num = mod.arg_int("con_frame_num", params.con_frame_num); + params.cut_frame = mod.arg_bool("cut_frame", params.cut_frame); + params.cut_frame_time_interval = mod.arg_float("cut_frame_time_interval", params.cut_frame_time_interval); + params.time_lag_imu_to_lidar = mod.arg_float("time_lag_imu_to_lidar", params.time_lag_imu_to_lidar); + // preprocess + params.scan_line = mod.arg_int("scan_line", params.scan_line); + params.scan_rate = mod.arg_int("scan_rate", params.scan_rate); + params.blind = mod.arg_float("blind", params.blind); + params.point_filter_num = mod.arg_int("point_filter_num", params.point_filter_num); + std::string lidar_type = mod.arg("lidar_type", "avia"); + params.lidar_type = lidar_type == "velodyne" ? 2 : lidar_type == "ouster" ? 3 : + lidar_type == "hesai" ? 4 : lidar_type == "unilidar" ? 5 : 1; + std::string ts_unit = mod.arg("timestamp_unit", "nanosecond"); + params.timestamp_unit = ts_unit == "second" ? 0 : ts_unit == "millisecond" ? 1 : + ts_unit == "microsecond" ? 2 : 3; + // mapping + params.use_imu_as_input = mod.arg_bool("use_imu_as_input", params.use_imu_as_input); + params.prop_at_freq_of_imu = mod.arg_bool("prop_at_freq_of_imu", params.prop_at_freq_of_imu); + params.check_satu = mod.arg_bool("check_satu", params.check_satu); + params.init_map_size = mod.arg_int("init_map_size", params.init_map_size); + params.space_down_sample = mod.arg_bool("space_down_sample", params.space_down_sample); + params.satu_acc = mod.arg_float("satu_acc", params.satu_acc); + params.satu_gyro = mod.arg_float("satu_gyro", params.satu_gyro); + params.acc_norm = mod.arg_float("acc_norm", params.acc_norm); + params.plane_thr = mod.arg_float("plane_thr", params.plane_thr); + params.filter_size_surf = mod.arg_float("filter_size_surf", params.filter_size_surf); + params.filter_size_map = mod.arg_float("filter_size_map", params.filter_size_map); + params.ivox_grid_resolution = mod.arg_float("ivox_grid_resolution", params.ivox_grid_resolution); + std::string ivox_nearby = mod.arg("ivox_nearby_type", "nearby6"); + params.ivox_nearby_type = ivox_nearby == "center" ? 0 : ivox_nearby == "nearby18" ? 18 : + ivox_nearby == "nearby26" ? 26 : 6; + params.cube_side_length = mod.arg_float("cube_side_length", params.cube_side_length); + params.det_range = mod.arg_float("det_range", params.det_range); + params.fov_degree = mod.arg_float("fov_degree", params.fov_degree); + params.imu_en = mod.arg_bool("imu_en", params.imu_en); + params.start_in_aggressive_motion = mod.arg_bool("start_in_aggressive_motion", params.start_in_aggressive_motion); + params.extrinsic_est_en = mod.arg_bool("extrinsic_est_en", params.extrinsic_est_en); + params.imu_time_inte = mod.arg_float("imu_time_inte", params.imu_time_inte); + params.lidar_meas_cov = mod.arg_float("lidar_meas_cov", params.lidar_meas_cov); + params.acc_cov_input = mod.arg_float("acc_cov_input", params.acc_cov_input); + params.vel_cov = mod.arg_float("vel_cov", params.vel_cov); + params.gyr_cov_input = mod.arg_float("gyr_cov_input", params.gyr_cov_input); + params.gyr_cov_output = mod.arg_float("gyr_cov_output", params.gyr_cov_output); + params.acc_cov_output = mod.arg_float("acc_cov_output", params.acc_cov_output); + params.b_gyr_cov = mod.arg_float("b_gyr_cov", params.b_gyr_cov); + params.b_acc_cov = mod.arg_float("b_acc_cov", params.b_acc_cov); + params.imu_meas_acc_cov = mod.arg_float("imu_meas_acc_cov", params.imu_meas_acc_cov); + params.imu_meas_omg_cov = mod.arg_float("imu_meas_omg_cov", params.imu_meas_omg_cov); + params.match_s = mod.arg_float("match_s", params.match_s); + params.gravity_align = mod.arg_bool("gravity_align", params.gravity_align); + if (auto g = parse_doubles(mod.arg("gravity", "")); !g.empty()) params.gravity = g; + if (auto gi = parse_doubles(mod.arg("gravity_init", "")); !gi.empty()) params.gravity_init = gi; + if (auto et = parse_doubles(mod.arg("extrinsic_t", "")); !et.empty()) params.extrinsic_T = et; + if (auto er = parse_doubles(mod.arg("extrinsic_r", "")); !er.empty()) params.extrinsic_R = er; + // odometry + params.publish_odometry_without_downsample = + mod.arg_bool("publish_odometry_without_downsample", params.publish_odometry_without_downsample); + params.odom_only = mod.arg_bool("odom_only", params.odom_only); // Point-LIO internal processing rates double msr_freq = mod.arg_float("msr_freq", 50.0f); @@ -336,7 +411,8 @@ int main(int argc, char** argv) { std::string lidar_ip = mod.arg("lidar_ip", "192.168.1.155"); g_frequency = mod.arg_float("frequency", 10.0f); g_frame_id = mod.arg_required("frame_id"); - g_child_frame_id = mod.arg_required("body_frame_id"); + g_child_frame_id = mod.arg_required("child_frame_id"); + g_sensor_frame_id = mod.arg_required("sensor_frame_id"); float pointcloud_freq = mod.arg_float("pointcloud_freq", 5.0f); float odom_freq = mod.arg_float("odom_freq", 50.0f); @@ -362,7 +438,7 @@ int main(int argc, char** argv) { printf("[pointlio] Starting Point-LIO + Livox Mid-360 native module\n"); printf("[pointlio] lidar topic: %s\n", g_lidar_topic.empty() ? "(disabled)" : g_lidar_topic.c_str()); printf("[pointlio] odometry topic: %s\n", g_odometry_topic.empty() ? "(disabled)" : g_odometry_topic.c_str()); - printf("[pointlio] config: %s\n", config_path.c_str()); + printf("[pointlio] tuning: filter_size_surf=%.3f ivox_res=%.3f lidar_type=%d\n", params.filter_size_surf, params.ivox_grid_resolution, params.lidar_type); printf("[pointlio] host_ip: %s lidar_ip: %s frequency: %.1f Hz\n", host_ip.c_str(), lidar_ip.c_str(), g_frequency); printf("[pointlio] pointcloud_freq: %.1f Hz odom_freq: %.1f Hz\n", pointcloud_freq, odom_freq); } @@ -378,7 +454,7 @@ int main(int argc, char** argv) { g_lcm = &lcm; if (debug) { printf("[pointlio] Initializing Point-LIO...\n"); } - PointLio point_lio(config_path, msr_freq, main_freq); + PointLio point_lio(params, msr_freq, main_freq); g_point_lio = &point_lio; if (debug) { printf("[pointlio] Point-LIO initialized.\n"); } diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index 86cac95413..679d678b18 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -24,16 +24,17 @@ PointLio.blueprint(host_ip="192.168.1.5", lidar_ip="192.168.1.155"), SomeConsumer.blueprint(), )).loop() + +Point-LIO tuning lives directly on ``PointLioConfig`` and is passed to the C++ +binary as plain CLI args (no YAML). """ from __future__ import annotations import os -from pathlib import Path -from typing import TYPE_CHECKING, Annotated +from typing import TYPE_CHECKING, Literal from pydantic import Field -from pydantic.experimental.pipeline import validate_as from reactivex.disposable import Disposable from dimos.core.core import rpc @@ -57,10 +58,17 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.navigation.nav_stack.frames import FRAME_ODOM +from dimos.navigation.nav_stack.frames import FRAME_BODY, FRAME_ODOM from dimos.spec import perception -_CONFIG_DIR = Path(__file__).parent / "config" +# Human-readable enums; the C++ binary (main.cpp) maps these strings to +# Point-LIO's int codes. +# LID_TYPE enum (Point-LIO src/preprocess.h). avia = 1 selects the Livox branch +# the Mid-360 emits. +LidarType = Literal["avia", "velodyne", "ouster", "hesai", "unilidar"] +TimestampUnit = Literal["second", "millisecond", "microsecond", "nanosecond"] +# iVox local-map neighbour stencil. +IvoxNearbyType = Literal["center", "nearby6", "nearby18", "nearby26"] class PointLioConfig(NativeModuleConfig): @@ -73,11 +81,13 @@ class PointLioConfig(NativeModuleConfig): lidar_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_POINTLIO_LIDAR_IP")) frequency: float = 10.0 - # Sensor frame for the cloud + odometry headers. - frame_id: str = "mid360_link" - # Published TF: body_start_frame_id -> body_frame_id. - body_start_frame_id: str = FRAME_ODOM - body_frame_id: str = "base_link" + # Odometry is published as frame_id (fixed) -> child_frame_id (moving body), + # and also broadcast on TF. The point cloud is stamped with sensor_frame_id + # (the lidar's own frame — get_body_cloud is the undistorted scan, not yet + # transformed into the body frame). + frame_id: str = FRAME_ODOM + child_frame_id: str = FRAME_BODY + sensor_frame_id: str = "mid360_link" # Point-LIO internal processing rates (Hz) msr_freq: float = 50.0 @@ -86,14 +96,65 @@ class PointLioConfig(NativeModuleConfig): pointcloud_freq: float = 10.0 odom_freq: float = 30.0 - # Point-LIO YAML config (relative to config/ dir, or absolute path). - config: Annotated[ - Path, - validate_as(...).transform(lambda path: path if path.is_absolute() else _CONFIG_DIR / path), - ] = Path("default.yaml") - debug: bool = False + # Point-LIO tuning, passed to the binary as plain CLI args (read in main.cpp). + # common + con_frame: bool = False + con_frame_num: int = 1 + cut_frame: bool = False + cut_frame_time_interval: float = 0.1 + time_lag_imu_to_lidar: float = 0.0 + # preprocess + lidar_type: LidarType = "avia" # 1 = AVIA (Livox) branch the Mid-360 emits + scan_line: int = 4 + scan_rate: int = 10 + timestamp_unit: TimestampUnit = "nanosecond" + blind: float = 0.5 # spherical min range (m) + point_filter_num: int = 3 # pre-KF decimation: keep every Nth raw point (1 = all) + # mapping + use_imu_as_input: bool = False # false = IMU-as-output model (robust path) + prop_at_freq_of_imu: bool = True + check_satu: bool = True + init_map_size: int = 10 + space_down_sample: bool = True # pre-KF voxel downsample (leaf = filter_size_surf) + satu_acc: float = 3.0 # g; accel >= this is treated as saturated, bounding velocity + satu_gyro: float = 35.0 + acc_norm: float = 1.0 # IMU accel unit: g + plane_thr: float = 0.1 + filter_size_surf: float = 0.2 # pre-KF scan downsample leaf (m), iff space_down_sample + filter_size_map: float = 0.5 + ivox_grid_resolution: float = 2.0 # iVox local-map grid (m) + ivox_nearby_type: IvoxNearbyType = "nearby6" + cube_side_length: float = 1000.0 + det_range: float = 100.0 + fov_degree: float = 360.0 + imu_en: bool = True + start_in_aggressive_motion: bool = False + extrinsic_est_en: bool = False + imu_time_inte: float = 0.005 + lidar_meas_cov: float = 0.01 + acc_cov_input: float = 0.1 + vel_cov: float = 20.0 + gyr_cov_input: float = 0.01 + gyr_cov_output: float = 1000.0 + acc_cov_output: float = 500.0 + b_gyr_cov: float = 0.0001 + b_acc_cov: float = 0.0001 + imu_meas_acc_cov: float = 0.01 + imu_meas_omg_cov: float = 0.01 + match_s: float = 81.0 + gravity_align: bool = True + gravity: list[float] = Field(default_factory=lambda: [0.0, 0.0, -9.81]) + gravity_init: list[float] = Field(default_factory=lambda: [0.0, 0.0, -9.81]) + extrinsic_t: list[float] = Field(default_factory=lambda: [-0.011, -0.02329, 0.04412]) + extrinsic_r: list[float] = Field( + default_factory=lambda: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + ) + # odometry + publish_odometry_without_downsample: bool = False + odom_only: bool = False + # SDK port configuration (see livox/ports.py for defaults) cmd_data_port: int = SDK_CMD_DATA_PORT push_msg_port: int = SDK_PUSH_MSG_PORT @@ -106,18 +167,6 @@ class PointLioConfig(NativeModuleConfig): host_imu_data_port: int = SDK_HOST_IMU_DATA_PORT host_log_data_port: int = SDK_HOST_LOG_DATA_PORT - # Resolved in __post_init__, passed as --config_path to the binary - config_path: str | None = None - - cli_exclude: frozenset[str] = frozenset({"config", "body_start_frame_id"}) - - def model_post_init(self, __context: object) -> None: - super().model_post_init(__context) - cfg = self.config - if not cfg.is_absolute(): - cfg = _CONFIG_DIR / cfg - self.config_path = str(cfg.resolve()) - class PointLio(NativeModule, perception.Lidar, perception.Odometry): config: PointLioConfig @@ -136,8 +185,8 @@ def start(self) -> None: def _on_odom_for_tf(self, msg: Odometry) -> None: self.tf.publish( Transform( - frame_id=self.config.body_start_frame_id, - child_frame_id=self.config.body_frame_id, + frame_id=self.frame_id, + child_frame_id=self.config.child_frame_id, translation=Vector3( msg.pose.position.x, msg.pose.position.y, diff --git a/dimos/hardware/sensors/lidar/pointlio/recorder.py b/dimos/hardware/sensors/lidar/pointlio/recorder.py index e1afbd502c..ba9a3230f6 100644 --- a/dimos/hardware/sensors/lidar/pointlio/recorder.py +++ b/dimos/hardware/sensors/lidar/pointlio/recorder.py @@ -14,169 +14,44 @@ """Record Point-LIO odometry + lidar into a memory2 SQLite db. -Subscribes to a PointLio's ``odometry`` / ``lidar`` outputs (auto-connected by -matching stream name + type — no remappings needed) and appends them to a -memory2 store. Timestamps are converted onto the db's existing clock so a run -can be appended to an existing db (e.g. a fastlio replay) and compared on one -timeline. Owns the db lifecycle: refuses to clobber existing streams unless -``force``, and derives the alignment reference from whatever the db already holds. - -Each lidar frame is stamped with the latest odometry pose, so ``pointlio_lidar`` -carries the trajectory and ``dimos map global`` can register it directly (it -transforms the body-frame cloud by that pose) — no ``dimos map pose-fill`` pass. +A ``Recorder`` that records its In ports under their own names +(``pointlio_odometry`` / ``pointlio_lidar``) — wire them to PointLio's +``odometry`` / ``lidar`` outputs with ``.remappings()``. Poses come straight +from the odometry stream (``@pose_setter_for``): each lidar frame is stamped with +the latest odometry pose so ``pointlio_lidar`` carries the trajectory and ``dimos +map global`` can register the body-frame cloud directly (no ``pose-fill`` pass). """ from __future__ import annotations -from collections.abc import AsyncIterator -import math -from pathlib import Path -import sqlite3 -import time - -from dimos.core.module import Module, ModuleConfig 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 -# Below this the db's existing timestamps are sensor-boot seconds, not unix time. -_SENSOR_CLOCK_MAX = 1e8 -# Strictly-increasing tie-breaker so two samples never collide on ts. -_EPS = 1e-9 -# Max sensor-ts gap to attach the latest odometry pose to a lidar frame, so -# pointlio_lidar carries the trajectory and `dimos map global` can register it -# (it transforms by the per-frame pose). Matches pose_fill's nearest-match window. -_POSE_MATCH_TOL = 0.1 - - -def _existing_min_ts(db_path: Path) -> float: - """Min ts across the db's existing streams, or -1.0 if none/absent.""" - if not db_path.exists(): - return -1.0 - con = sqlite3.connect(f"file:{db_path}?mode=ro", uri=True, timeout=2.0) - try: - tables = [ - row[0] - for row in con.execute("SELECT name FROM sqlite_master WHERE type='table'").fetchall() - ] - best: float | None = None - for table in tables: - if table.startswith("_") or table.startswith("sqlite_"): - continue - try: - cols = [col[1] for col in con.execute(f"PRAGMA table_info('{table}')").fetchall()] - if "ts" not in cols: - continue - row = con.execute(f"SELECT MIN(ts) FROM '{table}'").fetchone() - except sqlite3.OperationalError: - continue - if row and row[0] is not None: - best = row[0] if best is None else min(best, row[0]) - return best if best is not None else -1.0 - finally: - con.close() - - -class PointlioRecorderConfig(ModuleConfig): - """Configures the recorder with the target db and timing conversion.""" - db_path: str = "" - # db stream/table names the Point-LIO outputs are recorded under. - odom_stream_name: str = "pointlio_odometry" - lidar_stream_name: str = "pointlio_lidar" - # Explicit offset override; NaN means auto-derive from the db's earliest ts. - time_offset: float = float("nan") - # Drop pre-existing odom/lidar streams instead of refusing to overwrite. - force: bool = False +class PointlioRecorderConfig(RecorderConfig): + # Append into a populated db (keep other streams); replace only our own. + on_existing: OnExisting = OnExisting.APPEND -class PointlioRecorder(Module): +class PointlioRecorder(Recorder): config: PointlioRecorderConfig - lidar: In[PointCloud2] - odometry: In[Odometry] - _offset: float | None = None - _ref_start_ts: float = -1.0 - _last_odom_ts: float = 0.0 - _last_lidar_ts: float = 0.0 - _odom_count: int = 0 - _lidar_count: int = 0 - # Latest odometry pose + its raw sensor ts, stamped onto each lidar frame so - # pointlio_lidar carries the trajectory (no separate pose-fill pass). - _last_odom_pose: Pose | None = None - _last_odom_raw_ts: float = 0.0 - - async def main(self) -> AsyncIterator[None]: - # Deferred: the store is opened in the worker process that runs main(), - # not at module-scan/import time on the host. - from dimos.memory2.store.sqlite import SqliteStore - - cfg = self.config - self._store = SqliteStore(path=cfg.db_path) - names = (cfg.odom_stream_name, cfg.lidar_stream_name) - existing = sorted(set(self._store.list_streams()) & set(names)) - if existing and not cfg.force: - raise RuntimeError( - f"PointlioRecorder: {Path(cfg.db_path).name} already has {existing}; " - "set force=True to overwrite" - ) - for name in existing: - self._store.delete_stream(name) - # Reference is the db's earliest ts *after* dropping our own old streams, - # so only the data we're aligning against (e.g. a fastlio replay) counts. - self._ref_start_ts = _existing_min_ts(Path(cfg.db_path)) - self._os = self._store.stream(cfg.odom_stream_name, Odometry) - self._ls = self._store.stream(cfg.lidar_stream_name, PointCloud2) - yield - self._store.stop() + pointlio_odometry: In[Odometry] + pointlio_lidar: In[PointCloud2] - def _resolve_offset(self, first_ts: float) -> float: - override = self.config.time_offset - if not math.isnan(override): - return override - ref = self._ref_start_ts - if ref < 0.0 or ref < _SENSOR_CLOCK_MAX: - # Empty db, or db already on the sensor clock -> exact alignment. - return 0.0 - # db on wall-clock time -> start-align Point-LIO onto the db's earliest ts. - return ref - first_ts - - def _aligned_ts(self, raw_ts: float, last_ts: float) -> float: - """Convert a sensor ts onto the db clock, kept strictly above last_ts.""" - if self._offset is None: - self._offset = self._resolve_offset(raw_ts) - return max(raw_ts + self._offset, last_ts + _EPS) + _last_odom_pose: Pose | None = None - async def handle_odometry(self, msg: Odometry) -> None: - # `is not None`, not `or`: a real sensor ts of 0.0 must not fall back to - # wall time (would misclassify the stream's clock in _resolve_offset). - raw_ts_raw = getattr(msg, "ts", None) - raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() - ts = self._aligned_ts(raw_ts, self._last_odom_ts) - self._last_odom_ts = ts + @pose_setter_for("pointlio_odometry") + def _odom_pose(self, msg: Odometry) -> Pose | None: pose = getattr(msg, "pose", None) - pose_inner = getattr(pose, "pose", None) if pose is not None else None - self._os.append(msg, ts=ts, pose=pose_inner) - self._last_odom_pose = pose_inner - self._last_odom_raw_ts = raw_ts - self._odom_count += 1 - - async def handle_lidar(self, msg: PointCloud2) -> None: - raw_ts_raw = getattr(msg, "ts", None) - raw_ts = raw_ts_raw if raw_ts_raw is not None else time.time() - ts = self._aligned_ts(raw_ts, self._last_lidar_ts) - self._last_lidar_ts = ts - # Stamp the latest odometry pose (within tolerance) onto the frame so - # pointlio_lidar carries the trajectory; map global transforms the - # body-frame cloud by it. Both Point-LIO outputs share a publish ts, so - # the nearest odometry is at most ~one odom period stale. Frames with no - # match (e.g. before the first odometry) get None and are map-skipped. - pose = ( - self._last_odom_pose - if self._last_odom_pose is not None - and abs(raw_ts - self._last_odom_raw_ts) <= _POSE_MATCH_TOL - else None - ) - self._ls.append(msg, ts=ts, pose=pose) - self._lidar_count += 1 + 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/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py index a39678b549..b842634e33 100644 --- a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py @@ -18,10 +18,12 @@ # LFS archive, NOT the archive name) PCAP_PATH="$(python -c "from dimos.utils.data import get_data; print(get_data('mid360_shake_stairs/mid360_shake_stairs.pcap'))")" - # gen .db from pcap with your config (defaults to .db next to the pcap) + # gen .db from pcap (defaults to .db next to the pcap) + python -m dimos.hardware.sensors.lidar.pointlio.scripts.pcap_to_db --pcap "$PCAP_PATH" + + # override any PointLioConfig field via a small YAML/JSON doc, e.g. {acc_cov_input: 0.3} python -m dimos.hardware.sensors.lidar.pointlio.scripts.pcap_to_db \ - --config dimos/hardware/sensors/lidar/pointlio/config/default.yaml \ - --pcap "$PCAP_PATH" + --pcap "$PCAP_PATH" --config overrides.yaml # add to existing .db DB="mem2.db" @@ -61,6 +63,9 @@ _STARTUP_TIMEOUT_SEC = 60.0 # Max |Δts| to match a lidar frame to an odometry pose when aggregating the .rrd. _POSE_MATCH_TOL = 0.1 +# db stream/table names (= the recorder's In-port names). +_ODOM_STREAM = "pointlio_odometry" +_LIDAR_STREAM = "pointlio_lidar" # Extra seconds past the pcap's own duration before auto-stopping, when no # explicit --max-sensor-sec is given. _DRAIN_MARGIN_SEC = 4.0 @@ -199,7 +204,9 @@ def _write_rrd(db_path: Path, odom_stream: str, lidar_stream: str, voxel: float) store.stop() -def _build_blueprint(args: argparse.Namespace, db_path: Path, config_path: str) -> Blueprint: +def _build_blueprint( + args: argparse.Namespace, db_path: Path, overrides: dict[str, Any] +) -> Blueprint: """autoconnect(VirtualMid360 + PointLio + PointlioRecorder). PointLio's ``odometry``/``lidar`` outputs auto-wire to the recorder's @@ -211,36 +218,35 @@ def _build_blueprint(args: argparse.Namespace, db_path: Path, config_path: str) from dimos.hardware.sensors.lidar.pointlio.recorder import PointlioRecorder from dimos.hardware.sensors.lidar.virtual_mid360.module import VirtualMid360 - # `config` (not `config_path`, which PointLioConfig derives itself); already - # an absolute path so it bypasses the config-dir-relative resolution. Omit - # when empty to keep the default.yaml. - pointlio_kwargs: dict[str, object] = dict( + pointlio_kwargs: dict[str, Any] = dict( host_ip=args.host_ip, lidar_ip=args.lidar_ip, odom_freq=args.odom_freq, debug=False ) - if config_path: - pointlio_kwargs["config"] = config_path - - return autoconnect( - VirtualMid360.blueprint( - pcap=str(args.pcap_path), - rate=args.rate, - delay=args.warmup_sec, # hold streaming until PointLio's SDK is up - host_ip=args.host_ip, - lidar_ip=args.lidar_ip, - alias_iface=args.alias_iface, - # When the NIC is provisioned by hand, skip the module's own sudo - # (it runs in a tty-less worker where a password prompt can't appear). - setup_network=not args.no_network_setup, - ), - PointLio.blueprint(**pointlio_kwargs), - PointlioRecorder.blueprint( - db_path=str(db_path), - odom_stream_name=args.odom_stream_name, - lidar_stream_name=args.lidar_stream_name, - time_offset=float("nan") if args.time_offset is None else args.time_offset, - force=args.force, - ), - ).global_config(n_workers=4, robot_model="mid360_pointlio_pcap_to_db") + pointlio_kwargs.update(overrides) + + return ( + autoconnect( + VirtualMid360.blueprint( + pcap=str(args.pcap_path), + rate=args.rate, + delay=args.warmup_sec, # hold streaming until PointLio's SDK is up + host_ip=args.host_ip, + lidar_ip=args.lidar_ip, + alias_iface=args.alias_iface, + # When the NIC is provisioned by hand, skip the module's own sudo + # (it runs in a tty-less worker where a password prompt can't appear). + setup_network=not args.no_network_setup, + ), + PointLio.blueprint(**pointlio_kwargs), + PointlioRecorder.blueprint(db_path=str(db_path)), + ) + .remappings( + [ + (PointlioRecorder, "pointlio_odometry", "odometry"), + (PointlioRecorder, "pointlio_lidar", "lidar"), + ] + ) + .global_config(n_workers=4, robot_model="mid360_pointlio_pcap_to_db") + ) def _poll_until_drained( @@ -289,6 +295,21 @@ def _poll_until_drained( stagnant_since = None +def _load_overrides(config: str) -> dict[str, Any]: + """Load a YAML/JSON doc of PointLioConfig field overrides, e.g. {acc_cov_input: 0.3}.""" + if not config: + return {} + import yaml + + path = Path(config).expanduser().resolve() + if not path.exists(): + raise FileNotFoundError(f"--config not found: {path}") + data = yaml.safe_load(path.read_text()) or {} + if not isinstance(data, dict): + raise ValueError(f"--config must be a mapping of PointLioConfig fields, got {type(data)}") + return data + + def _run(args: argparse.Namespace) -> int: from dimos.core.coordination.module_coordinator import ModuleCoordinator @@ -299,13 +320,7 @@ def _run(args: argparse.Namespace) -> int: args.pcap_path = pcap_path db_path = Path(args.db).expanduser().resolve() if args.db else pcap_path.with_suffix(".db") db_path.parent.mkdir(parents=True, exist_ok=True) - # Resolve --config against the *invoking* cwd (pwd-relative) up front; the - # PointLio config field otherwise resolves a relative path against its own - # config/ dir, never the pwd. Absolute path passes through unchanged. - config_path = str(Path(args.config).expanduser().resolve()) if args.config else "" - if config_path and not Path(config_path).exists(): - print(f"[pcap_to_db] missing --config: {config_path}", file=sys.stderr) - return 2 + overrides = _load_overrides(args.config) # Default the stop bound to the pcap's own duration: Point-LIO keeps # dead-reckoning (publishing at full rate) after the pcap drains, so the @@ -326,15 +341,13 @@ def _run(args: argparse.Namespace) -> int: coord = None try: - coord = ModuleCoordinator.build(_build_blueprint(args, db_path, config_path)) - drained = _poll_until_drained( - db_path, args.odom_stream_name, args.lidar_stream_name, max_sensor_sec - ) + coord = ModuleCoordinator.build(_build_blueprint(args, db_path, overrides)) + drained = _poll_until_drained(db_path, _ODOM_STREAM, _LIDAR_STREAM, max_sensor_sec) finally: if coord is not None: coord.stop() - o_cnt, o_min, o_max = _odom_stats(db_path, args.odom_stream_name) + o_cnt, o_min, o_max = _odom_stats(db_path, _ODOM_STREAM) if o_cnt == 0 or not drained: print("[pcap_to_db] no odometry recorded — check the run above", file=sys.stderr) return 1 @@ -344,7 +357,7 @@ def _run(args: argparse.Namespace) -> int: ) if not args.no_rrd: try: - rrd = _write_rrd(db_path, args.odom_stream_name, args.lidar_stream_name, args.voxel) + rrd = _write_rrd(db_path, _ODOM_STREAM, _LIDAR_STREAM, args.voxel) if rrd is not None: print(f"[pcap_to_db] wrote {rrd.name} (aggregated lidar + pose path)", flush=True) except Exception as exc: # viz is a non-fatal bonus @@ -373,13 +386,6 @@ def main(argv: list[str]) -> int: default=0.0, help="stop after N sensor seconds (0 = whole pcap)", ) - parser.add_argument( - "--time-offset", - type=float, - default=None, - help="seconds added to every output ts (auto if omitted)", - ) - parser.add_argument("--force", action="store_true", help="overwrite existing pointlio streams") parser.add_argument( "--no-rrd", action="store_true", @@ -397,17 +403,7 @@ def main(argv: list[str]) -> int: parser.add_argument( "--config", default="", - help="Point-LIO YAML config (pwd-relative or absolute; default: module's default.yaml)", - ) - parser.add_argument( - "--odom-stream-name", - default="pointlio_odometry", - help="db stream/table name for the recorded odometry", - ) - parser.add_argument( - "--lidar-stream-name", - default="pointlio_lidar", - help="db stream/table name for the recorded point cloud", + help="YAML/JSON doc of PointLioConfig field overrides (e.g. {acc_cov_input: 0.3})", ) # Addressing knobs (override to run two replays at once). parser.add_argument("--host-ip", default="192.168.1.5") From 1a2d1299af65d385e1682a16ab504028e0c17aab Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sat, 20 Jun 2026 09:22:10 -0700 Subject: [PATCH 176/185] feat(zed): SDK-free ZedSimple color + IMU into go2_mid360 recording ZedSimple: UVC color (YUYV side-by-side, left eye) plus ZED-M IMU over USB-HID via the zed-open-capture protocol; used when pyzed is absent. Color and IMU run as independent fail-soft loops. (hid is an optional runtime dep; the module logs and skips IMU if it or libhidapi is missing.) record.py: - Select ZEDCamera (SDK) or ZedSimple, remap color_image->zed_color_image and imu->zed_imu; record both. Add an imu Out stream to ZEDCamera too. - Replace the in-process TUI teleop with the KeyboardTeleop module (cmd_vel->tele_cmd_vel); sit/stand via the auto-wired GO2ConnectionSpec. Mid360: auto-correct host_ip to a local interface on the lidar subnet (mirrors FastLio2), fixing the native bind failure when the configured host_ip is not assigned to this machine. --- dimos/hardware/sensors/camera/zed/camera.py | 78 ++++--- dimos/hardware/sensors/camera/zed/simple.py | 218 +++++++++++++++++++ dimos/hardware/sensors/lidar/livox/module.py | 41 ++++ dimos/mapping/recording/go2_mid360/record.py | 73 ++++--- pyproject.toml | 1 + 5 files changed, 352 insertions(+), 59 deletions(-) create mode 100644 dimos/hardware/sensors/camera/zed/simple.py diff --git a/dimos/hardware/sensors/camera/zed/camera.py b/dimos/hardware/sensors/camera/zed/camera.py index 5203fa9242..ba95610686 100644 --- a/dimos/hardware/sensors/camera/zed/camera.py +++ b/dimos/hardware/sensors/camera/zed/camera.py @@ -14,7 +14,7 @@ from __future__ import annotations -import atexit +import math import threading import time @@ -24,11 +24,11 @@ import reactivex as rx from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT +from dimos.core.coordination.blueprints import autoconnect from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import Out -from dimos.core.transport import LCMTransport from dimos.hardware.sensors.camera.spec import ( OPTICAL_ROTATION, DepthCameraConfig, @@ -39,9 +39,11 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.sensor_msgs.Imu import Imu from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.spec import perception from dimos.utils.reactive import backpressure +from dimos.visualization.vis_module import vis_module def default_base_transform() -> Transform: @@ -61,6 +63,7 @@ class ZEDCameraConfig(ModuleConfig, DepthCameraConfig): base_transform: Transform | None = Field(default_factory=default_base_transform) align_depth_to_color: bool = True enable_depth: bool = True + enable_imu: bool = True enable_pointcloud: bool = False pointcloud_fps: float = 5.0 camera_info_fps: float = 1.0 @@ -81,6 +84,7 @@ class ZEDCamera(DepthCameraHardware, Module, perception.DepthCamera): config: ZEDCameraConfig color_image: Out[Image] depth_image: Out[Image] + imu: Out[Imu] pointcloud: Out[PointCloud2] camera_info: Out[CameraInfo] depth_camera_info: Out[CameraInfo] @@ -89,6 +93,10 @@ class ZEDCamera(DepthCameraHardware, Module, perception.DepthCamera): def _camera_link(self) -> str: return f"{self.config.camera_name}_link" + @property + def _imu_frame(self) -> str: + return f"{self.config.camera_name}_imu_link" + @property def _color_frame(self) -> str: return f"{self.config.camera_name}_color_frame" @@ -121,6 +129,8 @@ def __init__(self, *args, **kwargs) -> None: # type: ignore[no-untyped-def] self._pointcloud_lock = threading.Lock() self._image_left: sl.Mat | None = None self._depth_map: sl.Mat | None = None + self._sensors_data: sl.SensorsData | None = None + self._has_imu = False self._pose: sl.Pose | None = None self._tracking_enabled = False self._stream_width = self.config.width @@ -166,11 +176,16 @@ def start(self) -> None: self._image_left = sl.Mat() self._depth_map = sl.Mat() self._pose = sl.Pose() + self._sensors_data = sl.SensorsData() self._sl_camera_info = self._zed.get_camera_information() if self._sl_camera_info is not None: self._stream_width = self._sl_camera_info.camera_configuration.resolution.width self._stream_height = self._sl_camera_info.camera_configuration.resolution.height + sensors_config = self._sl_camera_info.sensors_configuration + self._has_imu = self.config.enable_imu and sensors_config.is_sensor_available( + sl.SENSOR_TYPE.ACCELEROMETER + ) self._build_camera_info() self._get_extrinsics() @@ -330,8 +345,33 @@ def _capture_loop(self) -> None: self._latest_color_img = color_img self._latest_depth_img = depth_img + if self._has_imu: + self._publish_imu(ts) + self._publish_tf(ts) + def _publish_imu(self, ts: float) -> None: + if self._zed is None or self._sensors_data is None: + return + if ( + self._zed.get_sensors_data(self._sensors_data, sl.TIME_REFERENCE.IMAGE) + != sl.ERROR_CODE.SUCCESS + ): + return + imu_data = self._sensors_data.get_imu_data() + angular_velocity_deg = imu_data.get_angular_velocity() + linear_acceleration = imu_data.get_linear_acceleration() + orientation = imu_data.get_pose().get_orientation().get() + self.imu.publish( + Imu( + angular_velocity=Vector3(*(math.radians(v) for v in angular_velocity_deg)), + linear_acceleration=Vector3(*linear_acceleration), + orientation=Quaternion(*orientation), + frame_id=self._imu_frame, + ts=ts, + ) + ) + def _tracking_transform(self, ts: float) -> Transform | None: if not self._tracking_enabled or self._zed is None or self._pose is None: return None @@ -471,6 +511,8 @@ def stop(self) -> None: self._latest_depth_img = None self._image_left = None self._depth_map = None + self._sensors_data = None + self._has_imu = False self._pose = None self._sl_camera_info = None self._tracking_enabled = False @@ -490,33 +532,13 @@ def get_depth_scale(self) -> float: def main() -> None: - dimos = ModuleCoordinator() - dimos.start() + from dimos.core.global_config import global_config - camera = dimos.deploy(ZEDCamera, enable_pointcloud=True, pointcloud_fps=5.0) - camera.color_image.transport = LCMTransport("/camera/color", Image) - camera.depth_image.transport = LCMTransport("/camera/depth", Image) - camera.pointcloud.transport = LCMTransport("/camera/pointcloud", PointCloud2) - camera.camera_info.transport = LCMTransport("/camera/color_info", CameraInfo) - camera.depth_camera_info.transport = LCMTransport("/camera/depth_info", CameraInfo) - - def cleanup() -> None: - try: - dimos.stop() - except Exception: - pass - - atexit.register(cleanup) - dimos.start_all_modules() - - try: - while True: - time.sleep(0.1) - except (KeyboardInterrupt, SystemExit): - pass - finally: - atexit.unregister(cleanup) - cleanup() + blueprint = autoconnect( + ZEDCamera.blueprint(enable_pointcloud=True, pointcloud_fps=5.0), + vis_module(global_config.viewer), + ) + ModuleCoordinator.build(blueprint).loop() if __name__ == "__main__": diff --git a/dimos/hardware/sensors/camera/zed/simple.py b/dimos/hardware/sensors/camera/zed/simple.py new file mode 100644 index 0000000000..36c7622e87 --- /dev/null +++ b/dimos/hardware/sensors/camera/zed/simple.py @@ -0,0 +1,218 @@ +# Copyright 2025-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 __future__ import annotations + +import asyncio +from collections.abc import AsyncIterator +import glob +import math +import os +import struct +import time + +import cv2 + +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import Out +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +# ZED-M IMU over USB HID — protocol from Stereolabs' open-source zed-open-capture. +_ZED_USB_VENDOR = 0x2B03 +_ZED_M_MCU_PID = 0xF681 +_REP_ID_SENSOR_STREAM_STATUS = 0x32 +# Packed RawData struct (sensorcapture_def.hpp); we only read the IMU fields. +_RAW_DATA_FMT = " m/s^2 (+-8g range) +_GYRO_SCALE = (1000.0 / 32768.0) * (math.pi / 180.0) # raw -> rad/s (+-1000 deg/s) + + +def autodetect_zed_device() -> str | None: + """Resolve the ZED's UVC ``/dev/video*`` node via its v4l by-id symlink.""" + for link in sorted(glob.glob("/dev/v4l/by-id/*")): + if "ZED" in os.path.basename(link).upper(): + return os.path.realpath(link) + return None + + +class ZedSimpleConfig(ModuleConfig): + device: str | None = None + # Full side-by-side frame; the ZED is YUYV-only. 2560x720 => 1280x720 per eye. + width: int = 2560 + height: int = 720 + fps: int = 15 + side: str = "left" + fourcc: str = "YUYV" + enable_imu: bool = True + imu_pid: int = _ZED_M_MCU_PID + camera_name: str = "zed" + + +class ZedSimple(Module): + """SDK-free ZED color + IMU capture. + + Fallback for when the ZED SDK / ``pyzed`` is not installed. Color comes from + the UVC stereo stream (one side-by-side frame; we slice out one eye and + publish ``color_image``). The IMU is read straight off the camera's USB-HID + device using Stereolabs' open ``zed-open-capture`` protocol and published as + ``imu``. No depth or pointcloud — use the SDK-backed ``ZEDCamera`` for those. + + Color (UVC) and IMU (HID) are independent USB interfaces, so each runs and + fails independently: a missing camera does not stop the IMU and vice versa. + """ + + config: ZedSimpleConfig + color_image: Out[Image] + imu: Out[Imu] + + def __init__(self, *args, **kwargs) -> None: # type: ignore[no-untyped-def] + super().__init__(*args, **kwargs) + self._capture: cv2.VideoCapture | None = None + self._running = False + self._video_task: asyncio.Task[None] | None = None + self._imu_task: asyncio.Task[None] | None = None + + @property + def _optical_frame(self) -> str: + return f"{self.config.camera_name}_{self.config.side}_color_optical_frame" + + @property + def _imu_frame(self) -> str: + return f"{self.config.camera_name}_imu_link" + + def _open(self) -> cv2.VideoCapture: + device = self.config.device or autodetect_zed_device() + if device is None: + raise RuntimeError( + "No ZED UVC video device found under /dev/v4l/by-id. Confirm the " + "camera is on a USB-3 port, or set ZedSimpleConfig.device explicitly." + ) + capture = cv2.VideoCapture(device, cv2.CAP_V4L2) + if not capture.isOpened(): + raise RuntimeError(f"Failed to open ZED video device: {device}") + if self.config.fourcc: + capture.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc(*self.config.fourcc)) + if self.config.width: + capture.set(cv2.CAP_PROP_FRAME_WIDTH, self.config.width) + if self.config.height: + capture.set(cv2.CAP_PROP_FRAME_HEIGHT, self.config.height) + if self.config.fps: + capture.set(cv2.CAP_PROP_FPS, self.config.fps) + return capture + + async def main(self) -> AsyncIterator[None]: + self._running = True + + # IMU (USB-HID) runs independently of the camera (UVC). + if self.config.enable_imu: + self._imu_task = asyncio.create_task(asyncio.to_thread(self._imu_loop)) + + # Fail soft: a missing/flaky camera must not abort a multi-sensor + # recording (nor stop the IMU). Log loudly and run without frames. + try: + self._capture = await asyncio.to_thread(self._open) + except RuntimeError as error: + logger.error(f"ZedSimple: camera unavailable, no color_image will publish: {error}") + if self._capture is not None: + width = int(self._capture.get(cv2.CAP_PROP_FRAME_WIDTH)) + height = int(self._capture.get(cv2.CAP_PROP_FRAME_HEIGHT)) + logger.info( + f"ZedSimple: streaming {width}x{height} side-by-side, publishing {self.config.side} eye" + ) + self._video_task = asyncio.create_task(asyncio.to_thread(self._capture_loop)) + + yield + + self._running = False + for task in (self._video_task, self._imu_task): + if task is not None: + await task + if self._capture is not None: + self._capture.release() + self._capture = None + + def _capture_loop(self) -> None: + if self._capture is None: + return + while self._running: + ok, frame = self._capture.read() + if not ok: + time.sleep(0.01) + continue + half_width = frame.shape[1] // 2 + eye = frame[:, :half_width] if self.config.side == "left" else frame[:, half_width:] + self.color_image.publish( + Image( + data=cv2.cvtColor(eye, cv2.COLOR_BGR2RGB), + format=ImageFormat.RGB, + frame_id=self._optical_frame, + ts=time.time(), + ) + ) + + def _imu_loop(self) -> None: + try: + import hid + except ImportError: + logger.error("ZedSimple: `hid` not installed, no IMU will publish (pip install hid)") + return + + try: + device = hid.Device(_ZED_USB_VENDOR, self.config.imu_pid) + except Exception as error: + logger.error(f"ZedSimple: IMU HID device unavailable, no imu will publish: {error}") + return + + try: + device.send_feature_report(bytes([_REP_ID_SENSOR_STREAM_STATUS, 0x01])) + # ROS convention: orientation unknown (raw IMU, no fusion) -> covariance[0] = -1. + unknown_orientation_covariance = [-1.0] + [0.0] * 8 + while self._running: + report = device.read(_RAW_DATA_SIZE, timeout=200) + if not report or len(report) < _RAW_DATA_SIZE: + continue + fields = struct.unpack(_RAW_DATA_FMT, bytes(report[:_RAW_DATA_SIZE])) + imu_not_valid = fields[1] + if imu_not_valid: + continue + # fields: struct_id, imu_not_valid, timestamp, gX,gY,gZ, aX,aY,aZ, ... + gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z = fields[3:9] + self.imu.publish( + Imu( + angular_velocity=Vector3( + gyro_x * _GYRO_SCALE, gyro_y * _GYRO_SCALE, gyro_z * _GYRO_SCALE + ), + linear_acceleration=Vector3( + acc_x * _ACC_SCALE, acc_y * _ACC_SCALE, acc_z * _ACC_SCALE + ), + orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + orientation_covariance=unknown_orientation_covariance, + frame_id=self._imu_frame, + ts=time.time(), + ) + ) + finally: + try: + device.send_feature_report(bytes([_REP_ID_SENSOR_STREAM_STATUS, 0x00])) + except Exception: + pass + device.close() diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index e7507913c3..ac40196364 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -27,6 +27,7 @@ from __future__ import annotations +import ipaddress from typing import TYPE_CHECKING from dimos.core.core import rpc @@ -47,6 +48,10 @@ from dimos.msgs.sensor_msgs.Imu import Imu from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.spec import perception +from dimos.utils.generic import get_local_ips +from dimos.utils.logging_config import setup_logger + +_logger = setup_logger() class Mid360Config(NativeModuleConfig): @@ -90,12 +95,48 @@ class Mid360(NativeModule, perception.Lidar, perception.IMU): @rpc def start(self) -> None: + self._correct_host_ip() super().start() @rpc def stop(self) -> None: super().stop() + def _correct_host_ip(self) -> None: + """Auto-correct ``host_ip`` to a local interface on the lidar's subnet. + + The native driver binds to ``host_ip``; if it is not an address on this + machine the bind fails and the process dies. Mirrors FastLio2 so both + agree on the host address regardless of which machine runs the stack. + """ + host_ip = self.config.host_ip + lidar_ip = self.config.lidar_ip + local_ips = [ip for ip, _iface in get_local_ips()] + if host_ip in local_ips: + return + try: + lidar_net = ipaddress.IPv4Network(f"{lidar_ip}/24", strict=False) + same_subnet = [ip for ip in local_ips if ipaddress.IPv4Address(ip) in lidar_net] + except (ValueError, TypeError): + same_subnet = [] + if not same_subnet: + _logger.warning( + f"Mid360: host_ip={host_ip!r} not assigned locally and no interface shares " + f"the lidar subnet ({lidar_ip}); bind will likely fail.", + local_ips=local_ips, + ) + return + picked = same_subnet[0] + _logger.warning( + f"Mid360: host_ip={host_ip!r} not found locally. " + f"Auto-correcting to {picked!r} (same subnet as lidar {lidar_ip}).", + configured_ip=host_ip, + corrected_ip=picked, + lidar_ip=lidar_ip, + local_ips=local_ips, + ) + self.config.host_ip = picked + # Verify protocol port compliance (mypy will flag missing ports) if TYPE_CHECKING: diff --git a/dimos/mapping/recording/go2_mid360/record.py b/dimos/mapping/recording/go2_mid360/record.py index ef14c4d8be..6599a86c43 100644 --- a/dimos/mapping/recording/go2_mid360/record.py +++ b/dimos/mapping/recording/go2_mid360/record.py @@ -23,7 +23,6 @@ from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.core.global_config import global_config from dimos.core.stream import In -from dimos.core.transport import LCMTransport from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 from dimos.hardware.sensors.lidar.fastlio2.recorder import FastLio2Recorder, _default_recording_dir from dimos.hardware.sensors.lidar.fastlio2.speed_warner import SpeedWarner @@ -35,14 +34,13 @@ from dimos.memory2.stream import Stream from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.Imu import Imu from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.robot.unitree.go2.connection import GO2Connection -from dimos.robot.unitree.keyboard_teleop_tui import KeyboardTeleopTUI +from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop from dimos.utils.logging_config import set_run_log_dir, setup_logger logger = setup_logger() @@ -70,13 +68,14 @@ class Go2TfHackRecorder(FastLio2Recorder): go2_lidar: In[PointCloud2] go2_odom: In[PoseStamped] color_image: In[Image] + zed_color_image: In[Image] + zed_imu: In[Imu] livox_lidar: In[PointCloud2] livox_imu: In[Imu] # Shadow the parent's generic companion ports so they're not recorded as # empty `lidar`/`odom` streams; the go2-prefixed ports above take their place. lidar: None = None # type: ignore[assignment] odom: None = None # type: ignore[assignment] - tf: In[Transform] _latest_fastlio_odom: Odometry | None = None _warning_names: set[str] = set() @@ -94,7 +93,7 @@ def on_msg(msg: Any) -> None: world_to_base = self._world_to_base_from_fastlio() if world_to_base is not None: pose = world_to_base.to_pose() - elif name in ("color_image", "go2_color_image"): + elif name in ("color_image", "go2_color_image", "zed_color_image"): # anchor images to world frame as defined by fastlio odom world_to_base = self._world_to_base_from_fastlio() if world_to_base is not None: @@ -128,7 +127,36 @@ class FastLio2NoCap(FastLio2): pass +def _zed_camera_blueprint() -> Any: + """ZED color source, remapped to ``zed_color_image``. + + Prefer the SDK-backed ``ZEDCamera`` (depth/imu/pointcloud); fall back to the + UVC-only ``ZedSimple`` (color only) when ``pyzed`` is not installed. + """ + try: + import pyzed.sl # noqa: F401 + + from dimos.hardware.sensors.camera.zed.camera import ZEDCamera + + return ZEDCamera.blueprint(enable_depth=False, enable_pointcloud=False).remappings( + [ + (ZEDCamera, "color_image", "zed_color_image"), + (ZEDCamera, "imu", "zed_imu"), + ] + ) + except ImportError: + from dimos.hardware.sensors.camera.zed.simple import ZedSimple + + return ZedSimple.blueprint().remappings( + [ + (ZedSimple, "color_image", "zed_color_image"), + (ZedSimple, "imu", "zed_imu"), + ] + ) + + unitree_go2_record = autoconnect( + _zed_camera_blueprint(), MovementManager.blueprint(), GO2Connection.blueprint().remappings( [ @@ -161,6 +189,14 @@ class FastLio2NoCap(FastLio2): (SpeedWarner, "odometry", "fastlio_odometry"), ] ), + # Pygame keyboard teleop (WASD + Q/E, Z=lie down, X=stand). Its cmd_vel + # feeds MovementManager's tele_cmd_vel; sit/stand are handled internally + # via the auto-wired GO2ConnectionSpec. + KeyboardTeleop.blueprint(linear_speed=0.3, angular_speed=0.6).remappings( + [ + (KeyboardTeleop, "cmd_vel", "tele_cmd_vel"), + ] + ), ).global_config(n_workers=10, robot_model="unitree_go2") @@ -173,29 +209,4 @@ class FastLio2NoCap(FastLio2): unitree_go2_record, {Go2TfHackRecorder.name: {"recording_dir": recording_dir}}, ) - - # Sit/stand drive the Go2 directly via its RPCs. After standing the dog must - # re-enter BalanceStand before it will walk again, so on_stand chains - # standup -> balance_stand (mirrors GO2Connection.start()). - go2 = coordinator.get_instance(GO2Connection) - - def stand_and_ready() -> None: - go2.standup() - time.sleep(3.0) - go2.balance_stand() - - # Launch the TUI teleop in THIS process (not via the coordinator) so it owns - # the terminal's stdin and can read WASD keypresses. Its output is wired onto - # the same /tele_cmd_vel topic MovementManager subscribes to. - teleop = KeyboardTeleopTUI( - linear_speed=0.3, - angular_speed=0.6, - on_sit=go2.liedown, - on_stand=stand_and_ready, - ) - teleop.tele_cmd_vel.transport = LCMTransport("/tele_cmd_vel", Twist) - teleop.start() - try: - coordinator.loop() - finally: - teleop.stop() + coordinator.loop() diff --git a/pyproject.toml b/pyproject.toml index 6806733e06..1057bb43bd 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -530,6 +530,7 @@ module = [ "mcap", "mcap.*", "mujoco", + "hid", "mujoco_playground.*", "nav_msgs.*", "open_clip", From afadd1f0e2f563763951c2015ce23545f9e9bc14 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 22 Jun 2026 01:23:58 +0800 Subject: [PATCH 177/185] - --- dimos/memory2/module.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/dimos/memory2/module.py b/dimos/memory2/module.py index 600f82a592..00cd46a1d0 100644 --- a/dimos/memory2/module.py +++ b/dimos/memory2/module.py @@ -261,6 +261,7 @@ class OnExisting(str, enum.Enum): class RecorderConfig(MemoryModuleConfig): on_existing: OnExisting = OnExisting.BACKUP backup_keep_last: int = Field(default=10, ge=0) + root_frame: str = "world" default_frame_id: str = "base_link" tf_tolerance: float = 0.5 db_path: str | Path = "recording.db" @@ -409,7 +410,7 @@ def _resolve_pose(self, name: str, msg: Any, ts: float) -> Pose | None: return cast("Pose | None", setter(msg)) frame_id = getattr(msg, "frame_id", None) or self.config.default_frame_id transform = self.tf.get( - "world", frame_id, time_point=ts, time_tolerance=self.config.tf_tolerance + self.config.root_frame, frame_id, time_point=ts, time_tolerance=self.config.tf_tolerance ) return transform.to_pose() if transform is not None else None From 0edae56a8f0744bf5c3290cd6e12579fc38c8aa8 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 22 Jun 2026 02:31:16 +0800 Subject: [PATCH 178/185] feat(recording): switch go2_mid360 + mid360_realsense to Point-LIO MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replace FastLio2 with PointLio in both recording drivers. PointlioRecorder stamps each lidar frame with the live odometry pose at record time, so the drivers drop the FastLio2 TfHack static-transform pose logic entirely — they just declare the companion In ports and let the recorder handle poses. Rename the recorded stream names fastlio_* -> pointlio_* throughout the post-process toolchain (gtsam_gt, build_rrd, rec_check, multi_map_anchor) so recordings post-process and visualize. multi_map_anchor: drop the dead lidar_reanchor import (removed in the post-process refactor) and use the recorder-stamped pointlio_lidar for the map viz. Add docs/capabilities/navigation/recording_a_map.md guide. Regenerate all_blueprints. --- dimos/mapping/recording/go2_mid360/record.py | 86 +++----------- .../recording/mid360_realsense/record.py | 93 +++------------ dimos/mapping/recording/multi_map_anchor.py | 38 +++--- dimos/mapping/recording/utils/build_rrd.py | 14 +-- dimos/mapping/recording/utils/gtsam_gt.py | 4 +- dimos/mapping/recording/utils/rec_check.py | 14 +-- dimos/robot/all_blueprints.py | 4 +- docs/capabilities/navigation/readme.md | 4 + .../navigation/recording_a_map.md | 109 ++++++++++++++++++ 9 files changed, 176 insertions(+), 190 deletions(-) create mode 100644 docs/capabilities/navigation/recording_a_map.md diff --git a/dimos/mapping/recording/go2_mid360/record.py b/dimos/mapping/recording/go2_mid360/record.py index 2572c8fe86..6c8fdb32df 100644 --- a/dimos/mapping/recording/go2_mid360/record.py +++ b/dimos/mapping/recording/go2_mid360/record.py @@ -22,18 +22,10 @@ from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.core.global_config import global_config from dimos.core.stream import In -from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 -from dimos.hardware.sensors.lidar.fastlio2.recorder import FastLio2Recorder -from dimos.hardware.sensors.lidar.fastlio2.speed_warner import SpeedWarner from dimos.hardware.sensors.lidar.livox.module import Mid360 -from dimos.mapping.recording.go2_mid360.static_transforms import ( - BASE_TO_CAMERA_OPTICAL, - MID360_TO_BASE, -) -from dimos.memory2.module import pose_setter_for -from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.hardware.sensors.lidar.pointlio.module import PointLio +from dimos.hardware.sensors.lidar.pointlio.recorder import PointlioRecorder from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.Imu import Imu @@ -55,22 +47,18 @@ def _default_recording_dir() -> Path: return Path("recordings") / stamp -class Go2TfHackRecorder(FastLio2Recorder): - """Records with statically-applied transforms instead of querying tf. +class Go2Recorder(PointlioRecorder): + """Records Point-LIO odom + lidar plus the Go2's companion streams. - FastLio2 tracks the Mid-360 (``mid360_link``) and reports its pose in the - ``world`` frame as ``fastlio_odometry``; its registered cloud is likewise - already in that world frame. We anchor recorded observations to the robot - body, building every pose from the latest fastlio odom and fixed mounts: - - - ``fastlio_lidar`` -> ``base_link`` pose in world (odom, then mid360_link -> base_link) - - ``color_image`` -> ``camera_optical`` pose in world (odom, mid360_link -> base_link, - then base_link -> camera_optical) - - everything else (odom streams, imu) -> tf fallback / no pose + Point-LIO stamps each ``pointlio_lidar`` frame with the latest odometry pose + (inherited ``@pose_setter_for``), so the trajectory is baked into the cloud at + record time — no static-transform pose-fill needed here. The companion streams + (camera, raw livox, go2 odom/lidar) are recorded as-is; the offline + post-process aligns them. """ - fastlio_lidar: In[PointCloud2] - fastlio_odometry: In[Odometry] + pointlio_odometry: In[Odometry] + pointlio_lidar: In[PointCloud2] go2_lidar: In[PointCloud2] go2_odom: In[PoseStamped] color_image: In[Image] @@ -79,43 +67,6 @@ class Go2TfHackRecorder(FastLio2Recorder): livox_lidar: In[PointCloud2] livox_imu: In[Imu] - _latest_fastlio_odom: Odometry | None = None - - @pose_setter_for("fastlio_odometry") - def _odom_pose(self, msg: Odometry) -> Pose | None: - self._latest_fastlio_odom = msg - world_to_base = self._world_to_base_from_fastlio() - return world_to_base.to_pose() if world_to_base is not None else None - - @pose_setter_for("fastlio_lidar") - def _lidar_pose(self, msg: PointCloud2) -> Pose | None: - world_to_base = self._world_to_base_from_fastlio() - return world_to_base.to_pose() if world_to_base is not None else None - - @pose_setter_for("color_image", "zed_color_image") - def _image_pose(self, msg: Image) -> Pose | None: - world_to_base = self._world_to_base_from_fastlio() - if world_to_base is None: - return None - return (world_to_base + BASE_TO_CAMERA_OPTICAL).to_pose() - - @pose_setter_for("go2_odom") - def _go2_odom_pose(self, msg: PoseStamped) -> Pose | None: - return msg - - def _world_to_base_from_fastlio(self) -> Transform | None: - odom = self._latest_fastlio_odom - if odom is None: - return None - world_to_mid360 = Transform( - translation=odom.position, - rotation=odom.orientation, - frame_id="world", - child_frame_id="mid360_link", - ts=odom.ts, - ) - return world_to_mid360 + MID360_TO_BASE - def _zed_camera_blueprint() -> Any: """ZED color source, remapped to ``zed_color_image``. @@ -163,21 +114,16 @@ def _zed_camera_blueprint() -> Any: (Mid360, "imu", "livox_imu"), ] ), - FastLio2.blueprint( + PointLio.blueprint( frame_id="world", lidar_ip=_LIDAR_IP, ).remappings( [ - (FastLio2, "lidar", "fastlio_lidar"), - (FastLio2, "odometry", "fastlio_odometry"), - ] - ), - Go2TfHackRecorder.blueprint(), - SpeedWarner.blueprint().remappings( - [ - (SpeedWarner, "odometry", "fastlio_odometry"), + (PointLio, "lidar", "pointlio_lidar"), + (PointLio, "odometry", "pointlio_odometry"), ] ), + Go2Recorder.blueprint(), # Pygame keyboard teleop (WASD + Q/E, Z=lie down, X=stand). Its cmd_vel # feeds MovementManager's tele_cmd_vel; sit/stand are handled internally # via the auto-wired GO2ConnectionSpec. @@ -196,6 +142,6 @@ def _zed_camera_blueprint() -> Any: global_config.obstacle_avoidance = False coordinator = ModuleCoordinator.build( unitree_go2_record, - {Go2TfHackRecorder.name: {"db_path": str(recording_dir / "mem2.db")}}, + {Go2Recorder.name: {"db_path": str(recording_dir / "mem2.db")}}, ) coordinator.loop() diff --git a/dimos/mapping/recording/mid360_realsense/record.py b/dimos/mapping/recording/mid360_realsense/record.py index 83cbe58fab..4668cb6cd7 100644 --- a/dimos/mapping/recording/mid360_realsense/record.py +++ b/dimos/mapping/recording/mid360_realsense/record.py @@ -22,16 +22,9 @@ from dimos.core.global_config import global_config from dimos.core.stream import In from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera -from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2 -from dimos.hardware.sensors.lidar.fastlio2.recorder import FastLio2Recorder from dimos.hardware.sensors.lidar.livox.module import Mid360 -from dimos.mapping.recording.mid360_realsense.static_transforms import ( - MID360_TO_WORLD, - REALSENSE_COLOR_OPTICAL_FRAME_TO_MID360_IMU_FRAME, -) -from dimos.memory2.module import pose_setter_for -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.hardware.sensors.lidar.pointlio.module import PointLio +from dimos.hardware.sensors.lidar.pointlio.recorder import PointlioRecorder from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image @@ -41,22 +34,6 @@ logger = setup_logger() -# FAST-LIO odom is the Mid-360 IMU frame; map it to the RealSense color optical -# frame. See static_transforms.py for the geometry and sources. -MID360_TO_CAMERA_OPTICAL = REALSENSE_COLOR_OPTICAL_FRAME_TO_MID360_IMU_FRAME.inverse() - -# FAST-LIO inits level (gravity zeroes pitch/roll) with its origin at the Mid-360 IMU, so its -# world frame already shares our flat world's orientation and only differs by the screw->IMU -# lever arm. Re-anchor onto the camera screw with a pure translation -- no rotation, since both -# frames are level and share heading (the rig's screw->IMU offset is pure pitch). Applying the -# full mid360->world rotation here would wrongly tilt the trajectory by the lidar's ~26deg pitch. -WORLD_TO_FASTLIO_WORLD = Transform( - translation=MID360_TO_WORLD.inverse().translation, - frame_id="world", - child_frame_id="fastlio_world", -) - - _LIDAR_IP = os.getenv("LIDAR_IP", "192.168.1.107") @@ -66,22 +43,17 @@ def _default_recording_dir() -> Path: return Path("recordings") / stamp -class TfHackRecorder(FastLio2Recorder): - """Records with statically-applied transforms instead of querying tf. +class RealsenseRecorder(PointlioRecorder): + """Records Point-LIO odom + lidar plus the RealSense + raw Livox streams. - FastLio2 tracks the Mid-360 (``mid360_link``) and reports its pose in the - ``world`` frame as ``fastlio_odometry``; its registered cloud is likewise - already in that world frame. Recorded observations are anchored from the - latest fastlio odom and the fixed camera mount: - - - ``fastlio_lidar`` / ``fastlio_odometry`` -> ``mid360_link`` pose in world - - ``color_image`` / ``realsense_depth_image`` / ``realsense_pointcloud`` - -> ``camera_optical`` pose in world - - everything else (odom, camera_info, imu) -> tf fallback / no pose + Point-LIO stamps each ``pointlio_lidar`` frame with the latest odometry pose + (inherited ``@pose_setter_for``), so the trajectory is baked into the cloud at + record time. The camera and raw-livox streams are recorded as-is; the offline + post-process aligns them. """ - fastlio_lidar: In[PointCloud2] - fastlio_odometry: In[Odometry] + pointlio_odometry: In[Odometry] + pointlio_lidar: In[PointCloud2] color_image: In[Image] realsense_depth_image: In[Image] realsense_pointcloud: In[PointCloud2] @@ -91,41 +63,6 @@ class TfHackRecorder(FastLio2Recorder): livox_lidar: In[PointCloud2] livox_imu: In[Imu] - _latest_fastlio_odom: Odometry | None = None - - @pose_setter_for("fastlio_odometry") - def _odom_pose(self, msg: Odometry) -> Pose | None: - self._latest_fastlio_odom = msg - world_to_mid360 = self._world_to_mid360_from_fastlio() - return world_to_mid360.to_pose() if world_to_mid360 is not None else None - - @pose_setter_for("fastlio_lidar") - def _lidar_pose(self, msg: PointCloud2) -> Pose | None: - world_to_mid360 = self._world_to_mid360_from_fastlio() - return world_to_mid360.to_pose() if world_to_mid360 is not None else None - - @pose_setter_for("color_image", "realsense_depth_image", "realsense_pointcloud") - def _camera_pose(self, msg: object) -> Pose | None: - world_to_mid360 = self._world_to_mid360_from_fastlio() - if world_to_mid360 is None: - return None - return (world_to_mid360 + MID360_TO_CAMERA_OPTICAL).to_pose() - - def _world_to_mid360_from_fastlio(self) -> Transform | None: - odom = self._latest_fastlio_odom - if odom is None: - return None - fastlio_pose = Transform( - translation=odom.position, - rotation=odom.orientation, - frame_id="fastlio_world", - child_frame_id="mid360_link", - ts=odom.ts, - ) - world_to_mid360 = WORLD_TO_FASTLIO_WORLD + fastlio_pose - world_to_mid360.ts = odom.ts - return world_to_mid360 - realsense_mid360_record = autoconnect( RealSenseCamera.blueprint().remappings( @@ -145,16 +82,16 @@ def _world_to_mid360_from_fastlio(self) -> Transform | None: (Mid360, "imu", "livox_imu"), ] ), - FastLio2.blueprint( + PointLio.blueprint( frame_id="world", lidar_ip=_LIDAR_IP, ).remappings( [ - (FastLio2, "lidar", "fastlio_lidar"), - (FastLio2, "odometry", "fastlio_odometry"), + (PointLio, "lidar", "pointlio_lidar"), + (PointLio, "odometry", "pointlio_odometry"), ] ), - TfHackRecorder.blueprint(), + RealsenseRecorder.blueprint(), ).global_config(n_workers=6) @@ -165,6 +102,6 @@ def _world_to_mid360_from_fastlio(self) -> Transform | None: global_config.obstacle_avoidance = False coordinator = ModuleCoordinator.build( realsense_mid360_record, - {TfHackRecorder.name: {"db_path": str(recording_dir / "mem2.db")}}, + {RealsenseRecorder.name: {"db_path": str(recording_dir / "mem2.db")}}, ) coordinator.loop() diff --git a/dimos/mapping/recording/multi_map_anchor.py b/dimos/mapping/recording/multi_map_anchor.py index 28e9bbd066..0d54212696 100644 --- a/dimos/mapping/recording/multi_map_anchor.py +++ b/dimos/mapping/recording/multi_map_anchor.py @@ -52,7 +52,6 @@ from dimos.mapping.recording.utils.apriltags import detect_apriltags from dimos.mapping.recording.utils.build_rrd import _log_map, _log_path_gradient from dimos.mapping.recording.utils.gtsam_gt import build_gtsam_gt, write_gtsam_odom -from dimos.mapping.recording.utils.lidar_reanchor import reanchor_stream from dimos.mapping.recording.utils.post_process import CameraParams from dimos.memory2.store.sqlite import SqliteStore @@ -63,10 +62,9 @@ DB_NAME = "mem2.db" DOG_TAG_ID = 17 # mounted on the robot dog -> not a static landmark, ignored GTSAM_STREAM = "gtsam_odom" -FASTLIO_LIDAR = "fastlio_lidar" -FASTLIO_ODOM = "fastlio_odometry" +POINTLIO_LIDAR = "pointlio_lidar" +POINTLIO_ODOM = "pointlio_odometry" LOOP_LIDAR = "livox_lidar" # raw sensor-frame cloud (loop closure needs sensor, not world, frame) -CORRECTED_LIDAR = "fastlio_lidar_corrected" REALSENSE_INFO_STREAM = "realsense_camera_info" @@ -167,7 +165,7 @@ def _solve( detections, optical_in_base, exclude_marker_ids=(DOG_TAG_ID,), - pose_stream=FASTLIO_ODOM, + pose_stream=POINTLIO_ODOM, loop_lidar_stream=LOOP_LIDAR, add_loop_closures=add_loop_closures, return_landmarks=True, @@ -176,22 +174,14 @@ def _solve( def _write_corrected(db: Path, trajectory: Trajectory) -> None: - """Write `gtsam_odom` (+ .tum) and re-anchor the fastlio lidar onto it.""" + """Write `gtsam_odom` (+ .tum) — the drift-corrected groundtruth trajectory. + + Point-LIO already stamps ``pointlio_lidar`` with its odom pose at record time, + so there's no separate lidar re-anchor pass (dropped in the post-process + refactor); the map viz below uses that recorder-stamped cloud directly. + """ with SqliteStore(path=str(db)) as store: write_gtsam_odom(store, trajectory, GTSAM_STREAM, db.parent / "gtsam_odom.tum") - stream_names = store.list_streams() - if FASTLIO_LIDAR in stream_names and FASTLIO_ODOM in stream_names: - try: - reanchor_stream( - store, - str(db), - lidar_stream=FASTLIO_LIDAR, - odom_stream=FASTLIO_ODOM, - gtsam_stream=GTSAM_STREAM, - out_stream=CORRECTED_LIDAR, - ) - except Exception as error: - print(f" re-anchor {CORRECTED_LIDAR} failed: {error}") def build_combined_rrd( @@ -214,9 +204,9 @@ def build_combined_rrd( ) # raw odom dim, corrected odom bright; anchor in greens/cyan, target in warms. - _log_path_gradient(str(anchor_db), FASTLIO_ODOM, "world/anchor/fastlio_raw", (0, 110, 150)) + _log_path_gradient(str(anchor_db), POINTLIO_ODOM, "world/anchor/pointlio_raw", (0, 110, 150)) _log_path_gradient(str(anchor_db), GTSAM_STREAM, "world/anchor/corrected", (0, 220, 120)) - _log_path_gradient(str(target_db), FASTLIO_ODOM, "world/target/fastlio_raw", (150, 110, 0)) + _log_path_gradient(str(target_db), POINTLIO_ODOM, "world/target/pointlio_raw", (150, 110, 0)) _log_path_gradient(str(target_db), GTSAM_STREAM, "world/target/corrected", (255, 150, 60)) for marker_id, pose7 in sorted(tag_map.items()): @@ -234,9 +224,9 @@ def build_combined_rrd( if with_maps: with SqliteStore(path=str(anchor_db)) as store: - _log_map(store, CORRECTED_LIDAR, "world/anchor/map", 0.1, (0, 180, 170)) + _log_map(store, POINTLIO_LIDAR, "world/anchor/map", 0.1, (0, 180, 170)) with SqliteStore(path=str(target_db)) as store: - _log_map(store, CORRECTED_LIDAR, "world/target/map", 0.1, (240, 160, 40)) + _log_map(store, POINTLIO_LIDAR, "world/target/map", 0.1, (240, 160, 40)) print(f" wrote {out_path}") @@ -254,7 +244,7 @@ def main() -> None: parser.add_argument( "--loop", action="store_true", - help="add lidar loop-closure constraints (off by default: with fastlio_odometry as the " + help="add lidar loop-closure constraints (off by default: with pointlio_odometry as the " "pose chain the livox_lidar revisits are unreliable and currently degrade the solve)", ) parser.add_argument("--no-maps", action="store_true", help="skip the re-anchored lidar maps") diff --git a/dimos/mapping/recording/utils/build_rrd.py b/dimos/mapping/recording/utils/build_rrd.py index 92cc8edb8e..cb60cfefaa 100644 --- a/dimos/mapping/recording/utils/build_rrd.py +++ b/dimos/mapping/recording/utils/build_rrd.py @@ -173,7 +173,7 @@ def _log_odom_frames(db_path, stride=5): for stream, name in [ ("gtsam_odom", "gtsam"), ("go2_odom", "go2"), - ("fastlio_odometry", "fastlio"), + ("pointlio_odometry", "pointlio"), ("odom", "odom"), ]: try: @@ -244,7 +244,7 @@ def _log_apriltags(store, db_path, cam_xform, intrinsics, resolution, max_views_ return print(" rrd: placing april_tags in 3D ...", flush=True) connection = sqlite3.connect(db_path) - traj_stream = "gtsam_odom" if _has_rows(connection, "gtsam_odom") else "fastlio_odometry" + traj_stream = "gtsam_odom" if _has_rows(connection, "gtsam_odom") else "pointlio_odometry" pose_rows = connection.execute( f"SELECT ts,pose_x,pose_y,pose_z,pose_qx,pose_qy,pose_qz,pose_qw " f'FROM "{traj_stream}" WHERE pose_qw IS NOT NULL ORDER BY ts' @@ -436,15 +436,15 @@ def build_rrd( track = ( "/world/go2_frame" if "go2_odom" in streams - else "/world/fastlio_frame" - if "fastlio_odometry" in streams + else "/world/pointlio_frame" + if "pointlio_odometry" in streams else "/world/gtsam_frame" ) hide = { f"/world/{m}": rrb.EntityBehavior(visible=False) for m in ( "go2_map", - "fastlio_map", + "pointlio_map", "onboard_map", ) } @@ -476,7 +476,7 @@ def build_rrd( ci = 0 # rotate a distinct base color through each point cloud for name, stream in [ ("go2", "go2_lidar"), - ("fastlio", "fastlio_lidar"), + ("pointlio", "pointlio_lidar"), ("onboard", "lidar"), # legacy Go2 onboard L1, own frame ]: if stream in streams: @@ -490,7 +490,7 @@ def build_rrd( for stream, entity, base in [ ("gtsam_odom", "world/gtsam_path", (0, 220, 0)), # corrected GT -> green ("go2_odom", "world/go2_path", (220, 200, 0)), # go2 odom -> yellow - ("fastlio_odometry", "world/fastlio_path", (0, 200, 220)), # cyan + ("pointlio_odometry", "world/pointlio_path", (0, 200, 220)), # cyan ("odom", "world/odom_path", (255, 165, 0)), # Go2 onboard odom -> orange ]: _log_path_gradient(db_path, stream, entity, base) diff --git a/dimos/mapping/recording/utils/gtsam_gt.py b/dimos/mapping/recording/utils/gtsam_gt.py index 649d8a7076..d0a0cbb979 100644 --- a/dimos/mapping/recording/utils/gtsam_gt.py +++ b/dimos/mapping/recording/utils/gtsam_gt.py @@ -57,9 +57,9 @@ def _pose_to7(pose3): def pick_pose_stream(connection) -> str: - """The odom stream to use as the pose chain (go2_odom / fastlio_odometry preferred).""" + """The odom stream to use as the pose chain (go2_odom / pointlio_odometry preferred).""" stream_names = [row[0] for row in connection.execute("SELECT name FROM _streams").fetchall()] - candidates = [name for name in ["go2_odom", "fastlio_odometry"] if name in stream_names] + candidates = [name for name in ["go2_odom", "pointlio_odometry"] if name in stream_names] candidates += [ name for name in stream_names if "odom" in name.lower() and name not in candidates ] diff --git a/dimos/mapping/recording/utils/rec_check.py b/dimos/mapping/recording/utils/rec_check.py index 2ca04b1b1f..15936d3a84 100644 --- a/dimos/mapping/recording/utils/rec_check.py +++ b/dimos/mapping/recording/utils/rec_check.py @@ -32,8 +32,8 @@ "color_image", "livox_imu", "livox_lidar", - "fastlio_lidar", - "fastlio_odometry", + "pointlio_lidar", + "pointlio_odometry", "go2_odom", "go2_color_image", "realsense_color_image", @@ -136,7 +136,7 @@ def stream_rows(cur: sqlite3.Cursor, name: str) -> tuple[int, float | None, floa def odometry_travel(cur: sqlite3.Cursor) -> dict | None: rows = cur.execute( - "SELECT pose_x, pose_y, pose_z FROM fastlio_odometry WHERE pose_x IS NOT NULL ORDER BY ts" + "SELECT pose_x, pose_y, pose_z FROM pointlio_odometry WHERE pose_x IS NOT NULL ORDER BY ts" ).fetchall() if not rows: return None @@ -169,7 +169,7 @@ def summarize(directory: Path) -> dict[str, Any]: "files": {}, "pcap": None, "streams": {}, - "fastlio_odometry_travel": None, + "pointlio_odometry_travel": None, } for path in (pcap, db, directory / "mem2.db-wal", directory / "mem2.db-shm"): summary["files"][path.name] = path.stat().st_size if path.exists() else None @@ -205,7 +205,7 @@ def summarize(directory: Path) -> dict[str, Any]: "hz": (n - 1) / span if span > 0 else 0, "pose_pct": 100 * pose_n / n if n else 0, } - summary["fastlio_odometry_travel"] = odometry_travel(cur) + summary["pointlio_odometry_travel"] = odometry_travel(cur) connection.close() return summary @@ -282,7 +282,7 @@ def report(directory: Path) -> int: sx, sy, sz = travel["start"] ex, ey, ez = travel["end"] bx, by, bz = travel["bbox_x"], travel["bbox_y"], travel["bbox_z"] - print("fastlio_odometry travel:") + print("pointlio_odometry travel:") print(f" start x={sx:.2f} y={sy:.2f} z={sz:.2f}") print(f" end x={ex:.2f} y={ey:.2f} z={ez:.2f}") print(f" path_length {travel['path_length']:.2f} m") @@ -292,7 +292,7 @@ def report(directory: Path) -> int: f"y=[{by[0]:.1f},{by[1]:.1f}] z=[{bz[0]:.1f},{bz[1]:.1f}]" ) else: - print("fastlio_odometry travel: no pose-stamped rows") + print("pointlio_odometry travel: no pose-stamped rows") return 0 diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 8381f7d9da..5d1158dd55 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -171,8 +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-recorder": "dimos.mapping.recording.go2_mid360.record.Go2Recorder", "go2-teleop-module": "dimos.teleop.quest.quest_extensions.Go2TeleopModule", - "go2-tf-hack-recorder": "dimos.mapping.recording.go2_mid360.record.Go2TfHackRecorder", "google-maps-skill-container": "dimos.agents.skills.google_maps_skill_container.GoogleMapsSkillContainer", "gps-nav-skill-container": "dimos.agents.skills.gps_nav_skill.GpsNavSkillContainer", "grasping-module": "dimos.manipulation.grasping.grasping.GraspingModule", @@ -222,6 +222,7 @@ "quest-teleop-module": "dimos.teleop.quest.quest_teleop_module.QuestTeleopModule", "ray-tracing-voxel-map": "dimos.mapping.ray_tracing.module.RayTracingVoxelMap", "real-sense-camera": "dimos.hardware.sensors.camera.realsense.camera.RealSenseCamera", + "realsense-recorder": "dimos.mapping.recording.mid360_realsense.record.RealsenseRecorder", "receiver-module": "dimos.utils.demo_image_encoding.ReceiverModule", "recorder": "dimos.memory2.module.Recorder", "reid-module": "dimos.perception.detection.reid.module.ReidModule", @@ -241,7 +242,6 @@ "temporal-memory": "dimos.perception.experimental.temporal_memory.temporal_memory.TemporalMemory", "terrain-analysis": "dimos.navigation.nav_stack.modules.terrain_analysis.terrain_analysis.TerrainAnalysis", "terrain-map-ext": "dimos.navigation.nav_stack.modules.terrain_map_ext.terrain_map_ext.TerrainMapExt", - "tf-hack-recorder": "dimos.mapping.recording.mid360_realsense.record.TfHackRecorder", "twist-teleop-module": "dimos.teleop.quest.quest_extensions.TwistTeleopModule", "unitree-g1-skill-container": "dimos.robot.unitree.g1.skill_container.UnitreeG1SkillContainer", "unitree-skill-container": "dimos.robot.unitree.unitree_skill_container.UnitreeSkillContainer", diff --git a/docs/capabilities/navigation/readme.md b/docs/capabilities/navigation/readme.md index 66c59d3540..b5ac009dbd 100644 --- a/docs/capabilities/navigation/readme.md +++ b/docs/capabilities/navigation/readme.md @@ -9,3 +9,7 @@ Note: in the future these will be merged into one system. ## Simple Nav - [Simple Navigation](/docs/capabilities/navigation/native/index.md) — column-carving voxel mapping + slope-based costmap + +## Recording + +- [Recording a Map](/docs/capabilities/navigation/recording_a_map.md) — drive a Go2 + Mid-360 around a space and capture lidar, odometry, and camera for offline mapping diff --git a/docs/capabilities/navigation/recording_a_map.md b/docs/capabilities/navigation/recording_a_map.md new file mode 100644 index 0000000000..d0c8951b72 --- /dev/null +++ b/docs/capabilities/navigation/recording_a_map.md @@ -0,0 +1,109 @@ +# Recording a Map + +This walks you through driving a Go2 around a space and capturing everything you need to build a map afterward: the Mid-360 point cloud, Point-LIO odometry, and the camera. You drive, it records, you post-process. That's the whole loop. + +If you're on the RealSense rig instead of a Go2, the steps are the same — just use the `mid360_realsense` 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/mapping/recording/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. Those revisits are what let the post-process snap out accumulated drift. +- If you've got AprilTags up, get a clean look at each one more than once. They anchor the final groundtruth. + +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. + +## 5. Post-process + +This detects AprilTags, runs the drift-corrected trajectory solve, and writes a `.rrd` you can open in Rerun: + +```bash +uv run --no-sync python dimos/mapping/recording/go2_mid360/post_process.py +``` + +With no argument it grabs the most recent recording. Point it at a specific one if you want: + +```bash +uv run --no-sync python dimos/mapping/recording/go2_mid360/post_process.py recordings/2026-06-22_03-15pm-PST +``` + +## 6. Sanity-check it + +Before you trust a recording, look at it: + +```bash +uv run python dimos/mapping/recording/utils/rec_check.py +``` + +It reports stream rates and how far the odometry traveled. If `pointlio_lidar` or `pointlio_odometry` is empty, or the travel distance is wildly off, something went wrong on the lidar link — recheck the IP and the wired interface and run it again. From 9cda1083cf513738a9ede757a9d418530cab160a Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 22 Jun 2026 02:45:36 +0800 Subject: [PATCH 179/185] chore(recording): delete SpeedWarner (Point-LIO has built-in saturation guards) --- .../sensors/lidar/fastlio2/speed_warner.py | 79 ------------------- dimos/robot/all_blueprints.py | 1 - 2 files changed, 80 deletions(-) delete mode 100644 dimos/hardware/sensors/lidar/fastlio2/speed_warner.py diff --git a/dimos/hardware/sensors/lidar/fastlio2/speed_warner.py b/dimos/hardware/sensors/lidar/fastlio2/speed_warner.py deleted file mode 100644 index a0e4b87764..0000000000 --- a/dimos/hardware/sensors/lidar/fastlio2/speed_warner.py +++ /dev/null @@ -1,79 +0,0 @@ -# Copyright 2025-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. - -import math -import time - -from dimos.core.module import Module, ModuleConfig -from dimos.core.stream import In -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - -MPH_PER_MPS = 2.23694 - - -class SpeedWarnerConfig(ModuleConfig): - # Speed the Go2 can never physically reach; exceeding it means the estimate diverged. - speed_limit_mph_warning: float = 30.0 - status_print_interval_sec: float = 1.0 - - -class SpeedWarner(Module): - """Watches odometry; once speed ever exceeds the limit (impossible for the Go2, - so it indicates the FastLio2 estimate has diverged / sensor is about to crash), - latches and spams an error on every subsequent odom message until restart. - - FastLio2's C++ publisher hardcodes twist to zero (cpp/main.cpp), so msg.vx/vy/vz - are always 0. Speed is derived from pose deltas instead. - """ - - config: SpeedWarnerConfig - - odometry: In[Odometry] - - _tripped: bool = False - _max_mph: float = 0.0 - _last_pos: tuple[float, float, float] | None = None - _last_ts: float | None = None - _last_print_ts: float = 0.0 - - async def handle_odometry(self, msg: Odometry) -> None: - ts = msg.ts or time.time() - pos = (msg.pose.x, msg.pose.y, msg.pose.z) - last_pos, last_ts = self._last_pos, self._last_ts - self._last_pos, self._last_ts = pos, ts - if last_pos is None or last_ts is None: - return - dt = ts - last_ts - if dt <= 0: - return - dx, dy, dz = pos[0] - last_pos[0], pos[1] - last_pos[1], pos[2] - last_pos[2] - speed_mph = math.sqrt(dx * dx + dy * dy + dz * dz) / dt * MPH_PER_MPS - if speed_mph > self._max_mph: - self._max_mph = speed_mph - if ts - self._last_print_ts >= self.config.status_print_interval_sec: - self._last_print_ts = ts - print( - f"\rspeed: {speed_mph:6.2f} mph max: {self._max_mph:6.2f} mph ", - end="", - flush=True, - ) - if not self._tripped and speed_mph > self.config.speed_limit_mph_warning: - self._tripped = True - logger.error( - f"!!! FASTLIO ODOMETRY DIVERGED !!! reported {speed_mph:.1f} mph " - f"(limit {self.config.speed_limit_mph_warning:.1f} mph). Latching warnings." - ) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 5d1158dd55..4920af1685 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -236,7 +236,6 @@ "simple-planner": "dimos.navigation.nav_stack.modules.simple_planner.simple_planner.SimplePlanner", "spatial-memory": "dimos.perception.spatial_perception.SpatialMemory", "speak-skill": "dimos.agents.skills.speak_skill.SpeakSkill", - "speed-warner": "dimos.hardware.sensors.lidar.fastlio2.speed_warner.SpeedWarner", "tare-planner": "dimos.navigation.nav_stack.modules.tare_planner.tare_planner.TarePlanner", "teleop-recorder": "dimos.teleop.utils.recorder.TeleopRecorder", "temporal-memory": "dimos.perception.experimental.temporal_memory.temporal_memory.TemporalMemory", From 542a0ecdf45a3ef076de7033ca1d488a0cea3c1c Mon Sep 17 00:00:00 2001 From: "autofix-ci[bot]" <114827586+autofix-ci[bot]@users.noreply.github.com> Date: Mon, 22 Jun 2026 05:37:22 +0000 Subject: [PATCH 180/185] [autofix.ci] apply automated fixes --- dimos/hardware/sensors/camera/realsense/camera.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/hardware/sensors/camera/realsense/camera.py b/dimos/hardware/sensors/camera/realsense/camera.py index 2bb9ff3679..5371672862 100644 --- a/dimos/hardware/sensors/camera/realsense/camera.py +++ b/dimos/hardware/sensors/camera/realsense/camera.py @@ -203,7 +203,7 @@ def _start_imu(self) -> None: # accel @ 63 Hz, which D4xx firmware doesn't offer -> "Couldn't resolve requests". imu_config.enable_stream(rs.stream.accel, rs.format.motion_xyz32f, 200) imu_config.enable_stream(rs.stream.gyro, rs.format.motion_xyz32f, 200) - imu_profile = imu_pipeline.start(imu_config, self._on_imu_frame) + imu_pipeline.start(imu_config, self._on_imu_frame) except RuntimeError as error: print(f"RealSense IMU unavailable, disabling IMU stream: {error}") return From f8c94f9f1636f0c21303056a76b399bfd5ca2e69 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 22 Jun 2026 13:45:01 +0800 Subject: [PATCH 181/185] chore: drop temp bin/go2_rec helper from the branch --- bin/go2_rec | 10 ---------- 1 file changed, 10 deletions(-) delete mode 100755 bin/go2_rec diff --git a/bin/go2_rec b/bin/go2_rec deleted file mode 100755 index 8c8a5fe69e..0000000000 --- a/bin/go2_rec +++ /dev/null @@ -1,10 +0,0 @@ -#!/usr/bin/env bash - -# sudo ifconfig en7 inet 192.168.1.5 netmask 255.255.255.0 -source .venv/bin/activate -kd - -log_path="./go2_recordings/log.txt" -mkdir -p "$(dirname "$log_path")" -echo "NEW RECORDING: $(date)" >> "$log_path" -dimos --no-obstacle-avoidance --robot-ip 192.168.124.177 run unitree-go2-record | tee -a "$log_path" From db1d361290b114f304b24bebb53d22396005c2c4 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 22 Jun 2026 13:55:44 +0800 Subject: [PATCH 182/185] docs: revert native_modules.md example change --- docs/usage/native_modules.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/docs/usage/native_modules.md b/docs/usage/native_modules.md index bb31cdd456..3121ba8bf4 100644 --- a/docs/usage/native_modules.md +++ b/docs/usage/native_modules.md @@ -116,11 +116,11 @@ class MyConfig(NativeModuleConfig): If a config field shouldn't be a CLI arg, add it to `cli_exclude`: ```python skip -class MyNativeConfig(NativeModuleConfig): - executable: str = "./build/my_native" - acc_cov: float = 1.0 # rendered into a config file, not a CLI arg - config_path: str | None = None # set at start() to the generated file - cli_exclude: frozenset[str] = frozenset({"acc_cov"}) # only config_path is passed +class FastLio2Config(NativeModuleConfig): + executable: str = "./build/fastlio2" + config: str = "mid360.yaml" # human-friendly name + config_path: str = Field(default_factory=lambda m: str(Path(m["config"]).resolve())) + cli_exclude: frozenset[str] = frozenset({"config"}) # only config_path is passed ``` ## Using with blueprints From 30718ba59ff1c36818997f6e6c87ee963e78285b Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 22 Jun 2026 14:00:46 +0800 Subject: [PATCH 183/185] chore: move hid into its alphabetical slot in the mypy ignore list --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 2f410037ad..dcac7c6959 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -508,10 +508,10 @@ module = [ "etils", "faster_whisper", "geometry_msgs.*", + "hid", "mcap", "mcap.*", "mujoco", - "hid", "mujoco_playground.*", "nav_msgs.*", "open_clip", From 720cf594dfdf843b33d93a4b8f4e52028981730e Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 22 Jun 2026 14:11:01 +0800 Subject: [PATCH 184/185] revert(zed): remove ZED camera from go2 recording branch Drop ZedSimple (the import-hid module), revert ZEDCamera changes to main, and remove the ZED color/imu streams from the go2 recorder. Drop the now-orphaned hid mypy override and regenerate all_blueprints. --- dimos/hardware/sensors/camera/zed/camera.py | 78 +++---- dimos/hardware/sensors/camera/zed/simple.py | 218 ------------------- dimos/mapping/recording/go2_mid360/record.py | 32 --- dimos/robot/all_blueprints.py | 1 - pyproject.toml | 1 - 5 files changed, 28 insertions(+), 302 deletions(-) delete mode 100644 dimos/hardware/sensors/camera/zed/simple.py diff --git a/dimos/hardware/sensors/camera/zed/camera.py b/dimos/hardware/sensors/camera/zed/camera.py index ba95610686..5203fa9242 100644 --- a/dimos/hardware/sensors/camera/zed/camera.py +++ b/dimos/hardware/sensors/camera/zed/camera.py @@ -14,7 +14,7 @@ from __future__ import annotations -import math +import atexit import threading import time @@ -24,11 +24,11 @@ import reactivex as rx from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT -from dimos.core.coordination.blueprints import autoconnect from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import Out +from dimos.core.transport import LCMTransport from dimos.hardware.sensors.camera.spec import ( OPTICAL_ROTATION, DepthCameraConfig, @@ -39,11 +39,9 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.Imu import Imu from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.spec import perception from dimos.utils.reactive import backpressure -from dimos.visualization.vis_module import vis_module def default_base_transform() -> Transform: @@ -63,7 +61,6 @@ class ZEDCameraConfig(ModuleConfig, DepthCameraConfig): base_transform: Transform | None = Field(default_factory=default_base_transform) align_depth_to_color: bool = True enable_depth: bool = True - enable_imu: bool = True enable_pointcloud: bool = False pointcloud_fps: float = 5.0 camera_info_fps: float = 1.0 @@ -84,7 +81,6 @@ class ZEDCamera(DepthCameraHardware, Module, perception.DepthCamera): config: ZEDCameraConfig color_image: Out[Image] depth_image: Out[Image] - imu: Out[Imu] pointcloud: Out[PointCloud2] camera_info: Out[CameraInfo] depth_camera_info: Out[CameraInfo] @@ -93,10 +89,6 @@ class ZEDCamera(DepthCameraHardware, Module, perception.DepthCamera): def _camera_link(self) -> str: return f"{self.config.camera_name}_link" - @property - def _imu_frame(self) -> str: - return f"{self.config.camera_name}_imu_link" - @property def _color_frame(self) -> str: return f"{self.config.camera_name}_color_frame" @@ -129,8 +121,6 @@ def __init__(self, *args, **kwargs) -> None: # type: ignore[no-untyped-def] self._pointcloud_lock = threading.Lock() self._image_left: sl.Mat | None = None self._depth_map: sl.Mat | None = None - self._sensors_data: sl.SensorsData | None = None - self._has_imu = False self._pose: sl.Pose | None = None self._tracking_enabled = False self._stream_width = self.config.width @@ -176,16 +166,11 @@ def start(self) -> None: self._image_left = sl.Mat() self._depth_map = sl.Mat() self._pose = sl.Pose() - self._sensors_data = sl.SensorsData() self._sl_camera_info = self._zed.get_camera_information() if self._sl_camera_info is not None: self._stream_width = self._sl_camera_info.camera_configuration.resolution.width self._stream_height = self._sl_camera_info.camera_configuration.resolution.height - sensors_config = self._sl_camera_info.sensors_configuration - self._has_imu = self.config.enable_imu and sensors_config.is_sensor_available( - sl.SENSOR_TYPE.ACCELEROMETER - ) self._build_camera_info() self._get_extrinsics() @@ -345,33 +330,8 @@ def _capture_loop(self) -> None: self._latest_color_img = color_img self._latest_depth_img = depth_img - if self._has_imu: - self._publish_imu(ts) - self._publish_tf(ts) - def _publish_imu(self, ts: float) -> None: - if self._zed is None or self._sensors_data is None: - return - if ( - self._zed.get_sensors_data(self._sensors_data, sl.TIME_REFERENCE.IMAGE) - != sl.ERROR_CODE.SUCCESS - ): - return - imu_data = self._sensors_data.get_imu_data() - angular_velocity_deg = imu_data.get_angular_velocity() - linear_acceleration = imu_data.get_linear_acceleration() - orientation = imu_data.get_pose().get_orientation().get() - self.imu.publish( - Imu( - angular_velocity=Vector3(*(math.radians(v) for v in angular_velocity_deg)), - linear_acceleration=Vector3(*linear_acceleration), - orientation=Quaternion(*orientation), - frame_id=self._imu_frame, - ts=ts, - ) - ) - def _tracking_transform(self, ts: float) -> Transform | None: if not self._tracking_enabled or self._zed is None or self._pose is None: return None @@ -511,8 +471,6 @@ def stop(self) -> None: self._latest_depth_img = None self._image_left = None self._depth_map = None - self._sensors_data = None - self._has_imu = False self._pose = None self._sl_camera_info = None self._tracking_enabled = False @@ -532,13 +490,33 @@ def get_depth_scale(self) -> float: def main() -> None: - from dimos.core.global_config import global_config + dimos = ModuleCoordinator() + dimos.start() - blueprint = autoconnect( - ZEDCamera.blueprint(enable_pointcloud=True, pointcloud_fps=5.0), - vis_module(global_config.viewer), - ) - ModuleCoordinator.build(blueprint).loop() + camera = dimos.deploy(ZEDCamera, enable_pointcloud=True, pointcloud_fps=5.0) + camera.color_image.transport = LCMTransport("/camera/color", Image) + camera.depth_image.transport = LCMTransport("/camera/depth", Image) + camera.pointcloud.transport = LCMTransport("/camera/pointcloud", PointCloud2) + camera.camera_info.transport = LCMTransport("/camera/color_info", CameraInfo) + camera.depth_camera_info.transport = LCMTransport("/camera/depth_info", CameraInfo) + + def cleanup() -> None: + try: + dimos.stop() + except Exception: + pass + + atexit.register(cleanup) + dimos.start_all_modules() + + try: + while True: + time.sleep(0.1) + except (KeyboardInterrupt, SystemExit): + pass + finally: + atexit.unregister(cleanup) + cleanup() if __name__ == "__main__": diff --git a/dimos/hardware/sensors/camera/zed/simple.py b/dimos/hardware/sensors/camera/zed/simple.py deleted file mode 100644 index 36c7622e87..0000000000 --- a/dimos/hardware/sensors/camera/zed/simple.py +++ /dev/null @@ -1,218 +0,0 @@ -# Copyright 2025-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 __future__ import annotations - -import asyncio -from collections.abc import AsyncIterator -import glob -import math -import os -import struct -import time - -import cv2 - -from dimos.core.module import Module, ModuleConfig -from dimos.core.stream import Out -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - -# ZED-M IMU over USB HID — protocol from Stereolabs' open-source zed-open-capture. -_ZED_USB_VENDOR = 0x2B03 -_ZED_M_MCU_PID = 0xF681 -_REP_ID_SENSOR_STREAM_STATUS = 0x32 -# Packed RawData struct (sensorcapture_def.hpp); we only read the IMU fields. -_RAW_DATA_FMT = " m/s^2 (+-8g range) -_GYRO_SCALE = (1000.0 / 32768.0) * (math.pi / 180.0) # raw -> rad/s (+-1000 deg/s) - - -def autodetect_zed_device() -> str | None: - """Resolve the ZED's UVC ``/dev/video*`` node via its v4l by-id symlink.""" - for link in sorted(glob.glob("/dev/v4l/by-id/*")): - if "ZED" in os.path.basename(link).upper(): - return os.path.realpath(link) - return None - - -class ZedSimpleConfig(ModuleConfig): - device: str | None = None - # Full side-by-side frame; the ZED is YUYV-only. 2560x720 => 1280x720 per eye. - width: int = 2560 - height: int = 720 - fps: int = 15 - side: str = "left" - fourcc: str = "YUYV" - enable_imu: bool = True - imu_pid: int = _ZED_M_MCU_PID - camera_name: str = "zed" - - -class ZedSimple(Module): - """SDK-free ZED color + IMU capture. - - Fallback for when the ZED SDK / ``pyzed`` is not installed. Color comes from - the UVC stereo stream (one side-by-side frame; we slice out one eye and - publish ``color_image``). The IMU is read straight off the camera's USB-HID - device using Stereolabs' open ``zed-open-capture`` protocol and published as - ``imu``. No depth or pointcloud — use the SDK-backed ``ZEDCamera`` for those. - - Color (UVC) and IMU (HID) are independent USB interfaces, so each runs and - fails independently: a missing camera does not stop the IMU and vice versa. - """ - - config: ZedSimpleConfig - color_image: Out[Image] - imu: Out[Imu] - - def __init__(self, *args, **kwargs) -> None: # type: ignore[no-untyped-def] - super().__init__(*args, **kwargs) - self._capture: cv2.VideoCapture | None = None - self._running = False - self._video_task: asyncio.Task[None] | None = None - self._imu_task: asyncio.Task[None] | None = None - - @property - def _optical_frame(self) -> str: - return f"{self.config.camera_name}_{self.config.side}_color_optical_frame" - - @property - def _imu_frame(self) -> str: - return f"{self.config.camera_name}_imu_link" - - def _open(self) -> cv2.VideoCapture: - device = self.config.device or autodetect_zed_device() - if device is None: - raise RuntimeError( - "No ZED UVC video device found under /dev/v4l/by-id. Confirm the " - "camera is on a USB-3 port, or set ZedSimpleConfig.device explicitly." - ) - capture = cv2.VideoCapture(device, cv2.CAP_V4L2) - if not capture.isOpened(): - raise RuntimeError(f"Failed to open ZED video device: {device}") - if self.config.fourcc: - capture.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc(*self.config.fourcc)) - if self.config.width: - capture.set(cv2.CAP_PROP_FRAME_WIDTH, self.config.width) - if self.config.height: - capture.set(cv2.CAP_PROP_FRAME_HEIGHT, self.config.height) - if self.config.fps: - capture.set(cv2.CAP_PROP_FPS, self.config.fps) - return capture - - async def main(self) -> AsyncIterator[None]: - self._running = True - - # IMU (USB-HID) runs independently of the camera (UVC). - if self.config.enable_imu: - self._imu_task = asyncio.create_task(asyncio.to_thread(self._imu_loop)) - - # Fail soft: a missing/flaky camera must not abort a multi-sensor - # recording (nor stop the IMU). Log loudly and run without frames. - try: - self._capture = await asyncio.to_thread(self._open) - except RuntimeError as error: - logger.error(f"ZedSimple: camera unavailable, no color_image will publish: {error}") - if self._capture is not None: - width = int(self._capture.get(cv2.CAP_PROP_FRAME_WIDTH)) - height = int(self._capture.get(cv2.CAP_PROP_FRAME_HEIGHT)) - logger.info( - f"ZedSimple: streaming {width}x{height} side-by-side, publishing {self.config.side} eye" - ) - self._video_task = asyncio.create_task(asyncio.to_thread(self._capture_loop)) - - yield - - self._running = False - for task in (self._video_task, self._imu_task): - if task is not None: - await task - if self._capture is not None: - self._capture.release() - self._capture = None - - def _capture_loop(self) -> None: - if self._capture is None: - return - while self._running: - ok, frame = self._capture.read() - if not ok: - time.sleep(0.01) - continue - half_width = frame.shape[1] // 2 - eye = frame[:, :half_width] if self.config.side == "left" else frame[:, half_width:] - self.color_image.publish( - Image( - data=cv2.cvtColor(eye, cv2.COLOR_BGR2RGB), - format=ImageFormat.RGB, - frame_id=self._optical_frame, - ts=time.time(), - ) - ) - - def _imu_loop(self) -> None: - try: - import hid - except ImportError: - logger.error("ZedSimple: `hid` not installed, no IMU will publish (pip install hid)") - return - - try: - device = hid.Device(_ZED_USB_VENDOR, self.config.imu_pid) - except Exception as error: - logger.error(f"ZedSimple: IMU HID device unavailable, no imu will publish: {error}") - return - - try: - device.send_feature_report(bytes([_REP_ID_SENSOR_STREAM_STATUS, 0x01])) - # ROS convention: orientation unknown (raw IMU, no fusion) -> covariance[0] = -1. - unknown_orientation_covariance = [-1.0] + [0.0] * 8 - while self._running: - report = device.read(_RAW_DATA_SIZE, timeout=200) - if not report or len(report) < _RAW_DATA_SIZE: - continue - fields = struct.unpack(_RAW_DATA_FMT, bytes(report[:_RAW_DATA_SIZE])) - imu_not_valid = fields[1] - if imu_not_valid: - continue - # fields: struct_id, imu_not_valid, timestamp, gX,gY,gZ, aX,aY,aZ, ... - gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z = fields[3:9] - self.imu.publish( - Imu( - angular_velocity=Vector3( - gyro_x * _GYRO_SCALE, gyro_y * _GYRO_SCALE, gyro_z * _GYRO_SCALE - ), - linear_acceleration=Vector3( - acc_x * _ACC_SCALE, acc_y * _ACC_SCALE, acc_z * _ACC_SCALE - ), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - orientation_covariance=unknown_orientation_covariance, - frame_id=self._imu_frame, - ts=time.time(), - ) - ) - finally: - try: - device.send_feature_report(bytes([_REP_ID_SENSOR_STREAM_STATUS, 0x00])) - except Exception: - pass - device.close() diff --git a/dimos/mapping/recording/go2_mid360/record.py b/dimos/mapping/recording/go2_mid360/record.py index 6c8fdb32df..3dbed0a255 100644 --- a/dimos/mapping/recording/go2_mid360/record.py +++ b/dimos/mapping/recording/go2_mid360/record.py @@ -16,7 +16,6 @@ from datetime import datetime import os from pathlib import Path -from typing import Any from dimos.core.coordination.blueprints import autoconnect from dimos.core.coordination.module_coordinator import ModuleCoordinator @@ -62,42 +61,11 @@ class Go2Recorder(PointlioRecorder): go2_lidar: In[PointCloud2] go2_odom: In[PoseStamped] color_image: In[Image] - zed_color_image: In[Image] - zed_imu: In[Imu] livox_lidar: In[PointCloud2] livox_imu: In[Imu] -def _zed_camera_blueprint() -> Any: - """ZED color source, remapped to ``zed_color_image``. - - Prefer the SDK-backed ``ZEDCamera`` (depth/imu/pointcloud); fall back to the - UVC-only ``ZedSimple`` (color only) when ``pyzed`` is not installed. - """ - try: - import pyzed.sl # noqa: F401 - - from dimos.hardware.sensors.camera.zed.camera import ZEDCamera - - return ZEDCamera.blueprint(enable_depth=False, enable_pointcloud=False).remappings( - [ - (ZEDCamera, "color_image", "zed_color_image"), - (ZEDCamera, "imu", "zed_imu"), - ] - ) - except ImportError: - from dimos.hardware.sensors.camera.zed.simple import ZedSimple - - return ZedSimple.blueprint().remappings( - [ - (ZedSimple, "color_image", "zed_color_image"), - (ZedSimple, "imu", "zed_imu"), - ] - ) - - unitree_go2_record = autoconnect( - _zed_camera_blueprint(), MovementManager.blueprint(), GO2Connection.blueprint().remappings( [ diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 4920af1685..cae4190d74 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -254,5 +254,4 @@ "web-input": "dimos.agents.web_human_input.WebInput", "websocket-vis-module": "dimos.web.websocket_vis.websocket_vis_module.WebsocketVisModule", "zed-camera": "dimos.hardware.sensors.camera.zed.camera.ZEDCamera", - "zed-simple": "dimos.hardware.sensors.camera.zed.simple.ZedSimple", } diff --git a/pyproject.toml b/pyproject.toml index dcac7c6959..b5af8dc760 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -508,7 +508,6 @@ module = [ "etils", "faster_whisper", "geometry_msgs.*", - "hid", "mcap", "mcap.*", "mujoco", From 66e80668df882efdccbeec07710a977e1b9eb9c7 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Mon, 22 Jun 2026 14:18:53 +0800 Subject: [PATCH 185/185] RESTORE: revert KeyboardTeleop lie-down/stand-up wiring MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Removes this branch's additions to the pygame KeyboardTeleop (the _go2 GO2ConnectionSpec, _call_go2_pose, Z/X liedown/standup key handlers, and help text) — reverted to main. The Z=lie-down hack was handy for saving Go2 battery on long loops; tagged RESTORE so it's easy to git-revert this commit to bring it back later. --- dimos/mapping/recording/go2_mid360/record.py | 5 ++--- dimos/robot/unitree/keyboard_teleop.py | 20 -------------------- 2 files changed, 2 insertions(+), 23 deletions(-) diff --git a/dimos/mapping/recording/go2_mid360/record.py b/dimos/mapping/recording/go2_mid360/record.py index 3dbed0a255..c3da28b2fb 100644 --- a/dimos/mapping/recording/go2_mid360/record.py +++ b/dimos/mapping/recording/go2_mid360/record.py @@ -92,9 +92,8 @@ class Go2Recorder(PointlioRecorder): ] ), Go2Recorder.blueprint(), - # Pygame keyboard teleop (WASD + Q/E, Z=lie down, X=stand). Its cmd_vel - # feeds MovementManager's tele_cmd_vel; sit/stand are handled internally - # via the auto-wired GO2ConnectionSpec. + # Pygame keyboard teleop (WASD drive + Q/E strafe). Its cmd_vel feeds + # MovementManager's tele_cmd_vel. KeyboardTeleop.blueprint(linear_speed=0.3, angular_speed=0.6).remappings( [ (KeyboardTeleop, "cmd_vel", "tele_cmd_vel"), diff --git a/dimos/robot/unitree/keyboard_teleop.py b/dimos/robot/unitree/keyboard_teleop.py index f22dd4abfb..07af844c60 100644 --- a/dimos/robot/unitree/keyboard_teleop.py +++ b/dimos/robot/unitree/keyboard_teleop.py @@ -25,7 +25,6 @@ from dimos.core.stream import Out from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.robot.unitree.go2.connection_spec import GO2ConnectionSpec from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -52,8 +51,6 @@ class KeyboardTeleop(Module): cmd_vel: Out[Twist] - _go2: GO2ConnectionSpec | None = None - _stop_event: threading.Event _keys_held: set[int] | None = None _thread: threading.Thread | None = None @@ -108,18 +105,6 @@ def stop(self) -> None: super().stop() - def _call_go2_pose(self, action: str) -> None: - if self._go2 is None: - logger.warning(f"{action} ignored: no Go2 connection wired into teleop") - return - try: - if action == "liedown": - self._go2.liedown() - else: - self._go2.standup() - except Exception as error: - logger.error(f"{action} command failed: {error}") - def _pygame_loop(self) -> None: if self._keys_held is None: raise RuntimeError("_keys_held not initialized") @@ -148,10 +133,6 @@ def _pygame_loop(self) -> None: elif event.key == pygame.K_ESCAPE: # ESC quits self._stop_event.set() - elif event.key == pygame.K_z: - self._call_go2_pose("liedown") - elif event.key == pygame.K_x: - self._call_go2_pose("standup") elif event.type == pygame.KEYUP: self._keys_held.discard(event.key) @@ -251,7 +232,6 @@ def _update_display(self, twist: Twist) -> None: help_texts = [ "WS: Move | AD: Turn | QE: Strafe", "Shift: Boost | Ctrl: Slow", - "Z: Lie Down | X: Stand Up", "Space: E-Stop | ESC: Quit", ] for text in help_texts: