Diagnose Sensor Timestamp Problems

The problem

SLAM Toolbox reads /scan_relay, a relay that stands in for a driver with three possible timing faults: stamps ahead of the clock, stamps never filled in, and stamps taken when the packet arrived. A fourth node, a consumer of the same topic, receives nothing at all.

You will reproduce each fault, measure it, and repair it by the correct means. Measurement comes first: you need numbers, not guesses.

Expected behaviour. The stamps are close to the simulation clock (median delay between about 0 and 60 ms, never negative by more than 30 ms), never zero, never backwards; SLAM Toolbox processes every scan; the second consumer receives data.

Observable failure. Log lines about dropped messages or extrapolation (stamps in the future), no map at all (zero stamps), or a map whose walls double during turns (late stamps); a consumer node reports that it has received no scans although ros2 topic hz shows 10 Hz.

This is a real ROS 2 project, not a browser lab. You run it on your own machine with ROS 2, a simulator and the tools of Module 7. The website does not run ROS 2 and does not grade it; you grade yourself with the criteria below and the check.sh script.

Time: about 90 minutes · Level: Advanced · Theory: M7-T3 Sensor Topics and Message Timing

Prerequisites and setup

You need a computer with ROS 2 Humble on Ubuntu 22.04 (a virtual machine or container with a display is fine). Jazzy works for the SLAM and Nav2 parts; the simulator launch differs, and the project notes say where. Install once:

sudo apt update
sudo apt install ros-humble-turtlebot3-gazebo ros-humble-turtlebot3-teleop ros-humble-slam-toolbox \
  ros-humble-navigation2 ros-humble-nav2-bringup ros-humble-tf2-tools ros-humble-tf2-ros ros-humble-rviz2
source /opt/ros/humble/setup.bash
export TURTLEBOT3_MODEL=burger

Get the project files: ros2_labs/l3-diagnose-sensor-timestamp-problems in the site’s repository (clone it, or copy the files listed below from this page). The folder ros2_labs/common holds scripts shared by several labs.

1. Terminal 1: export TURTLEBOT3_MODEL=burger, then ros2 launch launch/lab3.launch.py fault:=offset. Later runs use fault:=zero and fault:=late.
2. Terminal 2: ros2 run turtlebot3_teleop teleop_keyboard; drive and turn.
3. Terminal 3: measurement. python3 ../common/stamp_probe.py --topic /scan_relay --n 60 --print-stamps from the project folder.

Verification status. The project files follow the documented Humble interfaces but have not been executed by the site’s automated tests (they cannot run ROS 2). If a command or parameter is rejected by your version, the error names it: treat that as part of the exercise and tell us.

The project files

You change launch arguments, not code. The relay is in common/scan_relay.py (its parameters are described in the docstring). The consumer that receives nothing is qos_probe.py.

launch/lab3.launch.py

"""Lab M7-L3: the lidar stream goes through a relay that damages its timestamps. SLAM Toolbox reads /scan_relay.

Run one fault at a time:   ros2 launch launch/lab3.launch.py fault:=offset     (or zero, late)
Fix it by changing ONLY the relay parameters passed below (stamp_mode, stamp_correction, delay) from the launch command line, e.g.
    ros2 launch launch/lab3.launch.py fault:=offset stamp_correction:=-0.1
"""
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

FAULTS = {
    'offset': {'stamp_mode': 'keep', 'stamp_offset': 0.4, 'delay': 0.0},   # stamps are a constant amount ahead of the simulation clock
    'zero': {'stamp_mode': 'zero', 'stamp_offset': 0.0, 'delay': 0.0},     # the header was never filled in
    'late': {'stamp_mode': 'now', 'stamp_offset': 0.0, 'delay': 0.15},     # stamped on arrival after a transport delay
}


def build(context):
    here = os.path.dirname(os.path.realpath(__file__))
    common = os.path.join(here, '..', '..', 'common')
    fault = dict(FAULTS[LaunchConfiguration('fault').perform(context)])
    # the learner's overrides on the command line win over the fault defaults
    fault['stamp_mode'] = LaunchConfiguration('stamp_mode').perform(context) or fault['stamp_mode']
    fault['delay'] = float(LaunchConfiguration('delay').perform(context) or fault['delay'])
    fault['stamp_correction'] = float(LaunchConfiguration('stamp_correction').perform(context))
    params = os.path.join(here, '..', 'config', 'slam_params.yaml')
    return [
        Node(executable='python3', arguments=[os.path.join(common, 'scan_relay.py')], name='scan_relay', output='screen',
             parameters=[dict(fault, use_sim_time=True)]),
        Node(package='slam_toolbox', executable='async_slam_toolbox_node', name='slam_toolbox', output='screen', parameters=[params]),
        Node(executable='python3', arguments=[os.path.join(here, '..', 'qos_probe.py')], name='qos_probe', output='screen',
             parameters=[{'use_sim_time': True, 'reliability': LaunchConfiguration('probe_reliability').perform(context)}]),
    ]


