How to Resolve IMU Data Packet Loss or Timestamp Desynchronization in ROS?

Summary

This report presents a thorough analysis of the common problems of random packet loss and timestamp desynchronization for Inertial Measurement Unit (IMU) data that is used in the Robot Operating System (ROS). The above issues are very detrimental to the performance of robotics applications such as Simultaneous Localization and Mapping (SLAM) systems, state estimation, and autonomous navigation, which are based on high-fidelity, temporally-consistent sensor data.

This paper examines the underlying reasons for such problems and provides an organized description of software-based optimization techniques, driver-level modifications and hardware-based synchronization techniques. A general focus is put on the architectural benefits provided by ROS 2, including its Data Distribution Service (DDS) middleware and Quality of Service (QoS) policies which offer more robust solutions than ROS 1. The report concludes that, although software approaches such as message_filters can be used with great success for many applications, to provide maximum precision and reliability, especially at high frequency, hardware-level synchronization is required using triggers such as Pulse Per Second (PPS) signals.

1. Introduction: The Criticality of IMU Data Integrity in ROS

The Inertial Measurement Unit (IMU) is one of the crucial sensors in the field of modern robotics which supplies key information about angular velocity and linear acceleration. In ROS, this data in the form of sensor_msgs/Imu messages is read by a variety of algorithms for ego-motion tracking, sensor fusion, stabilization etc. The assumption made for the performance of these algorithms is that the IMU data stream is full in both time and space, and time-stamped correctly.

However, real world systems are often faced with two crippling problems:

• Random Packet Loss: IMU messages are dropped randomly and not received by nodes that are subscribed to them.
• Timestamp Jumps and Desynchronization: The timestamp present in the message header (header.stamp) is not accurate, consistent or does not match the timestamps of other sensors within the system.

These problems can result in catastrophic failures in robotic systems including divergent state estimates, bad map generation in SLAM, etc. negative behavior and erratic control behavior. This report describes the reasons for these issues, as well as systematic methods for identifying, diagnosing, and rectifying the issues through the optimization of ROS drivers, and the use of hardware synchronization triggers.

2. Identifying & Diagnosis of the Problems

In order to implement a solution, it is crucial to correctly identify the nature of the data problem.

2.1. Identifying Random Packet Loss

Packet loss can be subtle. For instance, even a low percentage of dropped messages could be non-obvious, but could lead to a degradation in algorithm performance over time.

• Network-Level Issues: Most often, they are caused by the underlying communication layer. Unstable networks (Wi-Fi) are subject to interference and therefore have a higher packet loss rate and irregular latency¹. The selection of transport protocol (i.e. UDP) which is the default for most ROS 1 topics, prioritizes low latency over guaranteed transport, and can result in lost packets without any retransmission².
• System Performance Bottlenecks: In the host machine, high CPU load, lack of adequate memory (resulting in Out-Of-Memory errors) or poor configuration of process priorities may prevent a ROS node from processing the incoming messages in time³. If it is not possible to add the message to a publisher’s message queue or a subscriber’s input buffer, the message will be dropped.
• ROS Core and Driver Inefficiencies: Bugs in the ROS system or even in a particular sensor’s driver may cause the loss of data. For example, if the driver is not implemented well enough, it may not be able to read the data from the serial port or network socket at a high enough rate to keep up with the rate of the sensor output, resulting in an internal buffer overflow before the data is even published as a ROS message.

2.2. Identifying Timestamps Jumps and Desynchronization

More insidious is that the timestamps are often inaccurate. A common cause of error is the difference between time of occurrence at the sensor and time of occurrence by the host computer.

• Timestamp Source Ambiguity: Usually, the ROS messages are stamped with the time of the host system ( ros::Time::now() ) when they are published. This adds latency and jitter stemming from scheduling overhead in the OS, communication latency between the sensor and the host and processing overhead in the driver node⁴. Such a timestamped IMU message corresponds to the time that the ROS node processed the data, but not necessarily to the time that the IMU has obtained the data.
• Clock Drift: The internal clocks of the IMU and the host computer (and other sensors) are not perfectly synchronized and will drift with respect to one another over time⁴. This causes increasing out-of-sync throughout the whole system.
• Data Jumps: As a system tries to fuse together data from different sensors with inconsistent timestamps the data can cause incorrect state transformations to occur. For instance, if a timestamp used with tf is considered as “future”, compared to other data, there is a chance of lookup errors. This is common when the data from one sensor is so delayed and another is not.

3. Software Solutions and Driver Optimization

