通过理解 ROS 2 QoS 解决间歇性 FAST-LIO 启动延迟问题

How We Solved an Intermittent FAST-LIO Startup Latency Bug by Understanding ROS 2 QoS

1. Introduction

One of the most frustrating software bugs is one that cannot be reproduced consistently.

Recently, while developing an indoor SLAM drone platform based on Livox Mid360, FAST-LIO, ROS 2 Humble, and PX4, we encountered an intermittent startup problem. Most of the time the system worked perfectly, but occasionally FAST-LIO would start with a significant delay, causing poor localization performance and unstable downstream navigation.

At first glance, the problem appeared to be random. However, after several days of systematic debugging, we identified the actual root cause and implemented a permanent solution.

2. System Configuration

Hardware & Software:
• Jetson (Ubuntu 22.04)
• ROS 2 Humble
• Livox Mid360 LiDAR
• FAST-LIO
• PX4 Flight Controller
• MAVROS
• Custom ROS2 bridge

Expected Performance:
• FAST-LIO Odometry: 10 Hz
• End-to-end latency: approximately 30 ms
• Stable startup after every reboot

警告 3. The Problem

Most startups behaved normally. Occasionally, however, FAST-LIO would exhibit:

Very slow initialization
Delayed odometry output
Poor synchronization between IMU and LiDAR
Navigation instability

Even stranger, rebooting the system could make the problem disappear. Intermittent bugs like this are extremely difficult to diagnose because they often appear to be caused by hardware, timing, or operating system behavior.

4. Initial Assumptions

During the investigation we considered many possible causes:

• Livox driver delay
• PX4 communication delay
• ROS2 executor scheduling
• DDS communication
• CPU performance
• Thread synchronization
• FAST-LIO mapping algorithm
• Timestamp synchronization
• Network latency

None of these explanations fully matched the observed behavior.

5. The Debugging Strategy

Instead of guessing, we decided to instrument the code.

Instrumentation added:
• IMU callback age
• LiDAR callback age
• IMU minus LiDAR timestamp
• Mapping computation time
• Publishing latency

This transformed the debugging process from speculation into measurement.

6. The Discovery

The mapping algorithm itself was not slow. Instead, we discovered something unexpected.

Occasionally, the IMU callback was receiving data that was already more than one second old.

Typical healthy system:
IMU callback age: 0.0002 seconds

Faulty startup:
IMU callback age: 1.3 seconds

This explained everything. FAST-LIO was processing stale sensor data instead of fresh sensor data. The localization algorithm was simply doing exactly what it was told. The problem was occurring before the algorithm even started.

7. Understanding ROS 2 QoS

ROS 2 communication is controlled by Quality of Service (QoS). QoS defines how messages are delivered between publishers and subscribers.

Important policies include:
• Reliability
• History
• Queue depth
• Durability

Different applications require different communication behavior.

Configuration data should never be lost.

Camera images, however, should always be the newest frame rather than an old frame that has been waiting in a queue.

High-rate sensors such as IMUs belong to the second category. Freshness is more important than perfect delivery.

8. The Original Code

The IMU subscriber originally used a generic queue depth:

sub_imu = create_subscription<sensor_msgs::msg::Imu>(
 imu_topic,
 10,
 callback);

Although this configuration works in many situations, it is not specifically optimized for high-rate sensor streams. Under certain startup conditions, stale IMU messages could remain in the communication pipeline.

通过 9. The Solution

The subscriber was changed to use the ROS 2 sensor profile:

sub_imu = create_subscription<sensor_msgs::msg::Imu>(
 imu_topic,
 rclcpp::SensorDataQoS(),
 callback);

SensorDataQoS is specifically designed for sensors such as:
• IMU
• LiDAR
• Camera
• Radar

Its philosophy is simple:
"If old sensor data exists, discard it and process the newest data."

For localization systems, this behavior is almost always preferable.

10. Validation

After implementing the change, we performed extensive testing.

Validation included:
• Multiple software restarts
• Multiple complete system reboots
• Continuous runtime testing
• Odometry frequency monitoring
• End-to-end latency measurement

Results:
Odometry frequency: Approximately 10 Hz
End-to-end latency: Approximately 30–35 ms

The previous intermittent startup delay did not reappear.

11. What We Learned

The biggest lesson was that algorithms are not always the problem. If sensor data arrives late, even the best SLAM algorithm cannot produce correct results.

When debugging robotics systems:
• Do not only measure computation time.
• Also measure data age.
• These are completely different quantities.

Many localization problems are actually communication problems.

12. Practical Advice

If you encounter intermittent startup delays in ROS 2 SLAM systems:

1. Verify message timestamps.
2. Measure callback latency.
3. Inspect QoS settings.
4. Compare message age instead of only message frequency.
5. Instrument every stage of the pipeline before changing algorithms.

Systematic measurement is far more effective than trial-and-error debugging.

13. Conclusion

This debugging session solved more than a single software bug. It improved our understanding of how ROS 2 middleware, DDS communication, sensor timing, and FAST-LIO interact.

Changing a single line of code was simple. Discovering which line to change required careful observation, quantitative measurement, and a structured debugging methodology.

Sometimes the most important optimization is not making an algorithm faster—it is ensuring that the algorithm receives fresh data at the right time.

平台：Jetson · ROS2 Humble · Livox Mid360 · FAST-LIO · PX4
(c) 卓博泰科技 · FAST-LIO Startup Latency Bug Fix

How We Solved an Intermittent FAST-LIO Startup Latency Bug

Related Drobotics guides