def generate_launch_description():
    tb3 = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch', 'turtlebot3_world.launch.py')
    return LaunchDescription([
        DeclareLaunchArgument('fault', default_value='offset'),
        DeclareLaunchArgument('stamp_mode', default_value=''),
        DeclareLaunchArgument('delay', default_value=''),
        DeclareLaunchArgument('stamp_correction', default_value='0.0'),
        DeclareLaunchArgument('probe_reliability', default_value='reliable'),
        IncludeLaunchDescription(PythonLaunchDescriptionSource(tb3)),
        OpaqueFunction(function=build),
    ])

qos_probe.py

#!/usr/bin/env python3
"""A consumer of /scan_relay with a configurable QoS reliability (default 'reliable'). It reports whether messages arrive."""
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from sensor_msgs.msg import LaserScan


class Probe(Node):
    def __init__(self):
        super().__init__('qos_probe')
        self.declare_parameter('reliability', 'reliable')
        rel = ReliabilityPolicy.RELIABLE if self.get_parameter('reliability').value == 'reliable' else ReliabilityPolicy.BEST_EFFORT
        self.n = 0
        self.create_subscription(LaserScan, 'scan_relay', self.cb, QoSProfile(depth=5, reliability=rel, history=HistoryPolicy.KEEP_LAST))
        self.create_timer(2.0, self.report)

    def cb(self, m):
        self.n += 1

    def report(self):
        self.get_logger().info(f'qos_probe received {self.n} scans so far' if self.n else 'qos_probe has received NO scans')


def main():
    rclpy.init()
    rclpy.spin(Probe())


if __name__ == '__main__':
    main()

../common/scan_relay.py

#!/usr/bin/env python3
"""Republishes /scan as /scan_relay with deliberate, configurable defects (frame id, stamp offset, zero stamps, late stamping).

Parameters
  frame_id          '' keeps the original frame; otherwise overwrites header.frame_id
  stamp_mode        'keep' | 'zero' | 'now'    ('now' stamps with the arrival time, like a driver that only knows when a packet arrived)
  stamp_offset      seconds added to the stamp (a constant clock error)
  stamp_correction  seconds added after the offset: the learner's compensation
  delay             seconds the message is held before it is published (transport latency)
"""
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from rclpy.time import Time
from rclpy.duration import Duration
from sensor_msgs.msg import LaserScan


class ScanRelay(Node):
    def __init__(self):
        super().__init__('scan_relay')
        self.declare_parameter('frame_id', '')
        self.declare_parameter('stamp_mode', 'keep')
        self.declare_parameter('stamp_offset', 0.0)
        self.declare_parameter('stamp_correction', 0.0)
        self.declare_parameter('delay', 0.0)
        self.pub = self.create_publisher(LaserScan, 'scan_relay', qos_profile_sensor_data)
        self.sub = self.create_subscription(LaserScan, 'scan', self.on_scan, qos_profile_sensor_data)

    def emit(self, msg):
        mode = self.get_parameter('stamp_mode').value
        if mode == 'zero':
            msg.header.stamp = Time().to_msg()
        elif mode == 'now':
            msg.header.stamp = self.get_clock().now().to_msg()
        total = self.get_parameter('stamp_offset').value + self.get_parameter('stamp_correction').value
        if total != 0.0 and mode != 'zero':
            msg.header.stamp = (Time.from_msg(msg.header.stamp) + Duration(seconds=total)).to_msg()
        frame = self.get_parameter('frame_id').value
        if frame:
            msg.header.frame_id = frame
        self.pub.publish(msg)

    def on_scan(self, msg):
        delay = self.get_parameter('delay').value
        if delay > 0.0:
            holder = {}

            def fire():
                holder['t'].cancel()
                self.destroy_timer(holder['t'])
                self.emit(msg)

            holder['t'] = self.create_timer(delay, fire)
        else:
            self.emit(msg)


def main():
    rclpy.init()
    rclpy.spin(ScanRelay())


if __name__ == '__main__':
    main()

../common/stamp_probe.py

