rtabmap_ros/rtabmap_demos/test/README.md

Demo playback tests

test_demo_playback.py replays a demo's bag through the demo's own launch file and compares the graph rtabmap builds with a golden one, saved in golden/. It checks that a change anywhere in the pipeline (a node, a launch file, rtabmap itself) still produces the same map.

Scenario	Launch file	Bag
robot_mapping	robot_mapping_demo.launch.py	demo_mapping_bag
stereo_outdoor	stereo_outdoor_demo.launch.py	stereo_outdoorA_bag then stereo_outdoorB_bag
netherdrone_lidar3d	netherdrone_lidar3d_demo.launch.py	netherdrone_ouster_vertige_bag_0 to _5, played one after the other
find_object	find_object_demo.launch.py	demo_find_object_bag (skipped without find_object_2d, which Lyrical and Rolling do not have)

Running

The bags are about 5 GB, so they are not in the repository. Fetch them once, into test/data (each bag is listed with its SHA-256 in data/manifest.txt):

bash test/fetch_test_data.sh

Without them, the scenarios skip themselves. Then, from the workspace:

colcon test --packages-select rtabmap_demos --ctest-args -R test_demo_playback

Each scenario replays its bag in about real time on a desktop, longer on a slower machine. The test's CMake registration gives it its own DDS domain (ROS_DOMAIN_ID=110): when running pytest directly instead, set it yourself, or any other ROS node running on the machine will talk to the pipeline and spoil the run.

Environment variable	Effect
RTABMAP_DEMOS_TEST_DATA	Where the bags are (default test/data).
RTABMAP_DEMOS_TEST_RESULTS	Where each run's database, graph and launch log are kept (default: a temporary directory). In CI, they are uploaded when the test fails.
RTABMAP_DEMOS_UPDATE_GOLDEN=1	Write the run's graph as the new golden one instead of comparing with it.

How a run works

1. The demo's launch file is started through a small generated launch file (demo.launch.py in the results folder), which includes the demo with the scenario's launch_arguments after launch_ros' SetParametersFromFile. Its parameter file (demo.launch.yaml) gives the node named rtabmap its database path and ground truth frames, and every node the scenario's parameters. So the demos need no launch argument for the test, and a node's own value for a parameter, set by its launch file, wins over the file's.
2. The bag is replayed in lockstep with the pipeline (see below), and the golden trajectory is published in TF beside it, as golden_map → golden_base. rtabmap is given these as its ground truth frames, so it computes the error of its map against the golden trajectory itself (the Gt/* statistics of /info).
3. Once the bag is done, the launch file is stopped as Ctrl-C would, so rtabmap closes its database. The graph is exported from the database with rtabmap-export, every node optimized.
4. The run is compared with the golden graph: node count, loop closures and path length within each scenario's tolerances, and rtabmap's translational and rotational RMSE against the golden trajectory under each scenario's limits.

The lockstep player

ros2 bag play publishes at the bag's pace, whatever the nodes do. On a loaded machine, a node still busy with the previous frame drops the next one, and which frames get dropped changes from run to run. bag_lockstep.py instead publishes each sensor message only once the whole pipeline has finished with the previous one, so a slower machine only takes longer.

• Gated topics: the large messages something subscribes to, the ones the pipeline takes time to process: images (raw and compressed), point clouds and laser scans (GATED_TYPES). The other topics, IMU, camera info, odometry, TF, go out in the bag's order without waiting: before the next large message, the pipeline is idle again, so it has processed them. Topics nobody subscribes to are published in the bag's order as well.
• Idle: read from the kernel, not from the nodes. The player reads the state of every thread of every process started by ros2 launch (/proc/<pid>/task/*/stat): a thread with work to do is runnable, also while it waits for a CPU, and a node waiting for its next message has all its threads asleep. A node passing a message on wakes the next one before going back to sleep, so the pipeline only looks idle once its last stage is done. Idle means no runnable thread in 2 checks in a row, 2 ms apart.
• Clock: /clock is set to each gated message's stamp before it is published.
• TF: not gated, and sent ahead of the large messages: those are held back by 0.2 s, so the transforms a node needs for a frame are in its buffer when the frame arrives. The other ungated topics go out at their own stamps too, so 0.2 s ahead of them as well. The golden trajectory goes out the same way, with poses at most 0.25 s apart and 0.5 s ahead of their stamps.
• Synchronous publishing: the test sets RMW_FASTRTPS_PUBLICATION_MODE=SYNCHRONOUS, so that publish() sends the message before returning. Otherwise right after a publish() nothing has woken up yet, and the pipeline looks idle.

