From 6ba9ebf7b2b79b7b85eb279a7933ecf419b354c0 Mon Sep 17 00:00:00 2001 From: "Deng, Bing" Date: Wed, 15 Jul 2026 14:15:38 +0800 Subject: [PATCH] Stop VIO waiting on the LiDAR buffer in LIVO mode sync_packages() rejects every call up front when lid_raw_data_buffer is empty, before the mode-specific switch below even runs. In LIVO mode the LIO and VIO steps alternate, and the VIO step never touches lid_raw_data_buffer at all - it only reads img_buffer and imu_buffer. Once LIO has drained the LiDAR queue, that lone precondition still forces VIO to stall until the next LiDAR scan lands, adding tens of milliseconds of pure gate wait before every VIO update with no benefit. Move the LiDAR-buffer check out of the shared preamble and into the two switch branches that actually consume the buffer: the ONLY_LIO case and the LIVO WAIT/VIO case that performs the LIO step. The LIVO case that performs the VIO step is left without the check, since it has no dependency on lid_raw_data_buffer. Measured on our platforms this removes on the order of 30 ms of gate wait per VIO update at 10 Hz LiDAR input, with the effect shrinking as LiDAR rate rises and the queue refills sooner. Signed-off-by: Deng, Bing --- src/LIVMapper.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/LIVMapper.cpp b/src/LIVMapper.cpp index 15af3540..f7567ac3 100755 --- a/src/LIVMapper.cpp +++ b/src/LIVMapper.cpp @@ -883,7 +883,6 @@ void LIVMapper::img_cbk(const sensor_msgs::ImageConstPtr &msg_in) bool LIVMapper::sync_packages(LidarMeasureGroup &meas) { - if (lid_raw_data_buffer.empty() && lidar_en) return false; if (img_buffer.empty() && img_en) return false; if (imu_buffer.empty() && imu_en) return false; @@ -891,6 +890,7 @@ bool LIVMapper::sync_packages(LidarMeasureGroup &meas) { case ONLY_LIO: { + if (lid_raw_data_buffer.empty() && lidar_en) return false; if (meas.last_lio_update_time < 0.0) meas.last_lio_update_time = lid_header_time_buffer.front(); if (!lidar_pushed) { @@ -949,6 +949,7 @@ bool LIVMapper::sync_packages(LidarMeasureGroup &meas) case WAIT: case VIO: { + if (lid_raw_data_buffer.empty() && lidar_en) return false; // printf("!!! meas.lio_vio_flg: %d \n", meas.lio_vio_flg); double img_capture_time = img_time_buffer.front() + exposure_time_init; /*** has img topic, but img topic timestamp larger than lidar end time,