Software-level improvements are the first line of defense and are often enough for many applications. They concern a good management of data in the ROS ecosystem.

3.1. Mitigating Packet Loss by Using Software

• Network Configuration: In ROS 1, most topics are configured as UDP which is used for low latency, but it is possible to configure the subscriptions to use TCP ( TCPROS ) which ensures guaranteed delivery at a higher overhead and possible spikes in the latency. This is a trade-off:TCPROScan ensure no packet loss, but is not well suited to high-frequency and real-time control loops.
• Optimizing Node Performance: Make sure that the machine where the IMU driver and consuming nodes are running has sufficient CPU and memory resources. Diagnose profile nodes with programs such as top or htop to determine bottlenecks to the performance. Optimization of the driver code itself (e.g., by using more efficient data parsing methods and by making sure that I/O is non-blocking) can also drastically reduce the load.
• Buffer Management: The size of the queue is adjustable in a publisher and a subscriber node. That a larger queue can be used to absorb bursts of messages for a short period of time, and not drop messages that come from a short CPU spike, but it can also increase latency⁵.

3.2. Software Timestamp Desynchronization Correction.

The message_filters package is the most potent ROS software used to handle time issues.

• Synchronization using message_filters: This package contains classes which synchronize messages on one topic, based on the values of their header.stamp.
  • message_filters::Synchronizer(ApproximateTime): This is the most popular policy. It gathers messages from different topics and discovers a set of messages with timestamps that are “close enough” each other given a configurable time window. It is resistant to small frequency variations and sensor to sensor jitter⁶. Examples are provided on how to use it to synchronize IMU data with other sensors such as GPS or Velodyne point clouds.
  • message_filters::Synchronizer(ExactTime): This policy will ensure timestamps of identical messages are synchronized. This is hardly ever possible on real hardware without the use of a hardware trigger mechanism, but can be helpful when processing data from a rosbag where timestamps may have been manually sync’d.
• Driver-Level Timestamping: This is an important optimization, change the IMU driver to use one that is nearest to the hardware capture time as possible. It is possible to have many modern IMUs give their own hardware-level timestamps. If the driver is able to obtain this timestamp and insert it into the ROS message header, then it will be much more accurate than using ros::Time::now() on the host computer. This is an optional configuration of some drivers.
• Algorithmic Approaches: For minor, predictable desynchronization, however, it is sometimes possible to use data interpolation or other state estimator, such as a Kalman filter, which can explicitly model and remove the effect of time delays between different sensor inputs⁷. Systems such as VINS-Mono do close temporal coupling of IMU and visual data as part of their optimization process⁸.

4. Hardware Synchronization: Gold Standard Precision

As we will explore, for applications where the highest temporal precision is needed, such as high speed sensor fusion, or visual-inertial odometry, software methods are not sufficient. Hardware synchronization means that multiple sensors can take data at exactly the same time or give all the data a common high-accuracy time reference.

4.1. Hardware Triggering Mechanisms

• Pulse Per Second (PPS) Synchronization: Many of the GNSS/GPS receivers offer PPS signal output that is very accurate. This digital pulse is generated at exactly the same time at the beginning of each second with microsecond accuracy and can be used as a time authority in the entire system.
  • Usage: This PPS signal can either be connected to a pin on the host computer that is an interrupt (or, supported by a few IMUs and some industrial cameras) trigger-capable. The driver of the IMU contains an Interrupt Service Routine (ISR) which can be used to correlate the reception of data packet with the arrival of last PPS pulse for very precise timestamping⁹. The idea behind this is that IMU data is recorded between two PPS signals giving a strict temporal constraint.
• GPIO Triggering: It is possible to have a master controller (e.g. a microcontroller or a standard computer) programmed to provide a periodic trigger pulse on a General-purpose Input/Output (GPIO) pin to a series of sensors. All sensors set for use with this external trigger will begin capture/measurement process with the rising or falling edge of this pulse. This ensures that information is gathered from various sensors (cameras, IMUs, etc.) at the same time.

4.2. Implementation in ROS

Hardware-triggered data acquisition in ROS is not a trivial task that connects the hardware to the drivers and the user-space nodes in ROS. As the search results show, there is a large gap in the public code examples for this, which is end-to-end (Query: What are detailed code examples for implementing PPS-triggered IMU data acquisition in ROS with code examples?, Query: How to implement PPS-triggered IMU data acquisition in ROS using C++ with code examples for specific hardware interfaces??).

However, in a conceptual implementation it would be as follows:

