From 7ef91f69dae68f973b7de21cd34c4adbec5ff75c Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 3 Jun 2026 15:51:40 -0700 Subject: [PATCH 001/137] 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 002/137] 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 003/137] 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 a14bae88748c4f9b37d7b9959e4e802b13898f46 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 3 Jun 2026 20:53:02 -0700 Subject: [PATCH 004/137] 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 005/137] 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 f8a6b65da3dd125aff4dfebb28f08fca88b1794f Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 4 Jun 2026 19:14:02 -0700 Subject: [PATCH 006/137] 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 fa6325d524df954e01b47266bad792fdf7a87aef Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Thu, 4 Jun 2026 21:18:04 -0700 Subject: [PATCH 007/137] 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 89151a707eba16ed1d7f0b40f676ec402202344b Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 10 Jun 2026 17:07:33 -0700 Subject: [PATCH 008/137] 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 009/137] 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 010/137] 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 011/137] 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 01d8872951c44458dcb0dc967f24b4f8a4f813d3 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 12 Jun 2026 18:47:02 -0700 Subject: [PATCH 012/137] 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 013/137] 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 a472d25db2093ae4fb219ff9f79c56d42ce00b0f Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 12 Jun 2026 19:26:50 -0700 Subject: [PATCH 014/137] 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 3b5f98813a6eb5b217dc8fdc943c7c597cee47c0 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Fri, 12 Jun 2026 19:45:13 -0700 Subject: [PATCH 015/137] 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 016/137] 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 017/137] 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 018/137] =?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 22d9ec0e5ed3a3af938a523fb2a7cacf39cdab99 Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Sat, 13 Jun 2026 07:02:13 -0700 Subject: [PATCH 019/137] =?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 020/137] 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 021/137] 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 022/137] 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 023/137] 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 024/137] 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 025/137] 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 026/137] 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 027/137] =?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 028/137] 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 029/137] 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 030/137] 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 031/137] 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 032/137] 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 033/137] =?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 034/137] 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 035/137] 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 036/137] 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 037/137] 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 038/137] 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 039/137] 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 040/137] 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 041/137] 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 042/137] 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 043/137] 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 044/137] 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 045/137] 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 046/137] 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 047/137] 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 048/137] 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 049/137] 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 050/137] 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 051/137] 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 052/137] 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 053/137] 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 054/137] 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 055/137] 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 056/137] 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 057/137] 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 058/137] 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 059/137] 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 060/137] 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 061/137] 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 062/137] 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 063/137] - --- .../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 064/137] 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 065/137] =?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 066/137] 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 067/137] 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 068/137] 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 069/137] 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 070/137] 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 071/137] - --- .../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 072/137] 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 073/137] 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 074/137] 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 075/137] 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 076/137] 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 077/137] 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 078/137] 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 079/137] 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 080/137] 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 081/137] 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 082/137] 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 083/137] 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 084/137] 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 085/137] 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 086/137] 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 087/137] 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 088/137] 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 089/137] 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 090/137] 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 091/137] 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 092/137] 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 093/137] 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 094/137] 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 095/137] 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 096/137] 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 097/137] 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 098/137] 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 099/137] 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 100/137] 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 101/137] 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 102/137] 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 103/137] 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 104/137] 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 4b22007341778f9a824c45e9b99b0bcca3e6baef Mon Sep 17 00:00:00 2001 From: Jeff Hykin Date: Wed, 17 Jun 2026 07:18:02 -0700 Subject: [PATCH 105/137] 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 106/137] 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 107/137] 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 108/137] 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 109/137] 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 110/137] 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 111/137] 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 112/137] 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 113/137] 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 114/137] 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 115/137] 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 116/137] 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 117/137] 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 118/137] 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 119/137] 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 120/137] =?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 121/137] 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 122/137] 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 123/137] 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 124/137] 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 125/137] 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 126/137] 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 127/137] 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 128/137] 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 129/137] 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 130/137] 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 131/137] 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 132/137] 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 133/137] 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 134/137] 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 135/137] 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 136/137] 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 137/137] 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")