A sleeping thread looks idle even when work is still under way: a node waiting for a transform with a timeout, or a message still on its way to a subscriber. A message then goes out early, and waits in its subscriber's queue, so nothing is lost as long as every subscriber keeps what it receives:

• Reliable subscriptions: every QoS parameter of rtabmap's nodes is set to reliable (RELIABLE_QOS in test_demo_playback.py). Their default, the system's, is best effort with Fast DDS (the ROS 2 default), which drops a message arriving while the node is busy: a loaded CI runner lost up to two thirds of the lidar sweeps that way.
• Every frame processed: odometry with always_process_most_recent_frame on drops a frame arriving while it is busy. The scenarios set it off, and deepen the queues that need it (parameters, node_parameters).

RTABMAP_DEMOS_REPLAY=chunked replays without the idle check instead: at the bag's pace, 10 s at a time, waiting after each until rtabmap's /info has caught up (within each scenario's chunk_slack). It is as fast, but within a chunk a node slower than real time builds up a backlog: lockstep, the default, waits for it instead.

Examples

Each diagram follows one frame of a scenario through its pipeline. "Wait until every thread is asleep" is the player's idle check, before each gated message; the other messages go out without it.

robot_mapping: one camera frame

The bag holds a robot's laser scans, its camera's compressed color and depth images and camera info, and TF, which also carries the wheel odometry (odom → base_footprint). rgbd_sync uncompresses the images and pairs them, and rtabmap pairs that with the closest laser scan (approximate sync), takes the odometry from TF and maps, one update per second (Rtabmap/DetectionRate). The three gated topics are /jn0/base_scan, data_throttled_image/compressed and data_throttled_image_depth/compressedDepth; data_throttled_camera_info goes out without waiting. There are about five scans per camera frame:

sequenceDiagram
    autonumber
    participant P as Lockstep player
    participant S as rgbd_sync
    participant M as rtabmap
    participant T as Test

    Note over P: /tf of the next 0.2 s already sent (wheel odometry,<br/>robot frames), and the golden trajectory 0.5 s ahead

    loop about five times per camera frame
        P->>P: wait until every thread is asleep
        P->>M: /clock, then /jn0/base_scan
        Note over M: kept in its synchronizer's queue
    end

    P->>S: data_throttled_camera_info (no wait)
    P->>P: wait until every thread is asleep
    P->>S: data_throttled_image/compressed
    P->>P: wait until every thread is asleep
    P->>S: data_throttled_image_depth/compressedDepth
    S->>M: rgbd_image (color, depth and camera info, uncompressed)
    alt once a second (Rtabmap/DetectionRate)
        M->>M: pair with the closest scan, odometry from odom → base_footprint,<br/>add a node, look for loop closures,<br/>its ground truth from golden_map → golden_base
        M->>T: /info (statistics, Gt/* errors)
    end
    Note over P: everything asleep again:<br/>the next message can go

With output_queue_size: 10 on rgbd_sync, an image it publishes is not replaced before rtabmap has received it.

stereo_outdoor: one stereo frame

Each of the two bags holds the left and right compressed images and camera infos of a stereo camera, and TF. The demo uncompresses the images (image_transport republish), rectifies them (stereo_image_proc), pairs them (stereo_sync), computes visual odometry from them (stereo_odometry) and maps (rtabmap, one update per second, Rtabmap/DetectionRate). The two gated topics are the left and right image_raw_throttle/compressed; their camera_info_throttle go out without waiting. For one stereo frame:

sequenceDiagram
    autonumber
    participant P as Lockstep player
    participant R as republish (left, right)
    participant I as stereo_image_proc
    participant S as stereo_sync
    participant O as stereo_odometry
    participant M as rtabmap
    participant T as Test

    Note over P: /tf of the next 0.2 s already sent,<br/>and the golden trajectory 0.5 s ahead

    P->>S: left and right camera_info (no wait)
    Note over S: waits for the images

    P->>P: wait until every thread is asleep
    P->>R: /clock, then left image_raw_throttle/compressed
    R->>I: left/image_raw
    I->>S: left/image_rect (debayered, rectified)
    Note over S: waits for the right image

    P->>P: wait until every thread is asleep
    P->>R: right image_raw_throttle/compressed
    R->>I: right/image_raw
    I->>S: right/image_rect
    S->>O: rgbd_image (left and right, exact sync)
    S->>M: rgbd_image
    O->>M: /vo (odom), and odom → base_footprint on /tf
    alt once a second (Rtabmap/DetectionRate)
        M->>M: add a node, look for loop closures,<br/>its ground truth from golden_map → golden_base
        M->>T: /info (statistics, Gt/* errors)
    end
    Note over P: everything asleep again:<br/>the next frame can go

The pipeline is idle only once rtabmap is done with the frame, so the next frame is never published while any node is still working on this one. With always_process_most_recent_frame: False, odometry registers each frame in turn rather than skipping to the most recent, and with output_queue_size: 10 on stereo_sync, a frame it publishes is not replaced before odometry and rtabmap have received it.

netherdrone_lidar3d: one lidar sweep

The bag holds a drone's lidar sweeps (Ouster, 10 Hz), an IMU, its camera's compressed images and camera info, and TF (the sensors' mounting, the lidar's rotating mast). The demo includes lidar3d_assemble.launch.py from rtabmap_examples:

• complementary_filter_node filters the IMU into /imu/data_filtered, and imu_to_tf turns its orientation into TF, base_link_stabilized → base_link.
• lidar_deskewing corrects each sweep for the motion during it, from that TF, into /os_cloud_node/points/deskewed.
• icp_odometry registers each deskewed sweep (with the filtered IMU, and the stabilized frame as guess) into /icp_odom, and TF icp_odom → base_link_stabilized.
• point_cloud_assembler accumulates the deskewed sweeps over 4.3 s (half a turn of the mast) into /assembled_cloud.
• rgb_sync uncompresses the camera images into /camera/rgbd_image.
• rtabmap adds a node for each assembled cloud, with the camera image closest to it and a depth made from the lidar, and an intermediate node for each odometry pose in between (intermediate_nodes:=true).

The two gated topics are /os_cloud_node/points and /camera/image_raw/compressed; /imu/data_raw and /camera/camera_info go out without waiting. Between two sweeps, the bag holds about eight IMU messages and three camera frames:

sequenceDiagram
    autonumber
    participant P as Lockstep player
    participant F as complementary_filter
    participant U as imu_to_tf
    participant C as rgb_sync
    participant D as lidar_deskewing
    participant O as icp_odometry
    participant A as point_cloud_assembler
    participant M as rtabmap
    participant T as Test

    Note over P: /tf of the next 0.2 s already sent (mounting, mast),<br/>and the golden trajectory 0.5 s ahead

    loop about eight times per sweep
        P->>F: /imu/data_raw (no wait)
        F->>O: /imu/data_filtered
        F->>U: /imu/data_filtered
        U->>U: TF base_link_stabilized → base_link
    end

    loop about three times per sweep
        P->>C: /camera/camera_info (no wait)
        P->>P: wait until every thread is asleep
        P->>C: /clock, then /camera/image_raw/compressed
        C->>M: /camera/rgbd_image
        Note over M: kept in its synchronizer's queue
    end

    P->>P: wait until every thread is asleep
    P->>D: /clock, then /os_cloud_node/points
    D->>D: TF over the sweep (base_link_stabilized)
    D->>O: /os_cloud_node/points/deskewed
    D->>A: /os_cloud_node/points/deskewed
    O->>A: /icp_odom, and icp_odom → base_link_stabilized on /tf
    O->>M: /icp_odom (kept for an intermediate node)
    alt every 4.3 s, half a turn of the mast
        A->>M: /assembled_cloud
        M->>M: sync with /icp_odom and the closest camera image,<br/>add a node and the intermediate nodes since the last one,<br/>their ground truth from golden_map → golden_base
        M->>T: /info (statistics, Gt/* errors)
    end
    Note over P: everything asleep again:<br/>the next message can go

The idle check before each sweep comes after the IMU messages before it: by then, the filter and imu_to_tf have processed them, and the transforms lidar_deskewing needs are there. Every sweep reaches odometry with reliable subscriptions throughout (a best-effort lidar_deskewing lost up to two thirds of them on CI), and icp_odometry registers each in turn (always_process_most_recent_frame: False, topic_queue_size: 10). The six bags of the flight are played one after the other, about 6 minutes of data.

Golden graphs

golden/ holds each scenario's graph (.g2o) and trajectory (.tum). The golden graphs are made on a developer's machine, and the rtabmap built there with all its optional dependencies: CI builds rtabmap differently, which changes how many frames rtabmap merges into the previous node, hence each scenario's tolerances. To regenerate one after a deliberate change to the pipeline:

RTABMAP_DEMOS_UPDATE_GOLDEN=1 ROS_DOMAIN_ID=110 python3 -m pytest -s test/test_demo_playback.py -k stereo

Then replay it once more without RTABMAP_DEMOS_UPDATE_GOLDEN, to check a run matches it, and commit the two files.