FAST_LIO项目中里程计数据中断问题的分析与解决

问题现象描述

在FAST_LIO激光雷达惯性里程计项目中，用户在使用Jetson Xavier NX硬件平台搭配Livox MID360雷达时，发现里程计数据输出存在3-4秒的中断现象。通过调试发现，雷达数据本身没有问题，但cloudregistered话题的发布过程会出现阻塞，导致里程计数据输出中断。

问题特征分析

1. 随机性中断：中断出现的位置不确定，难以稳定复现，但发生频率较高
2. 系统资源正常：问题发生时CPU温度约47℃，CPU使用率不高
3. 时间戳跳跃：调试日志显示时间戳从1734076974.8直接跳到1734076977.113
4. 数据处理堆积：数据堵塞后会在一个时间戳集中处理

根本原因分析

经过深入排查，发现问题主要由以下几个因素导致：

1. 消息队列设置不合理：原始代码中雷达和IMU的订阅队列设置过大，导致消息积压
2. 主循环频率过高：主循环频率设置为5000Hz，远超过实际需要
3. 处理线程阻塞：当消息队列积压时，数据处理线程会出现阻塞
4. 时间同步问题：高频率下时间同步机制可能出现异常

解决方案

针对上述问题，我们采取了以下优化措施：

1. 调整消息队列大小：

   • 将雷达数据订阅队列从默认值调整为适当大小（建议10-20）
   • 同步调整IMU数据订阅队列大小

2. 优化主循环频率：

   • 将主循环频率从5000Hz降低到合理范围（建议100-200Hz）
   • 根据实际硬件性能调整频率参数

3. 改进数据处理流程：

   • 增加数据处理的超时机制
   • 实现数据处理的优先级队列
   • 优化时间戳同步算法

4. 资源监控与告警：

   • 增加系统资源监控模块
   • 当检测到消息积压时触发告警
   • 实现自适应频率调节机制

实施效果

经过上述优化后：

1. 里程计数据中断问题完全解决
2. 系统运行稳定性显著提高
3. CPU资源利用率更加合理
4. 时间戳连续性得到保障

经验总结

在激光雷达惯性里程计系统的开发中，需要特别注意：

1. 消息队列大小应根据实际数据传输速率合理设置
2. 主循环频率不是越高越好，需要与硬件性能匹配
3. 时间同步机制在高频率下容易出现异常
4. 系统资源监控是保证长期稳定运行的关键

通过本次问题的解决，我们对实时系统中的消息处理机制有了更深入的理解，这些经验也可以推广到其他类似的机器人感知系统中。