【FAST-LIO2 延迟维修】ROS2 实机部署 Odometry 延迟 1300ms → 68ms 修复全记录

前言我们在前几期已经部署好了完整的 ROS2 Humble FAST-LIO2 Mid-360 实机链路【10天速通ROS2-PX4无人机】(二) FAST-LIO2 仿真部署200HZ IMU前推的无限魅力本期就来讲讲在实际部署经常会遇到的延迟问题从时钟同步、消息格式、QoS、缓冲区堆积、启动顺序五个层面逐一拆解本文用的硬件为livox-mid360软件为livox_ros_driver2 v1.2.6Livox-SDK2FAST-LIO2 (hku-mars/FAST_LIO ROS2 分支)ROS2 humbleubuntu 22.04LTS (NVIDIA Jetson Orin NX)备注请永远相信 FAST-LIO2正常飞行场景下非高速、非无特征环境算法本身不会产生大幅延迟或波动。本文所描述的所有修复针对的都是数据进入算法之前的外部堆积——要么是雷达自身发布延迟要么是驱动到 SLAM 之间的缓冲积压。算法内部没有问题。文章目录前言1 问题表现2 雷达延迟确认2-1 PTP 时钟同步2-2 消息格式差异 — CustomMsg vs PointCloud22-3 lidar_type 映射陷阱3 QoS 修复 — RELIABLE 到 BEST_EFFORT4 Buffer 堆积修复 — 核心问题4-1 堆积为什么发生4-2 修复一kdtree 初始化后重置缓冲4-3 修复二sync_packages 跳帧 时间戳新鲜度兜底4-4 启动顺序从源头避免堆积总结1 问题表现启动 FAST-LIO2 Mid-360 雷达后写一个脚本同时检测雷达点云和 Odom 的延迟。核心逻辑是对比消息的header.stamp和当前系统时间extra列 Odom 延迟 - 雷达延迟即 FAST-LIO2 自身的处理开销#!/bin/bash# check_delay_all.sh — 同时输出雷达和 Odom 延迟source/opt/ros/humble/setup.bashsource/home/terra/px4_ws/install/setup.bash2/dev/null python3-c import rclpy, time, signal from rclpy.qos import QoSProfile, ReliabilityPolicy from sensor_msgs.msg import PointCloud2 from nav_msgs.msg import Odometry rclpy.init() n rclpy.create_node(delay_checker) lidar_delay 0.0 def cb_lidar(msg): global lidar_delay s msg.header.stamp.sec msg.header.stamp.nanosec / 1e9 lidar_delay (time.time() - s) * 1000 def cb_odom(msg): global lidar_delay s msg.header.stamp.sec msg.header.stamp.nanosec / 1e9 odom_delay (time.time() - s) * 1000 extra odom_delay - lidar_delay status OK if odom_delay 200 else (WARN if odom_delay 500 else DELAY) print(f\rlidar{lidar_delay:5.0f}ms odom{odom_delay:5.0f}ms extra{extra:5.0f}ms [{status}] , end, flushTrue) n.create_subscription(PointCloud2, /livox/lidar, cb_lidar, 10) qos_be QoSProfile(depth10, reliabilityReliabilityPolicy.BEST_EFFORT) n.create_subscription(Odometry, /Odometry, cb_odom, qos_be) # 与发布端 QoS 一致 print( lidar odom extra(FAST-LIO2 overhead)) print( -------- ------- -------------------------) rclpy.spin(n) 正常时输出lidar odom extra(FAST-LIO2 overhead)lidar54msodom68msextra14ms[OK]lidar55msodom70msextra15ms[OK]异常时输出lidar odom extra(FAST-LIO2 overhead)lidar54msodom1388msextra1334ms[DELAY]lidar55msodom1373msextra1318ms[DELAY]雷达本身稳定在 54ms但 Odom 的extraFAST-LIO2 处理开销飙到了1300ms。延迟显然不是雷达端引入的问题在 FAST-LIO2 的内部处理链路中2 雷达延迟确认2-1 PTP 时钟同步Livox Mid-360 使用 PTPPrecision Time Protocol精密时间协议做时钟同步master 端机载电脑通过ptp4l与雷达 slave 端保持纳秒级一致出厂默认的ptp4l.service缺少-f automotive-master.cfg配置文件导致数据时钟未锁定雷达以自由运行模式发数据——时间戳以~31ms/s的速率持续漂移说人话就是雷达和电脑的手表对不上每过一秒差 31ms几分钟后差好几秒点云时间戳全部错乱正确配置如下# /etc/linuxptp/mid360_master.cfg[global]delay_mechanism E2E# E2E (End-to-End) 延迟测量模式logSyncInterval1# 125ms 快速 Sync 间隔clockClass6# 高时钟质量声明clockAccuracy 0x20# /etc/systemd/system/ptp4l.service [Unit] DescriptionLinux PTP (ptp4l) - Mid-360 Master Clock Sync Afternetwork-online.target Wantsnetwork-online.target [Service] Typesimple ExecStart/usr/sbin/ptp4l -f /etc/linuxptp/mid360_master.cfg -i enP8p1s0 -S -m Restarton-failure RestartSec5 [Install] WantedBymulti-user.target注意网口名enP8p1s0是 Jetson 平台的其他平台可能是eth0需要根据ip addr的实际输出填写启用服务sudosystemctl daemon-reloadsudosystemctlenableptp4lsudosystemctl start ptp4l关于phc2sys如果网卡没有硬件 PTP 时钟ethtool -T enP8p1s0显示PTP Hardware Clock: none则不需要也装不上phc2sys。此时ptp4l使用软件时间戳模式-S参数雷达会以 Jetson 为 PTP master 进行同步2-2 消息格式差异 — CustomMsg vs PointCloud2livox_ros_driver2的xfer_format参数控制数据输出格式xfer_format 1CustomMsgLivox 私有格式包含逐点时间戳、tag、line 等丰富信息xfer_format 0PointCloud2标准 ROS2 点云格式pcl::PointXYZI结构实测 CustomMsg 延迟~200ms、PointCloud2 延迟~60ms为什么 CustomMsg 慢这么多直接看驱动源码lddc.cpp中两条路径的实现差异PointCloud2 路径——先拷到一个局部vector然后一次memcpy写入消息体// lddc.cpp, InitPointcloud2Msg()std::vectorLivoxPointXyzrtltpoints;for(size_t i0;ipkg.points_num;i){LivoxPointXyzrtlt point;point.xpkg.points[i].x;// ... 逐字段赋值 ...points.push_back(std::move(point));// 拷入局部 vector连续内存}cloud.data.resize(pkg.points_num*sizeof(LivoxPointXyzrtlt));memcpy(cloud.data.data(),points.data(),// ← 一次 memcpy 写入消息pkg.points_num*sizeof(LivoxPointXyzrtlt));CustomMsg 路径——每个点逐一push_back进 ROS 消息体// lddc.cpp, FillPointsToCustomMsg()for(uint32_ti0;ipoints_num;i){CustomPoint point;point.xpoints[i].x;// ... 逐字段赋值 ...point.offset_timestatic_castuint32_t(points[i].offset_time-pkg.base_time);livox_msg.points.push_back(std::move(point));// ← 每个点一次 push_back// 每次 push_back 可能触发 vector 扩容 → 重新分配 整体拷贝}差异总结PointCloud2N次字段赋值 1次memcpy连续内存块拷贝CPU cache 友好CustomMsgN次字段赋值 N次push_back每次都可能触发 vector 扩容重分配Mid-360 每帧约 2 万个点累计开销巨大说人话就是PointCloud2 先把点拷到一块连续内存然后啪一下全贴过去。CustomMsg 是一个一个往购物车里扔购物车满了还要换辆更大的再搬一遍——2 万个点搬很多遍关键结论实物 Mid-360 优先用xfer_format0PointCloud2。CustomMsg 的逐点时间戳在大多数场景下不是刚需——FAST-LIO2 用 IMU 反向补偿去畸变不依赖 Livox 私有时间戳2-3 lidar_type 映射陷阱设xfer_format0后雷达发的是标准 PointCloud2但 FAST-LIO2 的 YAML 配置里有一个容易踩的坑lidar_type: 1→ 走 AVIA 分支 → 订阅 CustomMsg → 收不到 PointCloud2 → 永远没数据 → 不发 odomlidar_type: 4→ 走 mid360_handler → 写死了reflectivity字段但驱动 2.0 用的是intensity字段 → 点云强度为 0但还能跑最可靠的方案改用lidar_type: 5将其映射到default_handler——直接用标准pcl::PointXYZI解析不关心tag/line/reflectivity等私有字段FAST-LIO2 的本质是直接法——它不依赖点云的线号来提取特征所以default_handler完全够用但default_handler有一个代价PointXYZI结构不包含逐点时间偏移。看源码preprocess.cpp// preprocess.cpp, default_handler()// PointCloud2 用 pcl::PointXYZI 解析, 只有 x/y/z/intensityfor(uint i0;iplsize;i){// ...added_pt.curvature0.;// ← 没有逐点时间戳, curvature 全部置零// ...}对比mid360_handlerlidar_type: 4——从扫描角度反向推算每个点的 curvature// preprocess.cpp, mid360_handler()// 通过扫描线的 yaw 角变化推算偏移时间doubleomega_l0.361*SCAN_RATE;// scan angular velocityadded_pt.curvature(yaw_fp[layer]-yaw_angle)/omega_l;// ← 算出了逐点偏移curvature是各点相对扫描起始时间的偏移ms在laserMapping.cpp中被用于计算帧结束时间// laserMapping.cpp, sync_packages()// 情况 A: curvature 有效 → 用最后一点的 curvature 算 lidar_end_timelidar_end_timemeas.lidar_beg_timemeas.lidar-points.back().curvature/double(1000);// 情况 B: curvature0PointCloud2 default_handler 走这里// 判断: 0 0.5 * lidar_mean_scantime → TRUE → 用默认估时lidar_end_timemeas.lidar_beg_timelidar_mean_scantime;// lidar_mean_scantime 1.0 / scan_rate 100ms10Hz或 50ms20Hzdefault_handler不走mid360_handler的角度推算直接curvature0导致 FAST-LIO2 用scan_rate的固定估值代替精确帧时长。这对去畸变精度有一点点影响但 FAST-LIO2 用 IMU 的前向/后向传播来补偿实际效果完全够用# mid360.yaml 关键参数preprocess:lidar_type:5# default_handler — 标准 pcl::PointXYZIscan_line:4blind:0.1point_filter_num:1scan_rate:20# 与驱动 publish_freq 一致, 用于推算帧时长3 QoS 修复 — RELIABLE 到 BEST_EFFORTFAST-LIO2 默认以 RELIABLE QoS 发布/Odometry和/Odom_high_freq。RELIABLE 模式下发布者会缓存消息直到订户确认——如果下游如vins_to_mavros处理慢消息就堆积订户收到的是旧数据改成 BEST_EFFORT旧消息直接丢弃订户始终拿到最新的// laserMapping.cpp, Node 初始化时// 修改前默认 RELIABLE:pubOdomAftMapped_this-create_publishernav_msgs::msg::Odometry(/Odometry,20);// 修改后BEST_EFFORT:pubOdomAftMapped_this-create_publishernav_msgs::msg::Odometry(/Odometry,rclcpp::SensorDataQoS());// BEST_EFFORT: 旧消息直接丢弃pubOdomHighFreq_this-create_publishernav_msgs::msg::Odometry(/Odom_high_freq,rclcpp::SensorDataQoS());注意下游订阅方如桥接节点也必须使用 BEST_EFFORT 订阅否则 RELIABLE 订阅 BEST_EFFORT 发布 QoS 不兼容收不到消息4 Buffer 堆积修复 — 核心问题4-1 堆积为什么发生FAST-LIO2 的laserMapping.cpp中维护了两个关键队列// 第 109-111 行dequedoubletime_buffer;// LiDAR 帧时间戳队列dequePointCloudXYZI::Ptrlidar_buffer;// LiDAR 帧点云队列dequesensor_msgs::msg::Imu::ConstSharedPtrimu_buffer;每帧 LiDAR 到来时回调函数将其推入队尾第 369-370 行lidar_buffer.push_back(ptr);time_buffer.push_back(cur_time);处理函数sync_packages()每次从队首front取帧第 469-470 行meas.lidarlidar_buffer.front();meas.lidar_beg_timetime_buffer.front();这是一个 FIFO先进先出队列。正常情况下队列深度 1-2 帧处理速度追得上传入速度当出现 “No point, skip this scan”地面盲区、晃动导致有效点不足时帧被弹出但不做处理新帧持续推入。反复几次后队首全是旧帧恢复后front()取到的还是堆积期的旧帧——其时间戳可能已经在1.3s之前。之后发布的每一条 Odom 用的都是这个过时的时间戳说人话就是雷达一直在发但 SLAM 罢工了几秒。恢复后第一口吃到的是一盘馊菜——后面的新鲜菜虽然也摆上来了但得按顺序先把馊菜吃完4-2 修复一kdtree 初始化后重置缓冲FAST-LIO2 在接收到第一帧有效点云时会初始化 kdtree增量地图数据结构。kdtree 初始化之前已经有若干帧被sync_packages弹出但未能成功处理点数不足它们的时间戳被写入Measures.lidar_beg_time初始化完成后这一帧带着旧时间戳继续往下走最终被写入 odometry。而且队列里可能还堆着更多旧帧在 kdtree 初始化完成处加一段清队代码并return跳过当前帧的 odometry 发布// laserMapping.cpp, timer_callback() → kdtree 初始化块/*** initialize the map kdtree ***/if(ikdtree.Root_Nodenullptr){RCLCPP_INFO(this-get_logger(),Initialize the map kdtree);// 清空初始化期间堆积的缓冲// 当前帧已被 sync_packages 弹出, 时间戳是旧的, return 跳过// 下次回调会拿到清空后最新入队的帧intdroppedlidar_buffer.size();lidar_buffer.clear();time_buffer.clear();imu_buffer.clear();RCLCPP_INFO(this-get_logger(),Cleared %d lidar frames imu buffer during init.,dropped);if(feats_down_size5){ikdtree.set_downsample_param(filter_size_map_min);// ... kdtree Build ...ikdtree.Build(feats_down_world-points);}return;// 跳过当前旧帧, 下次回调拿新鲜的}这段代码只执行一次ikdtree.Root_Node nullptr在 kdtree 建好之后永远为 false解决了初始化阶段的堆积问题4-3 修复二sync_packages 跳帧 时间戳新鲜度兜底初始化只清一次运行期间也可能因为 “No point” 导致堆积。在sync_packages取帧时加保护直接给出最终方案——同时基于队列数量和__时间戳新鲜度__判断超过200ms的帧全部丢弃队列空了就等新数据。不走中间版本一步到位// laserMapping.cpp, sync_packages() 函数中, 约第 469 行/*** push a lidar scan ***/if(!lidar_pushed){doublenow_secrclcpp::Clock(RCL_SYSTEM_TIME).now().nanoseconds()/1e9;intskipped0;// 主要路径: 丢弃时间戳 200ms 之前的所有帧包括最后一帧// 雷达振动/暂停后可能一次性泻出一批历史帧——全丢, 等新鲜的while(lidar_buffer.size()0(now_sec-time_buffer.front())0.2){lidar_buffer.pop_front();time_buffer.pop_front();skipped;}// 兜底: 时间戳都新鲜但队列数量异常正常走不到这里if(skipped0lidar_buffer.size()3){while(lidar_buffer.size()1){lidar_buffer.pop_front();time_buffer.pop_front();skipped;}}if(skipped0)printf([WARN] Skipped %d old LiDAR frame(s) to prevent odometry delay.\n,skipped);// 全部过期 → 等新鲜帧, 下次回调再试if(lidar_buffer.empty())returnfalse;meas.lidarlidar_buffer.front();meas.lidar_beg_timetime_buffer.front();// ...}两条路径互补主要路径——时间戳检测覆盖雷达自身缓存后泻出的旧帧即使只有 1 帧也是旧的兜底路径——数量检测覆盖处理速度跟不上导致的队列堆积所有帧都新鲜但太多了为什么 IMU 缓冲不能一起跳IMU 数据是 ESKF 前向传播的输入每一帧都必须参与积分。跳 IMU 帧等于缺失状态推演步骤滤波器直接发散。LiDAR 帧可以跳——IMU 已经在这段空白里持续前推状态只需最新一帧来做观测校正三道防线总结kdtree init 时清初始化期堆积 → sync_packages 跳帧时间戳兜底 → 启动顺序反转从源头杜绝。层层递进确保每条 Odom 都带着新鲜时间戳4-4 启动顺序从源头避免堆积三道防线加好后初始化期仍然可能产生堆积——如果 LiDAR 先启、FAST-LIO2 后启启动间隙的几秒内帧已经堆起来了靠防线的跳帧逻辑能清理但实测反转顺序后偶尔仍有Skipped 1-4的轻微触发说明保护逻辑在工作但最好的办法还是从源头避免先启 FAST-LIO2等订阅就绪后再启雷达启动顺序反转后第一条 LiDAR 帧到达时 FAST-LIO2 已经准备好不会产生任何初始堆积。修改后的完整启动脚本如下#!/bin/bash# # 1_bringup.sh — FAST-LIO2 Mid-360 雷达一键启动# 先启 SLAM → 等 Node init → 再启雷达, 避免初始帧堆积# set-esource/opt/ros/humble/setup.bashsource/home/terra/px4_ws/install/setup.bash# 清理残留pkill-9-ffastlio_mapping2/dev/null||truepkill-9-flivox_ros_driver22/dev/null||truesleep2# PTP 检查ifsystemctl is-active--quietptp4l;thenecho[INFO] ptp4l is activeelseecho[WARN] ptp4l not running, starting...echoterra|sudo-Ssystemctl start ptp4lsleep2fi# 网络检查ping-c1-W2192.168.1.165/dev/nullecho[INFO] Mid-360 reachable# ---- 先启 FAST-LIO2, 等初始化完成 ----echo[INFO] Starting FAST-LIO2 first...ros2 launch fast_lio mapping.launch.py config_file:mid360.yaml rviz:falseFASTLIO_PID$!sleep8# ---- 再启雷达, 此时 SLAM 已就绪 ----echo[INFO] Starting LiDAR driver...ros2 launch livox_ros_driver2 msg_MID360_launch.pyRADAR_PID$!wait$FASTLIO_PID2/dev/nullkill$RADAR_PID2/dev/null总结本文从实机部署 FAST-LIO2 时遇到的1300ms Odom 延迟问题出发系统排查了四个层面最终将延迟从1300ms降到稳定的68ms核心要点回顾雷达端PTP 正确配置 → 选 PointCloud2 格式 → 正确设置 lidar_typeQoS 修复RELIABLE → BEST_EFFORT订户始终拿到最新消息三道防线kdtree init 清堆积 → sync_packages 跳帧时间戳兜底 → 启动顺序从源头杜绝启动脚本反转启动顺序FAST-LIO2 先启等就绪后再启雷达所有修复集中在src/FAST_LIO/src/laserMapping.cpp约 30 行修改、config/mid360.yaml4 个参数调整、launch_ROS2/msg_MID360_launch.py2 个参数调整和启动脚本启动顺序反转无需改动 FAST-LIO2 的算法核心如有错误欢迎指出感谢观看