#!/usr/bin/env python3
"""Measures the header stamps of a topic against the node clock (simulated time by default).

Usage: python3 stamp_probe.py --topic /scan_relay --n 60 [--print-stamps] [--wall-time]
Prints the median delay (now - stamp), the rate, the number of zero / backwards stamps, and, with --print-stamps,
one stamp per line that you can paste into the timestamp analyser on the lab page.
"""
import argparse
import statistics
import rclpy
from rclpy.node import Node
from rclpy.parameter import Parameter
from rclpy.qos import qos_profile_sensor_data
from rosidl_runtime_py.utilities import get_message


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument('--topic', required=True)
    ap.add_argument('--type', default='sensor_msgs/msg/LaserScan')
    ap.add_argument('--n', type=int, default=60)
    ap.add_argument('--print-stamps', action='store_true')
    ap.add_argument('--wall-time', action='store_true', help='use the wall clock instead of /clock')
    a = ap.parse_args()
    rclpy.init()
    node = Node('stamp_probe', parameter_overrides=[Parameter('use_sim_time', Parameter.Type.BOOL, not a.wall_time)])
    got = []

    def cb(m):
        now = node.get_clock().now().nanoseconds * 1e-9
        s = m.header.stamp.sec + m.header.stamp.nanosec * 1e-9
        got.append((s, now - s))

    node.create_subscription(get_message(a.type), a.topic, cb, qos_profile_sensor_data)
    while rclpy.ok() and len(got) < a.n:
        rclpy.spin_once(node, timeout_sec=0.5)
    if len(got) < 5:
        print('Fewer than 5 messages received: check the topic name, the QoS and use_sim_time.')
        return 1
    stamps = [g[0] for g in got]
    dts = [b - a_ for a_, b in zip(stamps, stamps[1:])]
    delays = [g[1] for g in got]
    print(f'messages: {len(got)}  median delay (now - stamp): {statistics.median(delays) * 1000:.0f} ms')
    print(f'median interval: {statistics.median(dts) * 1000:.1f} ms  zero stamps: {sum(1 for s in stamps if s == 0.0)}  backwards: {sum(1 for d in dts if d < 0)}')
    if a.print_stamps:
        print('--- stamps (seconds) ---')
        print('\n'.join(f'{s:.6f}' for s in stamps))
    return 0


if __name__ == '__main__':
    raise SystemExit(main())

What to do

Follow the loop of this track: Problem → Experiment → Diagnose → Fix → Evaluate → Understand.

1. Offset. Launch with fault:=offset. Read the SLAM log. Run the stamp probe: what is the median delay (now − stamp)? Paste the printed stamps into the analyser below with the reference clock set to the current simulation time (ros2 topic echo /clock --once). Estimate the offset and fix it with stamp_correction:=…; confirm with the probe.
2. Zero stamps. Relaunch with fault:=zero. What does the analyser report? Fix it with the relay’s stamp_mode.
3. Late stamps. Relaunch with fault:=late. The probe shows a small, positive and jittery delay; the map will smear in turns. Why is this harder to see than a constant offset? Fix it at the source.
4. QoS. With any fault fixed, read the log of qos_probe and run ros2 topic info /scan_relay -v. Explain the mismatch using the compatibility table of M7-T3 and fix it with probe_reliability:=….
5. Evaluate. Run ./check.sh. Then drive a quick turn in place and look at the map in RViz.
6. Understand. For each fault write: the symptom, the measurement that exposed it, the size of the error it would cause at ω = 1 rad/s (M7-T3) and the correct place to fix it on a real robot (in the driver, not in a relay).

Pre-flight tool

Expected output

Typical output when things are right. Exact numbers, timestamps and wording differ between machines and versions.

The stamp probe on a healthy stream

messages: 60  median delay (now - stamp): 8 ms
median interval: 200.0 ms  zero stamps: 0  backwards: 0

A constant offset (fault:=offset)

messages: 60  median delay (now - stamp): -392 ms     <- negative: stamps are in the future

Zero stamps (fault:=zero)

median interval: 0.0 ms  zero stamps: 60  backwards: 0

A QoS mismatch

qos_probe has received NO scans            (with probe_reliability:=reliable)
qos_probe received 120 scans so far      (with probe_reliability:=best_effort)

check.sh when everything is fixed

PASS  stamps are not zero
PASS  stamps never go backwards
PASS  median stamp delay is between -30 ms and +60 ms
PASS  transform map -> base_footprint is available (scans are being used)
...
5 passed, 0 failed

Troubleshooting

Evaluation criteria

You are done when every criterion passes. check.sh automates most of them (run it from the project folder, with the launch file running and ROS 2 sourced). The remaining ones you judge yourself.

cd ros2_labs/l3-diagnose-sensor-timestamp-problems
./check.sh

Worked explanation

Try the lab before opening this.

Why it happens

A transform lookup answers “where was the robot when this was measured?”. If the stamp is wrong, the answer is wrong or does not exist (M7-T3). The measurements that matter are the median delay (offset), the spread (jitter), the zero and backward counts (driver faults) and the QoS of each endpoint. The correct place to repair a stamp is the source, because a correction in a relay only works while the fault stays constant.

What this project does not cover

• The faults are injected and constant. Real clock errors drift and driver latency varies with load.
• The simulated lidar runs at a lower rate than most real ones.
• The stamp probe measures against the node’s own clock; on real hardware with several machines, measure the offsets between machines first.
• The project files have not been run by the site’s tests.

What’s next

Other projects in this module

• M7-L1: Debug a ROS 2 SLAM Pipeline
• M7-L2: Fix a TF Configuration Error
• M7-L3: Diagnose Sensor Timestamp Problems: this project.
• M7-L4: Configure Mapping and Localization
• M7-L5: Debug SLAM-to-Nav2 Integration