1. Hardware Interface: Interfacing the PPS or the GPIO signal to an interrupt capable pin on the host processor (in the case of say Raspberry Pi, Jetson or Industrial PC).
2. Kernel/Low-Level Driver: A driver or program with the right privileges (i.e. pigpio, /dev/gpiomem on a Raspberry Pi, custom kernel module etc.) would be required to listen to the interrupt from the hardware of the trigger pin.
3. ROS Node Logic: The ROS driver node for the IMU would be developed with the intention of being able to interact with this interrupt. On receiving an interrupt signal it would either:
   • Immediately force measurement of the IMU (if the IMU supports this).
   • More commonly, read the latest data packet from the IMU’s serial/I2C buffer and time stamp it with a high precision timestamp that is associated with the interrupt event This compensates quite well for the variable latency of the communication bus and the OS scheduling.

There are a lot of ROS packages available for reading data from sensors such as MPU6050 over serial or I2C¹⁰. Most of these are software polling loop implementations and they don’t integrate external hardware triggers. To have a PPS-triggered implementation one will need to write custom C++ code that combines low-level hardware access and the ROS publishing mechanism¹¹.

5. Key Advancements in ROS 2 for Data Integrity

In essence, ROS 2 is a completely re-designed version of ROS 1, specifically to overcome most of the shortcomings of ROS 1, primarily in terms of real-time performance and communication reliability. These changes to architecture offer high-power and built-in tools for reducing the problems caused by packet loss and timestamps.

5.1. DDS Middleware & Quality of Service (QoS)

The most significant change in ROS 2 is that the custom TCPROS/UDPROS protocol and central Master node are replaced by the Data Distribution Service (DDS) as its default middleware . This as a powerful feature, the Quality of Service (QoS) policies.

QoS enables developers to control communication behavior of each publisher and subscriber on a topic-by-topic basis. For IMU data, the following policies are of particular interest:

• Reliability Policy: This is directly related to packet loss.
  • BEST_EFFORT: This is similar to ROS 1 default udp behavior. It has low latency as a priority but does not provide delivery.
  • RELIABLE: This policy tells DDS to make sure all messages are delivered and can use acknowledgments and retransmissions to do this, similar to TCP. By as much as adding the reliability policy set to RELIABLE to an IMU topic, developers can effectively remove packet loss due to network errors, a huge improvement over ROS 1.
• History and Depth: This policy regulates the number of messages that can be kept for late joining subscribers. it can be set to KEEP_LAST with some depth, which can be used to manage memory and make sure that the newest data is present in the new nodes.
• Deadline Policy: This can be used to ensure that data is produced at a minimal frequency. If the publisher doesn’t send a message within a set time, a notification can be generated, as in, the system can know if a “stale” or failed sensor node exists.

5.2. Improved Time Handling and Synchronization

While the message_filters package (renamed to the ros2_message_filters) is still used in ROS 2 for software based synchronization, the ROS 2 framework is better prepared with respect to time handling. DDS itself has provisions for timestamping and managing message lifecycle and ROS 2’s architecture is designed to be more suitable for real-time systems . Furthermore, specific drivers for ROS 2 are being developed together with time correction capabilities, with often external signals such as PPS in mind. For example, some drivers expose parameters such as time_correction_en or imu_time_base_control that would allow the users to use the hardware synchronization capabilities directly using configuration.

In other words, ROS 2 does not come with a single “magic” tool in order to synchronise IMUs, but instead offers a far more powerful and configurable communication framework (QoS) and deeper real-time foundation, giving developers better tools to create robust systems.

6. Conclusion and Recommendations

To resolve the issues of IMU data packet loss and timestamp desynchronization in ROS, you need to deal with it in a systematic and multi-layered way. The best solution is application dependent; it depends on the required precision and reliability of the application.

Recommended Action Plan:

1. Diagnose First: Since there are several issues that occur in the communication between ROS nodes, the first step should be to use ROS tools (rostopic hz, rostopic echo, custom analysis scripts) to find out whether the problem is packet loss, or the timestamp inconsistency, or both.
2. Implement Software Solutions:
   • For Desynchronization: The main tool to be used should be message_filters (ApproximateTime policy) to synchronize the IMU data with other incoming sensor streams. Some of the problems to be addressed: Investigate whether the IMU driver can be set up to use hardware-based timestamps from the device itself, rather than host-side ros::Time::now().
   • Packet Loss: For ROS 1, it is necessary to analyze the system performance to find the bottlenecks and use tcpros for the critical topics if latency is not a significant issue. If using ROS 2 the definitive solution is to set the QoS Reliability policy to RELIABLE for the IMU publisher and subscriber.
