Handling different rates, delays and out-of-order messages

1. Engineering problem

An IMU at 200 Hz, a camera at 30 Hz and a LiDAR at 10 Hz produce data with different delays, and messages can arrive out of order. An estimator needs data in time order and at consistent times.

2. When is this needed?

• Fusing sensors at different rates.
• When a filter receives a measurement older than its state.
• When network transport reorders messages.

3. Selection and design decisions

The message_filters.TimeSequencer (C++ only; not implemented in Python in Jazzy) delivers messages in stamp order after waiting a delay; a Cache retrieves messages in a time interval. The same ideas are implemented in estimators as a measurement queue with a fixed lag.

4. Advantages and disadvantages

• Longer buffer delay reorders more and adds latency to the output.
• Interpolation assumes smooth change between samples.
• Dropping late data is safe and loses information.

5. Requirements and constraints

• Buffer delay is greater than the maximum expected delay jitter and below the control latency budget.
• Interpolation only between samples close enough in time.
• Dropped and late counts are published.

6. Architecture

streams ─▶ delay buffer (release in stamp order) ─▶ interpolate to target time ─▶ estimator
                        late beyond buffer ─▶ drop + count

7. Implementation

1. Measure the delay jitter and the reordering distance of each stream.
2. Set the buffer delay just above the maximum.
3. Release in stamp order (below) and count anything later.
4. Interpolate fast streams to the measurement time of the slow one.
5. Check the added latency against the control requirement.

import bisect
arrivals = [(0.00, 0.00), (0.05, 0.05), (0.12, 0.10), (0.11, 0.15), (0.20, 0.20)]   # (stamp, arrival time) with one swap
def sequence(msgs, delay):
    buf, out = [], []
    for stamp, arrival in sorted(msgs, key=lambda m: m[1]):
        bisect.insort(buf, stamp)
        while buf and buf[0] <= arrival - delay:
            out.append(buf.pop(0))
    return out + buf
print("raw order:", [m[0] for m in sorted(arrivals, key=lambda m: m[1])])
print("sequenced:", sequence(arrivals, 0.05))
def interp(t, ts, vs):
    i = bisect.bisect_right(ts, t)
    t0, t1, v0, v1 = ts[i - 1], ts[i], vs[i - 1], vs[i]
    return v0 + (v1 - v0) * (t - t0) / (t1 - t0)
print("interpolated at 0.123:", round(interp(0.123, [0.10, 0.20], [1.0, 2.0]), 3))

Printed (checked by the site’s tests):

raw order: [0.0, 0.05, 0.12, 0.11, 0.2]
sequenced: [0.0, 0.05, 0.11, 0.12, 0.2]
interpolated at 0.123: 1.23

Reading it: two messages arrive swapped (stamp 0.12 before 0.11); a 50 ms delay buffer releases them in order. A 1 to 2 value stream sampled at 0.10 and 0.20 interpolates to 1.23 at 0.123. The stamps are illustrative; the buffer here is a minimal sketch of the TimeSequencer idea.

8. Real examples

• VIO: IMU samples interpolated to each camera frame time before pre-integration.

9. Failure scenarios

• A filter applying a late measurement as if it were current.
• A buffer shorter than the jitter, so order is still wrong.
• Interpolating across a 1 s gap.

10. Troubleshooting

1. Plot stamp versus arrival to see reordering.
2. Check the late counter.
3. Check the buffer delay against the observed maximum.

11. Validation

• The example prints the lines shown (executed by the site tests).
• No measurement is applied out of order in a long run; the late counter is within the requirement.

12. Engineering checklist

• Delay jitter measured
• Buffer delay set
• Late data counted
• Interpolation limits
• Latency budget checked

13. Further references

• ROS 2 message_filters documentation (Jazzy branch): doc/index.rst, read 2026-11-06.

Related on FixTheRobot