diff --git a/dimos/control/blueprints/mobile.py b/dimos/control/blueprints/mobile.py index 8bd63264e5..0a0611490c 100644 --- a/dimos/control/blueprints/mobile.py +++ b/dimos/control/blueprints/mobile.py @@ -34,9 +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.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 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 @@ -120,16 +117,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, - map_freq=1.0, - config="default.yaml", ), create_nav_stack( planner="simple", @@ -181,7 +173,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/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/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 b710afd29e..0000000000 --- a/dimos/hardware/sensors/lidar/fastlio2/config/mid360.yaml +++ /dev/null @@ -1,42 +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: 0.1 - 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). - 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/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 deleted file mode 100644 index bbbfdf1929..0000000000 --- a/dimos/hardware/sensors/lidar/fastlio2/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/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/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/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/flake.lock b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock index ed10ba8629..34bf2f67bf 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": 1781776629, + "narHash": "sha256-Ik3OwjSUZza/C545iPC4G/fzfJfFsdIo2GvplgN45hA=", "owner": "dimensionalOS", "repo": "dimos-module-fastlio2", - "rev": "d93bc6795babe9e63cc77d2bf2b72294d9afa839", + "rev": "a32c9f599940a94595aa72868e2e4ab436a44b75", "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 d73c4fd631..c7c4319440 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 = { @@ -79,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 5c53381aa3..94294ec312 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -4,14 +4,15 @@ // 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 \ // --lidar '/lidar#sensor_msgs.PointCloud2' \ // --odometry '/odometry#nav_msgs.Odometry' \ -// --config_path /path/to/mid360.yaml \ +// --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 @@ -30,9 +31,7 @@ #include "livox_sdk_config.hpp" -#include "cloud_filter.hpp" #include "dimos_native_module.hpp" -#include "voxel_map.hpp" // dimos LCM message headers #include "geometry_msgs/Quaternion.hpp" @@ -51,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; @@ -61,61 +58,18 @@ static FastLio* g_fastlio = nullptr; 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 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; -// 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; 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; @@ -126,19 +80,33 @@ static uint64_t get_timestamp_ns(const LivoxLidarEthernetPacket* pkt) { using dimos::time_from_seconds; using dimos::make_header; -// --------------------------------------------------------------------------- -// Publish lidar (world-frame point cloud) -// --------------------------------------------------------------------------- +// 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 void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, - const std::string& topic = "") { +// 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 = "") { 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()); sensor_msgs::PointCloud2 pc; - pc.header = make_header(g_frame_id, timestamp); + pc.header = make_header(frame_id, timestamp); pc.height = 1; pc.width = num_points; pc.is_bigendian = 0; @@ -168,77 +136,33 @@ 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); - 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; } g_lcm->publish(chan, &pc); } -// --------------------------------------------------------------------------- // 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); 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) { @@ -257,13 +181,11 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times 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; +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; @@ -279,7 +201,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; @@ -292,7 +214,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; @@ -304,11 +226,12 @@ 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; } - double ts = static_cast(get_timestamp_ns(data)) / 1e9; + uint64_t pkt_ts_ns = get_timestamp_ns(data); + + double ts = static_cast(pkt_ts_ns) / 1e9; auto* imu_pts = reinterpret_cast(data->data); uint16_t dot_num = data->dot_num; @@ -322,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); @@ -351,25 +270,90 @@ 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); EnableLivoxLidarImuData(handle, nullptr, nullptr); } -// --------------------------------------------------------------------------- // Signal handling -// --------------------------------------------------------------------------- static void signal_handler(int /*sig*/) { g_running.store(false); } -// --------------------------------------------------------------------------- // 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 = 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(); + if (cloud && !cloud->empty()) { + publish_lidar(cloud, ts, g_sensor_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); @@ -377,19 +361,34 @@ 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"); 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); @@ -401,15 +400,14 @@ 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); - 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); + + // 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); // Verbose logging — propagates to the FAST-LIO C++ core via the // `fastlio_debug` global. Default false → only real errors print. @@ -419,57 +417,24 @@ 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); - - // 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]; - } - } + 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"); - 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] 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); - 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); - 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); + 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 @@ -485,153 +450,73 @@ 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); + 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)); + 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)); + + // 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; - // Init Livox SDK (in-memory config, no temp files). - // Pass `debug` so the SDK's spdlog console sink is disabled when off. + // 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; } - - // 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 - auto frame_interval = std::chrono::microseconds( - static_cast(1e6 / g_frequency)); - auto last_emit = std::chrono::steady_clock::now(); - 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(); - - // 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(); - 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)); - } + if (debug) { printf("[fastlio2] SDK started, waiting for device...\n"); } 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; - - { - std::lock_guard lock(g_pc_mutex); - if (!g_accumulated_points.empty()) { - points.swap(g_accumulated_points); - frame_start = g_frame_start_ns; - g_frame_has_timestamp = false; - } - } - - 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; - } - - // Run FAST-LIO processing step (high frequency) - fast_lio.process(); - - // Check for new results and accumulate/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(); - 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) { - publish_lidar(filtered, ts); - last_pc_publish = now; - } - - // Global map: insert, prune, and publish at map_freq - if (global_map) { - global_map->insert(filtered); - - if (now - last_map_publish >= map_interval) { - 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; - } - } - } - - // Publish odometry (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; - } - } - - // Handle LCM messages + 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); - // 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) { - 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 - if (debug) printf("[fastlio2] Shutting down...\n"); - g_fastlio = nullptr; + // 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"); } 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/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/fastlio_blueprints.py b/dimos/hardware/sensors/lidar/fastlio2/fastlio_blueprints.py index 89e8ec2c14..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, map_voxel_size=voxel_size, map_freq=-1), + 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, map_voxel_size=voxel_size, map_freq=3.0), + FastLio2.blueprint(), 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(), 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 7c7ff59ff8..32150a6eb2 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -14,35 +14,27 @@ """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. +Binds Livox SDK2 into FAST-LIO-NON-ROS for real-time LiDAR SLAM; outputs +sensor/body-frame point clouds (register via the odometry pose) 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() +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 ipaddress -from pathlib import Path -import socket +import os import time -from typing import TYPE_CHECKING, Annotated +from typing import TYPE_CHECKING, Literal -from pydantic.experimental.pipeline import validate_as +from pydantic import Field 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.net import resolve_host_ip from dimos.hardware.sensors.lidar.livox.ports import ( SDK_CMD_DATA_PORT, SDK_HOST_CMD_DATA_PORT, @@ -55,39 +47,37 @@ 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_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 +from dimos.spec import perception -_CONFIG_DIR = Path(__file__).parent / "config" -_logger = setup_logger() +# Human-readable enums; the C++ binary maps these strings to FAST-LIO's int codes. +LidarType = Literal["livox", "velodyne", "ouster"] +TimestampUnit = Literal["second", "millisecond", "microsecond", "nanosecond"] 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 - # 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 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. + # 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" # FAST-LIO internal processing rates msr_freq: float = 50.0 @@ -97,24 +87,38 @@ 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 - - # 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("mid360.yaml") - debug: bool = False + # 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 + # 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. 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 + b_gyr_cov: float = 0.0001 + 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 + 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] + ) + # 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 + # 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 @@ -127,38 +131,12 @@ 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 - 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"}) - - 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 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: @@ -193,72 +171,15 @@ 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, - ) - - # 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"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}" - ) + if not lidar_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, - ) + "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) diff --git a/dimos/hardware/sensors/lidar/fastlio2/recorder.py b/dimos/hardware/sensors/lidar/fastlio2/recorder.py new file mode 100644 index 0000000000..08784cfe89 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/recorder.py @@ -0,0 +1,57 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Record FAST-LIO odometry + lidar into a memory2 SQLite db. + +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 +the latest odometry pose so ``fastlio_lidar`` carries the trajectory and ``dimos +map global`` can register it. +""" + +from __future__ import annotations + +from dimos.core.stream import In +from dimos.memory2.module import OnExisting, Recorder, RecorderConfig, pose_setter_for +from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + + +class FastLio2RecorderConfig(RecorderConfig): + # Append into a populated db (keep other streams); replace only our own. + on_existing: OnExisting = OnExisting.APPEND + + +class FastLio2Recorder(Recorder): + config: FastLio2RecorderConfig + + fastlio_odometry: In[Odometry] + fastlio_lidar: In[PointCloud2] + + _last_odom_pose: Pose | None = None + + @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("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. + 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 new file mode 100644 index 0000000000..794c24a327 --- /dev/null +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -0,0 +1,518 @@ +# 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. + +""" +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 (defaults to .db next to the pcap) + python -m dimos.hardware.sensors.lidar.fastlio2.tools.pcap_to_db --pcap "$PCAP_PATH" + + # 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" --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) + DB="mem2.db" + 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 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 +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 +from pathlib import Path +import sqlite3 +import subprocess +import sys +import time +from typing import TYPE_CHECKING, Any + +if TYPE_CHECKING: + from dimos.core.coordination.blueprints import Blueprint + +# 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 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 +# 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 +# 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: + 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: + """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]: + """(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) + try: + try: + row = con.execute(f"SELECT COUNT(*), MIN(ts), MAX(ts) FROM '{table}'").fetchone() + except sqlite3.OperationalError: + return 0, 0.0, 0.0 + return row[0] or 0, row[1] or 0.0, row[2] or 0.0 + finally: + con.close() + + +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. + + 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 + + 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: 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(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()) / ((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). + + 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 + from dimos.hardware.sensors.lidar.fastlio2.recorder import FastLio2Recorder + from dimos.hardware.sensors.lidar.virtual_mid360.module import VirtualMid360 + + fastlio_kwargs: dict[str, Any] = dict( + host_ip=args.host_ip, lidar_ip=args.lidar_ip, odom_freq=args.odom_freq, debug=False + ) + 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)), + ) + .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( + 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 _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 (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", + 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 + + pcap_path = Path(args.pcap).expanduser() + if not pcap_path.exists(): + 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) + 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 + # 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} stop_at={max_sensor_sec or 'drain'}", + flush=True, + ) + + 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) + finally: + if coord is not None: + coord.stop() + + 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 + print( + 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, _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: + 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 + + +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 -> 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)" + ) + parser.add_argument( + "--odom-freq", type=float, default=30.0, help="FAST-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( + "--no-rrd", + 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" + ) + parser.add_argument( + "--warmup-sec", + type=float, + default=4.0, + help="seconds the fake lidar waits before streaming (lets FAST-LIO come up first)", + ) + parser.add_argument( + "--config", + 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", + ) + # 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="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)", + ) + + args = parser.parse_args(argv) + if not args.pcap: + parser.error("--pcap is required") + return _run(args) + + +if __name__ == "__main__": + raise SystemExit(main(sys.argv[1:])) diff --git a/dimos/hardware/sensors/lidar/virtual_mid360/blueprints.py b/dimos/hardware/sensors/lidar/virtual_mid360/blueprints.py index d3635a3f63..bd8902c408 100644 --- a/dimos/hardware/sensors/lidar/virtual_mid360/blueprints.py +++ b/dimos/hardware/sensors/lidar/virtual_mid360/blueprints.py @@ -12,17 +12,25 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""PointLio fed by a VirtualMid360 replaying a recorded pcap (live SDK path). +"""Demos: a SLAM consumer fed by a VirtualMid360 replaying a pcap (live SDK path). 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. +DIMOS_FASTLIO_* / DIMOS_POINTLIO_* for the consumer); 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.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_fastlio = autoconnect( + VirtualMid360.blueprint(), + FastLio2.blueprint(), + vis_module("rerun"), +).global_config(n_workers=3, robot_model="virtual_mid360_fastlio") + demo_virtual_mid360_pointlio = autoconnect( VirtualMid360.blueprint(), PointLio.blueprint(), diff --git a/dimos/memory2/module.py b/dimos/memory2/module.py index ac4bd35f24..600f82a592 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 @@ -44,6 +47,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 +255,7 @@ class OnExisting(str, enum.Enum): OVERWRITE = "overwrite" ERROR = "error" BACKUP = "backup" + APPEND = "append" class RecorderConfig(MemoryModuleConfig): @@ -259,10 +264,32 @@ 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 + # 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"] + + +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:: @@ -271,10 +298,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() @@ -285,13 +322,17 @@ 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 # 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 +344,20 @@ def start(self) -> None: else: raise FileExistsError(f"Recording already exists: {db_path}") - if not self.inputs: + self._prepare_streams() + + if not self.inputs and not self.config.record_tf: 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_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)", name, port.type.__name__) + logger.info("Recording %s -> %s (%s)", name, stream_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. @@ -323,23 +370,73 @@ 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 _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.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()): + 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.""" + 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. 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 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 + ) + 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" (no-op without a pubsub tf).""" + topic = getattr(self.tf.config, "topic", None) + 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) + + 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(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/all_blueprints.py b/dimos/robot/all_blueprints.py index 291af8540e..0b35c6e75e 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.virtual_mid360.blueprints:demo_virtual_mid360_fastlio", "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", @@ -158,6 +159,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", diff --git a/dimos/robot/diy/alfred/blueprints/alfred_nav.py b/dimos/robot/diy/alfred/blueprints/alfred_nav.py index 91a8db9bec..27f80cea65 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,9 +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"], - map_freq=1.0, - config="default.yaml", ), create_nav_stack(**nav_config), MovementManager.blueprint(), @@ -73,7 +70,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..e3ccbe6a36 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,10 +27,7 @@ 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"], - 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") 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, }, 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