3. Advance to Hardware Synchronization for High-Precision Needs:
   • For applications where microsecond level accuracy is a must (i.e. tightly coupled visual-inertial SLAM), hardware synchronization is mandatory.
   • The use of a PPS signal from a GNSS receiver is the recommended best practice for establishing a global high-accuracy time reference to timestamp.
   • Using the GPIO based triggering is a great alternative to ensure that we capture on more than one sensor simultaneously when the global time reference is not as important as a relative synchronicity.
   • Preparing for heavy development work in this area is necessary as it will need some custom C++ code to work with hardware interrupts and incorporate them into ROS node that is a field of which only very few examples are available.

Ultimately the migration to ROS 2 is highly encouraged for new robotic systems as the underlying DDS and QOS architecture offers more powerful and flexible mechanisms for building robust, reliable, and temporally-aware applications, and directly addresses the core problems of communication in ROS 1 such as packet loss and timing.

1. Anis Koubaa Editor. “Robot Operating System (ROS) The Complete Reference (Volume 1)“, P. 262, Studies in Computational Intelligence, Springer International Publishing Switzerland, 2016. DOI: 10.1007/978-3-319-26054-9.
2. Tianhao Wang, Ke Chen, Zhaohua Zheng, Jiahao Guo, Xiying Zhao 1 and Shenhui Zhang, “PrivShieldROS: An Extended Robot Operating System Integrating Ethereum and Interplanetary File System for Enhanced Sensor Data Privacy,” P. 21, 2024
3. Yuheng Shen, Jianzhong Liu, Yiru Xu, Hao Sun, Mingzhe Wang, Nan Guan, Heyuan Shi, and Yu Jiang. “Enhancing ROS System Fuzzing through Callback Tracing.” P. 8, Proceedings of the 33rd ACM SIGSOFT International Symposium on Software Testing and Analysis (ISSTA ’24), 16–20 Sept. 2024, Vienna, Austria. ACM, 2024, pp. 1-12. DOI: 10.1145/3650212.3652111.
4. Andrew English, Patrick Ross, David Ball, Ben Upcroft and Peter Corke, “TriggerSync: A Time Synchronisation Tool,” Proceedings of the 2015 IEEE International Conference on Robotics and Automation (ICRA 2015), edited by N. M. Amato, Institute of Electrical and Electronics Engineers Inc., 2015, pp. 6220-6226. DOI: 10.1109/ICRA.2015.7140072.
5. Jorin Kouril, Bernd Schäufele, Ilja Radusch, and Bettina Schnor. “Performance Evaluation of a ROS2 Based Automated Driving System.” Proceedings of the 10th International Conference on Vehicle Technology and Intelligent Transport Systems (VEHITS 2024), SCITEPRESS – Science and Technology Publications, 2024, p. 59. DOI: 10.5220/0012556800003702.
6. Zhang, Jun. “Multi-sensor Calibration and Multi-modal Perception for Intelligent Systems,” P. 41, Doctoral thesis, Nanyang Technological University, Singapore, 2022. DOI: 10.32657/10356/161102.
7. Peter Aerts and Eric Demeester. “Time Synchronisation of Low-cost Camera Images with IMU Data Based on Similar Motion.” Proceedings of the 16th International Conference on Informatics in Control, Automation and Robotics (ICINCO 2019), SCITEPRESS – Science and Technology Publications, 2019, P. 1 of pp. 292-299. DOI: 10.5220/0007837602920299.
8. Tong Qin, Peiliang Li, and Shaojie Shen. “VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator.” arXiv, 13 Aug. 2017, arXiv:1708.03852.
9. David T. Chelmins, Richard T. Powis, Jr., and Obed Sands. “Sampling and Control Circuit Board for an Inertial Measurement Unit.” PP. 9-10, United States Patent, US 9,423,426 B1, 23 Aug. 2016.
10. Heta Diwan, “Development of an Obstacle Detection and Navigation System for Autonomous Powered Wheelchairs,” Master’s thesis, Ontario Tech University, 2019
11. Tang Hailiang, Zhang Tisheng, Bao Linfeng, and Niu Xiaoji. “Design and Evaluation of High Precision Integrated Navigation Module for Rotor UAV.” P. 840, Chinese Journal of Sensors and Actuators, vol. 32, no. 6, 2019, pp. 7230-7320E. doi:10.3969/j.issn.1004-1699.2019.06.007.