rtabmap_sync tests and doc (#1454)

* rtabmap_sync tests and doc

* Added some diagrams

* cleanup some diagrams

* fixing running tests in parallels

.github/workflows/coverage.yml

@@ -60,7 +60,7 @@ jobs:
       # tested package depends on them (--packages-up-to), just not measured.
       - uses: ros-tooling/[email protected]
         with:
-          package-name: rtabmap_conversions rtabmap_util
+          package-name: rtabmap_conversions rtabmap_util rtabmap_sync
           target-ros2-distro: humble
           # RTAB-Map is installed in the image, not as an apt package, so rosdep
           # cannot resolve the key and must not try.
@@ -115,7 +115,7 @@ jobs:
           # `source` does not exist. GitHub picks sh whenever it cannot find
           # bash in the image's PATH, and says so in the log ("shell: sh -e").
           . /opt/ros/humble/setup.sh
-          PKGS="rtabmap_conversions rtabmap_util"
+          PKGS="rtabmap_conversions rtabmap_util rtabmap_sync"
           # Baseline from the .gcno files. Without it a source file that no test
           # ever loaded is missing from the report altogether rather than
           # counted as 0%, which quietly inflates the result.

.gitignore

@@ -2,7 +2,11 @@
 .settings
 .vscode
 __pycache__
-# rosdoc2 build artifacts
+# rosdoc2 build artifacts. docs_build/ holds a copy of each package manifest, so
-docs_build
+# colcon would otherwise see two packages of every name and fail with "Duplicate
+# package names not supported" -- hence the COLCON_IGNORE, which is committed so
+# nobody has to know that. rosdoc2 leaves an existing marker in place.
+docs_build/*
+!docs_build/COLCON_IGNORE
 cross_reference
 doc_output

README.md

@@ -37,7 +37,7 @@ The stack is split into small packages so a pipeline only pulls in what it uses.
 |---|---|
 | `rtabmap_slam` | The `rtabmap` node itself: appearance-based loop closure detection, graph optimization, memory management and map assembly. |
 | `rtabmap_odom` | Odometry nodes — `rgbd_odometry`, `stereo_odometry` and `icp_odometry`. Any external odometry can be used instead. |
-| `rtabmap_sync` | Synchronizes camera and lidar topics into a single message so they reach the SLAM node together — `rgbd_sync`, `stereo_sync`, `rgbdx_sync`. |
+| [`rtabmap_sync`](rtabmap_sync/README.md) | Synchronizes camera and lidar topics into a single message so they reach the SLAM node together — `rgbd_sync`, `stereo_sync`, `rgbdx_sync`. |
 
 ### Sensor processing
 
@@ -133,6 +133,18 @@ export CYCLONEDDS_URI="<Disc><DefaultMulticastAddress>0.0.0.0</></>"
 * **Papers and videos** — [introlab.github.io/rtabmap](https://introlab.github.io/rtabmap/).
 * **Old tutorials** — the [ROS 1 wiki](http://wiki.ros.org/rtabmap_ros/Tutorials), for anything not covered above; parameters and topic names are unchanged.
 
+## Building the documentation
+
+Each package's API reference is generated with [rosdoc2](https://github.com/ros-infrastructure/rosdoc2) from the Doxygen comments in its public headers, and published to docs.ros.org. rosdoc2 documents one package per invocation, so building the whole stack is a loop over them — run it from the repository root:
+
+```bash
+for pkg in rtabmap_*/; do
+  rosdoc2 build --package-path "$pkg" --output-directory doc_output || break
+done
+```
+
+Each package lands in `doc_output/<package>/index.html`.
+
 # License
 
 BSD-3-Clause, see [LICENSE](LICENSE). RTAB-Map itself may be built with components under other licenses; see the [rtabmap](https://github.com/introlab/rtabmap) repository.

codecov.yml

@@ -52,15 +52,19 @@ component_management:
       name: rtabmap_util
       paths:
         - rtabmap_util/**
+    - component_id: rtabmap_sync
+      name: rtabmap_sync
+      paths:
+        - rtabmap_sync/**
 
 comment:
   layout: "condensed_header, diff, components, files"
   behavior: default
   require_changes: true # stay quiet when coverage doesn't move
 
-# Only rtabmap_conversions and rtabmap_util have tests today, so everything
+# Only rtabmap_conversions, rtabmap_util and rtabmap_sync have tests today, so
-# else would report as 0% and drag the total down to a number that says
+# everything else would report as 0% and drag the total down to a number that
-# nothing. As a package gains tests, drop its line here and add it to
+# says nothing. As a package gains tests, drop its line here and add it to
 # individual_components above.
 ignore:
   - "rtabmap_costmap_plugins/**"
@@ -72,6 +76,5 @@ ignore:
   - "rtabmap_python/**"
   - "rtabmap_rviz_plugins/**"
   - "rtabmap_slam/**"
-  - "rtabmap_sync/**"
   - "rtabmap_viz/**"
   - "**/test/**"

rtabmap_conversions/README.md

@@ -64,20 +64,6 @@ colcon test --packages-select rtabmap_conversions
 colcon test-result --verbose
 ```
 
-## Documentation
-
-API documentation is generated with [rosdoc2](https://github.com/ros-infrastructure/rosdoc2) from the Doxygen comments in the public header, and published to [docs.ros.org](https://docs.ros.org/en/jazzy/p/rtabmap_conversions/). To build it locally:
-
-```bash
-rosdoc2 build --package-path rtabmap_conversions --output-directory doc_output
-```
-
-Besides `doc_output`, rosdoc2 writes `docs_build/` and `cross_reference/` scratch directories into the current directory. `docs_build/` contains a copy of the package manifest, so colcon then sees two packages of the same name and every later build fails with `Duplicate package names not supported`. Mark it once and the problem goes away for good — rosdoc2 leaves an existing marker in place on subsequent runs:
-
-```bash
-touch docs_build/COLCON_IGNORE
-```
-
 ## License
 
 BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license).

rtabmap_sync/CMakeLists.txt

@@ -197,4 +197,44 @@ install(DIRECTORY include/
    FILES_MATCHING PATTERN "*.h"
 )
 
+#############
+## Testing ##
+#############
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(RTABMap REQUIRED)
+
+  # Each node gets its own test binary: a crash or a stuck executor in one node cannot
+  # take the others down, and every binary starts with a clean DDS graph.
+  #
+  # Each binary also gets its own DDS domain. colcon tests packages in parallel and ctest
+  # can run these binaries in parallel, while these suites share topic names -- rgb/image,
+  # rgbd_image, odom -- with rtabmap_util's. On a shared domain they discover each other's
+  # publishers, and assertions then see traffic the test never sent. rtabmap_util numbers
+  # its own from 30; keep the two ranges apart.
+  set(rtabmap_sync_test_domain_id 50)
+  macro(rtabmap_sync_add_node_test test_name)
+    ament_add_gtest(${test_name} test/${test_name}.cpp
+      ENV ROS_DOMAIN_ID=${rtabmap_sync_test_domain_id})
+    math(EXPR rtabmap_sync_test_domain_id "${rtabmap_sync_test_domain_id} + 1")
+    if(TARGET ${test_name})
+      target_include_directories(${test_name} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test)
+      target_link_libraries(${test_name} rtabmap_sync_plugins rtabmap_sync)
+      if("$ENV{ROS_DISTRO}" STRLESS "lyrical")
+        ament_target_dependencies(${test_name} ${AmentLibraries})
+      else()
+        target_link_libraries(${test_name} ${Libraries} ${PublicLibraries})
+      endif()
+    endif()
+  endmacro()
+
+  rtabmap_sync_add_node_test(test_rgbd_sync)
+  rtabmap_sync_add_node_test(test_rgb_sync)
+  rtabmap_sync_add_node_test(test_stereo_sync)
+  rtabmap_sync_add_node_test(test_rgbdx_sync)
+  rtabmap_sync_add_node_test(test_common_data_subscriber)
+  rtabmap_sync_add_node_test(test_common_data_subscriber_sync)
+  rtabmap_sync_add_node_test(test_sync_diagnostic)
+endif()
+
 ament_package(CONFIG_EXTRAS ${CMAKE_CURRENT_BINARY_DIR}/cmake/extra_configs.cmake)

rtabmap_sync/README.md

@@ -0,0 +1,77 @@
+# rtabmap_sync
+
+Synchronization of the sensor topics [RTAB-Map](https://github.com/introlab/rtabmap) consumes.
+
+A SLAM node needs a camera's color image, its depth image and its calibration as one measurement, not as three topics that happen to be arriving. This package does that matching — once, in one place — and offers it in two forms: standalone nodes that pack a camera into a single [`RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html), and a base class that the consuming nodes subscribe through.
+
+Every node is a [composable node](https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Composition.html) as well as a standalone executable. **Compose these into the camera driver's process where you can**: they copy every pixel of every frame, and across a process boundary that copy is a serialization plus a memcpy per image.
+
+## Nodes
+
+One page per node.
+
+| Node | Description |
+|---|---|
+| [rgbd_sync](doc/rgbd_sync.md) | Color + depth + calibration → one `RGBDImage`. |
+| [stereo_sync](doc/stereo_sync.md) | Left + right + two calibrations → one `RGBDImage`. |
+| [rgb_sync](doc/rgb_sync.md) | Color + calibration → one `RGBDImage`, with no depth. |
+| [rgbdx_sync](doc/rgbdx_sync.md) | 2 to 8 `RGBDImage` topics → one `RGBDImages`. |
+
+## Library
+
+The package also installs a C++ library, whose API is documented in the [C++ API reference](https://docs.ros.org/en/jazzy/p/rtabmap_sync/generated/index.html) generated from the headers.
+
+**`CommonDataSubscriber`** is the piece worth knowing about. RTAB-Map can be fed in a dozen shapes — RGB-D, stereo, RGB-only, one or several `RGBDImage`s, a 2D or 3D scan, a whole `SensorData` — each optionally alongside odometry, an `OdomInfo` and user data. Every combination needs its own `message_filters` synchronizer, so a node that wired them by hand would be mostly synchronizer boilerplate. This class owns all of them: it reads the `subscribe_*` parameters, builds the one synchronizer that matches, and calls back with a uniform set of arguments whichever inputs were used.
+
+[`rtabmap_slam`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_slam)'s `rtabmap` node and [`rtabmap_viz`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_viz) both derive from it, which is why they take identical input topics and parameters. If you are looking for where `subscribe_depth` or `rgbd_cameras` is implemented, it is here rather than in those packages.
+
+**`SyncDiagnostic`** is what every node here reports through. It watches two rates — messages going into a synchronizer, and messages coming out — because a node can be receiving everything it asked for and still publish nothing. One camera lagging is enough to stop a synchronizer emitting, and only the pair of rates tells that apart from a camera that went silent.
+
+## Conventions
+
+A few things recur across these nodes, and across the `subscribe_*` interface of `rtabmap` and `rtabmap_viz`.
+
+**`approx_sync`.** Inputs are either matched by nearest stamp or required to carry identical ones. **Prefer the exact policy wherever the sensor allows it**: it is cheaper and cannot mismatch. Its failure mode is unforgiving, though — stamps a nanosecond apart mean **nothing is ever published and nothing says why**, which is the most common reason a pipeline built on this package is silent.
+
+Which one is the default follows the sensor. `stereo_sync` and `rgb_sync` default to exact, because a stereo pair is hardware-triggered and a camera publisher sends the image and its `camera_info` together. `rgbd_sync` and `rgbdx_sync` default to approximate, for backward compatibility with the many RGB-D cameras that do not stamp color and depth identically.
+
+**`approx_sync_max_interval`.** Approximate matching pairs *whatever it has* if that is the best available, so a camera that stalls and resumes produces one pairing of a fresh frame with a stale one, silently. This rejects a set spanning more than a given number of seconds. It defaults to `0` (disabled), and it is worth setting — roughly a tenth of the frame period.
+
+**`qos`.** An integer selecting the reliability of the subscriptions: `0` system default, `1` reliable, `2` best effort. It has to be compatible with the publisher or **no messages arrive at all**, with nothing said. Sensor drivers commonly publish images best effort and `camera_info` reliable, which is why `qos_camera_info` can be set apart from `qos`.
+
+**`topic_queue_size` and `sync_queue_size`.** The first is the depth of each individual subscription, the second the depth of the synchronizer's own buffer. Raise `sync_queue_size` when inputs arrive at different rates or with different delays; raise `topic_queue_size` when one input arrives in bursts. The older `queue_size` parameter is deprecated and copied into `sync_queue_size`.
+
+**Compressed output.** `rgbd_sync`, `stereo_sync` and `rgb_sync` each publish a second topic carrying the same frame with compressed images, for sending over a slow link. Color is JPEG; depth is PNG, because JPEG artifacts in a depth image are not blur, they are invented geometry. Neither output is produced unless it has a subscriber, and `compressed_rate` caps the compressed one without touching the raw one.
+
+## Build options
+
+Two synchronizer families are behind CMake options, off by default, because each multiplies the number of templates the package instantiates — and so its build time and its binary size.
+
+| Option | Default | Effect |
+|---|---|---|
+| `RTABMAP_SYNC_MULTI_RGBD` | `OFF` | Lets a `CommonDataSubscriber` consumer synchronize 2 to 6 `RGBDImage` topics itself (`rgbd_cameras` > 1). |
+| `RTABMAP_SYNC_USER_DATA` | `OFF` | Lets `subscribe_user_data` add a `UserData` topic to any of the combinations. |
+
+```bash
+colcon build --packages-select rtabmap_sync --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON
+```
+
+For several cameras, **prefer turning `RTABMAP_SYNC_MULTI_RGBD` on**. The consumer then subscribes to each camera's `RGBDImage` directly and synchronizes them itself, which is one node and one full-frame copy per camera per frame less than routing everything through [rgbdx_sync](doc/rgbdx_sync.md) on the way in.
+
+`rgbdx_sync` is the route that needs no rebuild — against binary packages, say — and the only one that goes past 6 cameras. See [Feeding it to rtabmap](doc/rgbdx_sync.md#feeding-it-to-rtabmap).
+
+Without the options, asking for either is refused rather than ignored quietly — `subscribe_user_data` is reset to false with an error, and `rgbd_cameras` > 1 leaves nothing subscribed and says so.
+
+## Building and testing
+
+```bash
+colcon build --packages-select rtabmap_sync
+colcon test --packages-select rtabmap_sync
+colcon test-result --verbose
+```
+
+The tests drive each node over real ROS topics inside the gtest binary — no launch files and no separate processes — so they also serve as worked examples of each node's topics and parameters.
+
+## License
+
+BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license).

rtabmap_sync/doc/rgb_sync.md

@@ -0,0 +1,90 @@
+# rgb_sync
+
+Groups a camera's color image and calibration into a single [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html), with no depth.
+
+The monocular counterpart of [rgbd_sync](rgbd_sync.md). It exists for pipelines that have no depth to offer: a single camera doing appearance-based loop closure detection and relocalization against a map built earlier, where images are used to recognize places rather than to reconstruct them.
+
+Without depth, RTAB-Map cannot build a metric map from these frames alone. It can still detect that a place has been seen before, which is enough for relocalization in an existing map and for adding loop closure constraints to a graph whose geometry comes from odometry or a lidar.
+
+The camera cannot supply a pose here — visual odometry needs depth or a stereo baseline — so the pose has to come from somewhere else:
+
+```mermaid
+flowchart LR
+    CAM["camera driver"]
+    SYNC["rgb_sync"]
+    ODOM["odometry source<br>wheel, lidar or external"]
+    MAP["rtabmap"]
+    CAM -->|rgb/image| SYNC
+    CAM -->|rgb/camera_info| SYNC
+    SYNC -->|rgbd_image| MAP
+    ODOM -->|odometry| MAP
+```
+
+## Usage
+
+```bash
+ros2 run rtabmap_sync rgb_sync --ros-args \
+  -r rgb/image:=/camera/image_raw \
+  -r rgb/camera_info:=/camera/camera_info
+```
+
+```python
+ComposableNode(
+    package='rtabmap_sync',
+    plugin='rtabmap_sync::RGBSync',
+    name='rgb_sync',
+    remappings=[('rgb/image', '/camera/image_raw'),
+                ('rgb/camera_info', '/camera/camera_info')])
+```
+
+## Subscribed Topics
+
+| Topic | Type | Description |
+|---|---|---|
+| `rgb/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Color image. Goes through `image_transport`. |
+| `rgb/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Calibration of the camera. |
+
+## Published Topics
+
+| Topic | Type | Description |
+|---|---|---|
+| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The image and its calibration. The depth slot is left empty unless `fill_empty_depth`. Published only when someone is subscribed. |
+| `rgbd_image/compressed` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The same frame with the image as JPEG. Published only when someone is subscribed. |
+
+The output's `header.frame_id` comes from the camera_info; its `header.stamp` is the image's.
+
+## Parameters
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `approx_sync` | `bool` | `false` | Match the image and its calibration by nearest stamp. Defaults to **exact**; see [Synchronization](#synchronization). |
+| `approx_sync_max_interval` | `double` | `0.0` | Reject pairs spanning more than this many seconds. `0` disables. |
+| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. |
+| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. |
+| `queue_size` | `int` | — | **Deprecated**, renamed to `sync_queue_size`. |
+| `qos` | `int` | `0` | Reliability of the subscription and the publishers: `0` system default, `1` reliable, `2` best effort. |
+| `qos_camera_info` | `int` | value of `qos` | Reliability of the `rgb/camera_info` subscription alone. |
+| `fill_empty_depth` | `bool` | `false` | Add an all-zero depth image the size of the color one. See below. |
+| `compressed_rate` | `double` | `0.0` | Maximum rate, in Hz, of `rgbd_image/compressed`. `0` means every frame. |
+| `image_transport` | `string` | `"raw"` | Transport for `rgb/image`, e.g. `compressed`. |
+
+## fill_empty_depth
+
+By default the output carries no depth image and no depth calibration, which is how a consumer tells "this camera has no depth" from "this frame's depth happens to be all zeros".
+
+Some consumers refuse a message without one. `fill_empty_depth` gives them a depth image of the right size and encoding (`16UC1`) filled with zeros, registered to the color camera and sharing its calibration. Zero in a depth image means *no reading*, so the frame still carries no geometry — the flag changes the shape of the message, not its content. Leave it off unless something downstream requires it.
+
+## Synchronization
+
+`approx_sync` defaults to **false** here. A driver built on `image_transport`'s camera publisher sends the image and its `camera_info` as a pair carrying the same stamp, so there is nothing to approximate: the exact policy is cheaper and cannot mismatch.
+
+Set `approx_sync:=true` when the two do not share a stamp — a `camera_info` republished on its own timer, or read from a YAML file and stamped with the current time. That is the case to watch for if the node is silent: the calibration values are constant and look fine, but their stamps never match an image.
+
+```bash
+ros2 topic echo --once /camera/image_raw --field header.stamp
+ros2 topic echo --once /camera/camera_info --field header.stamp
+```
+
+## Diagnostics
+
+The node publishes to `/diagnostics` — input rate, output rate, and a warning in the log every 5 seconds while nothing is arriving.

rtabmap_sync/doc/rgbd_sync.md

@@ -0,0 +1,164 @@
+# rgbd_sync
+
+Groups an RGB-D camera's color image, depth image and calibration into a single [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html).
+
+A camera driver publishes three topics that only mean anything together. Keeping them together as one message is worth doing for its own sake — one topic to remap, one topic to record, and no chance of a bag holding a depth frame whose color frame was dropped — but the reason this node exists is that the synchronization has to happen *somewhere*, and doing it once here is cheaper than doing it again in every consumer.
+
+Doing it once also keeps the consumers *consistent*. A pipeline usually runs [`rgbd_odometry`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_odom) and `rtabmap` — often `rtabmap_viz` too — over the same camera. Given the three raw topics, each of those nodes synchronizes them independently, and with approximate matching they can settle on different pairings. `rtabmap` then maps a color/depth pair that odometry never saw, at a pose computed from a different one.
+
+**Without `rgbd_sync`** — each consumer matches the three topics for itself, with its own synchronizer:
+
+```mermaid
+flowchart LR
+    CAM["camera driver"]
+    ODOM["rgbd_odometry<br>sync A"]
+    MAP["rtabmap<br>sync B"]
+    CAM -->|rgb/image| ODOM & MAP
+    CAM -->|depth/image| ODOM & MAP
+    CAM -->|rgb/camera_info| ODOM & MAP
+    ODOM -->|odometry| MAP
+```
+
+**With `rgbd_sync`** — matched once, then fanned out:
+
+```mermaid
+flowchart LR
+    CAM["camera driver"]
+    SYNC["rgbd_sync"]
+    ODOM["rgbd_odometry"]
+    ODOMT(["odometry"])
+    MAP["rtabmap"]
+    VIZ["rtabmap_viz"]
+    CAM -->|rgb/image| SYNC
+    CAM -->|depth/image| SYNC
+    CAM -->|rgb/camera_info| SYNC
+    SYNC -->|rgbd_image| ODOM & MAP & VIZ
+    ODOM --> ODOMT
+    ODOMT --> MAP & VIZ
+```
+
+The same holds when the pose comes from elsewhere — a wheel encoder, a lidar, or an external VIO. The camera then feeds only the mapping side, but every node on it still sees the identical frame:
+
+```mermaid
+flowchart LR
+    CAM["camera driver"]
+    SYNC["rgbd_sync"]
+    ODOM["odometry source<br>wheel, lidar or external"]
+    ODOMT(["odometry"])
+    MAP["rtabmap"]
+    VIZ["rtabmap_viz"]
+    CAM -->|rgb/image| SYNC
+    CAM -->|depth/image| SYNC
+    CAM -->|rgb/camera_info| SYNC
+    SYNC -->|rgbd_image| MAP & VIZ
+    ODOM --> ODOMT
+    ODOMT --> MAP & VIZ
+```
+
+Subscribe them all to one `RGBDImage` and the question does not arise: every node processes the identical message.
+
+It also gives a pipeline one place to synchronize. A consumer that has to match a camera against something on a different rate — a lidar, an IMU, odometry — matches one `RGBDImage` against them rather than three topics plus the others all at once. Synchronizing a large set in one go is the harder problem: the policy has to find a window that satisfies every input, and the more inputs with different rates and delays, the more often it settles for a poor match or none at all. Resolving the camera first, where the three topics are tightly correlated, leaves the downstream synchronizer a much easier job.
+
+It can also decimate the images, rescale depth into the unit RTAB-Map expects, and publish a compressed copy for a slow link. See [Compressing for a slow link](#compressing-for-a-slow-link).
+
+For a monocular camera use [rgb_sync](rgb_sync.md); for a stereo pair, [stereo_sync](stereo_sync.md); for several RGB-D cameras, one of these per camera feeding [rgbdx_sync](rgbdx_sync.md).
+
+## Usage
+
+```bash
+ros2 run rtabmap_sync rgbd_sync --ros-args \
+  -r rgb/image:=/camera/color/image_raw \
+  -r depth/image:=/camera/depth/image_rect_raw \
+  -r rgb/camera_info:=/camera/color/camera_info \
+  -p approx_sync:=true
+```
+
+```python
+ComposableNode(
+    package='rtabmap_sync',
+    plugin='rtabmap_sync::RGBDSync',
+    name='rgbd_sync',
+    parameters=[{'approx_sync': True}],
+    remappings=[('rgb/image', '/camera/color/image_raw'),
+                ('depth/image', '/camera/depth/image_rect_raw'),
+                ('rgb/camera_info', '/camera/color/camera_info')])
+```
+
+**Compose it into the driver's process.** This node copies every pixel of every frame; across a process boundary that copy is a serialization and a memcpy per image, which on a 720p RGB-D stream is real CPU. In the same process with an intra-process-capable driver it is a pointer.
+
+## Subscribed Topics
+
+| Topic | Type | Description |
+|---|---|---|
+| `rgb/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Color image. Goes through `image_transport`, so `rgb/image/compressed` is used instead when `image_transport` is set. |
+| `depth/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Depth image, registered to the color camera. `16UC1` in millimeters or `32FC1` in meters. Goes through `image_transport` under `depth_transport`. |
+| `rgb/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Calibration of the color camera. Copied into both calibration slots of the output — depth is assumed registered. |
+
+## Published Topics
+
+| Topic | Type | Description |
+|---|---|---|
+| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The three inputs, raw. Published only when someone is subscribed. |
+| `rgbd_image/compressed` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The same frame with JPEG color and PNG depth instead of raw images. Published only when someone is subscribed. |
+
+The output's `header.frame_id` is taken from the **camera_info**, which is the frame the calibration is expressed in — so make sure the images and the `camera_info` carry the same `frame_id`. If they disagree, the output is labelled with the calibration's frame while the pixels were measured in another, and every point projected out of them lands somewhere else.
+
+The output's `header.stamp` is the **later** of the color and depth stamps, so the message is never stamped before data it contains.
+
+## Parameters
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `approx_sync` | `bool` | `true` | Match the inputs by nearest stamp. **Set `false` if your camera allows it** — see [Synchronization](#synchronization). |
+| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. Worth setting; see [Synchronization](#synchronization). |
+| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. |
+| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. |
+| `queue_size` | `int` | — | **Deprecated**, renamed to `sync_queue_size`. Still copied to it, with a warning. |
+| `qos` | `int` | `0` | Reliability of the subscriptions and the publishers: `0` system default, `1` reliable, `2` best effort. |
+| `qos_camera_info` | `int` | value of `qos` | Reliability of the `rgb/camera_info` subscription alone. Drivers often publish images best effort and `camera_info` reliable. |
+| `depth_scale` | `double` | `1.0` | Multiplies every depth pixel. See [Depth units](#depth-units). |
+| `decimation` | `int` | `1` | Downsample both images by this factor, scaling the calibration to match. Must divide the depth image size exactly, or it is ignored. |
+| `compressed_rate` | `double` | `0.0` | Maximum rate, in Hz, of `rgbd_image/compressed`. `0` means every frame. Does not affect `rgbd_image`. |
+| `image_transport` | `string` | `"raw"` | Transport for `rgb/image`, e.g. `compressed`. |
+| `depth_transport` | `string` | `"raw"` | Transport for `depth/image`, e.g. `compressedDepth`. |
+| `rgb_image_transport` | `string` | — | **Deprecated**, renamed to `image_transport`. |
+| `depth_image_transport` | `string` | — | **Deprecated**, renamed to `depth_transport`. |
+
+## Synchronization
+
+**Use `approx_sync:=false` when your camera allows it.** The exact policy is cheaper and cannot mismatch a color frame with the wrong depth frame. The catch is that it is all-or-nothing: if the stamps differ by even a nanosecond, **nothing is ever published**, with no error. That is the single most common reason a pipeline built on this node is silent, so check before switching:
+
+```bash
+ros2 topic echo --once /camera/color/image_raw --field header.stamp
+ros2 topic echo --once /camera/depth/image_rect_raw --field header.stamp
+```
+
+The default is nevertheless approximate, for backward compatibility: many RGB-D cameras do not stamp color and depth identically, the two sensors being read at slightly different instants. A stereo pair is normally hardware-triggered instead, which is why [stereo_sync](stereo_sync.md) defaults the other way.
+
+When you do stay on approximate matching, note that it pairs *whatever it has* if that is the best available. A camera that stalls for a second and resumes produces one pairing of a fresh frame with a second-old one, and nothing says so. `approx_sync_max_interval` is the guard: a set spanning more than that many seconds is dropped instead. **Set it.** A tenth of the frame period is a reasonable starting point — `0.003` for a 30 Hz camera.
+
+Leaving it at `0` is also what enables the warning about a large stamp difference in the log; setting it suppresses that warning.
+
+## Depth units
+
+RTAB-Map reads `16UC1` depth as millimeters and `32FC1` as meters. A driver that publishes `16UC1` in some other unit — centimeters, or a raw disparity count — produces a map at the wrong scale, and nothing about it looks broken until you measure something.
+
+`depth_scale` multiplies every depth pixel on the way through, so a camera publishing centimeters is fixed with `depth_scale:=10.0`. It is applied after decimation and before compression, so both outputs carry the corrected values.
+
+## Compressing for a slow link
+
+`rgbd_image/compressed` carries the same frame with the color image as **JPEG** and the depth image as **PNG**. Depth stays lossless deliberately: JPEG artifacts in a depth image are not blur, they are invented geometry.
+
+Neither output is produced unless it has a subscriber, so the compression costs nothing until something subscribes.
+
+`compressed_rate` caps the compressed topic's rate without touching the raw one — for a robot that maps locally at full rate while sending a few frames a second to an operator.
+
+## Decimation
+
+`decimation` halves (or thirds, …) both images and scales the calibration with them, which is the part that is easy to get wrong by hand: an image downsampled without its focal length being scaled produces a point cloud with the wrong field of view.
+
+The factor must divide the **depth** image size exactly. If it does not, the node logs a warning and stops decimating rather than resampling depth in a way that would misalign it against color. A value below 1 is treated as 1.
+
+## Diagnostics
+
+The node publishes to `/diagnostics`: the rate of the incoming color frames, the rate of the published messages, and a warning in the log every 5 seconds while nothing is arriving at all. If the input rate is healthy and the output rate is not, the inputs are arriving but not pairing — look at `approx_sync` and the stamps first.

rtabmap_sync/doc/rgbdx_sync.md

@@ -0,0 +1,145 @@
+# rgbdx_sync
+
+Groups the [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) topics of 2 to 8 cameras into a single [`rtabmap_msgs/msg/RGBDImages`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImages.html).
+
+For a robot carrying several RGB-D cameras. Each camera gets its own [rgbd_sync](rgbd_sync.md) (or [stereo_sync](stereo_sync.md)), and this node synchronizes those outputs into one message so that the SLAM node sees all of them as one measurement.
+
+**Consider the alternative first.** Rebuilt with `RTABMAP_SYNC_MULTI_RGBD=ON`, the SLAM node subscribes to each camera's `RGBDImage` and synchronizes them itself, with no node in between — one process and one full-frame copy per camera per frame less than passing through here. This node exists for the cases that rule that out: running against binary packages, or more than the 6 cameras the build option supports. See [Feeding it to rtabmap](#feeding-it-to-rtabmap).
+
+The reason it is a build option at all is that each supported camera count is a separate synchronizer template, and instantiating them all costs build time and binary size.
+
+This node only groups: it never touches the images, the calibrations or the individual stamps.
+
+Each camera is packed by its own [rgbd_sync](rgbd_sync.md) first, and this node groups those into the one message the consumers subscribe to:
+
+```mermaid
+flowchart LR
+    CAM0["camera 0 driver"]
+    CAM1["camera 1 driver"]
+    SYNC0["rgbd_sync"]
+    SYNC1["rgbd_sync"]
+    XSYNC["rgbdx_sync"]
+    ODOM["rgbd_odometry"]
+    ODOMT(["odometry"])
+    MAP["rtabmap"]
+    VIZ["rtabmap_viz"]
+    CAM0 -->|"rgb, depth,<br>camera_info"| SYNC0
+    CAM1 -->|"rgb, depth,<br>camera_info"| SYNC1
+    SYNC0 -->|rgbd_image0| XSYNC
+    SYNC1 -->|rgbd_image1| XSYNC
+    XSYNC -->|rgbd_images| ODOM & MAP & VIZ
+    ODOM --> ODOMT
+    ODOMT --> MAP & VIZ
+```
+
+**With odometry from elsewhere** — a wheel encoder, a lidar, or an external VIO — the cameras feed only the mapping side:
+
+```mermaid
+flowchart LR
+    CAM0["camera 0 driver"]
+    CAM1["camera 1 driver"]
+    SYNC0["rgbd_sync"]
+    SYNC1["rgbd_sync"]
+    XSYNC["rgbdx_sync"]
+    ODOM["odometry source<br>wheel, lidar or external"]
+    ODOMT(["odometry"])
+    MAP["rtabmap"]
+    VIZ["rtabmap_viz"]
+    CAM0 -->|"rgb, depth,<br>camera_info"| SYNC0
+    CAM1 -->|"rgb, depth,<br>camera_info"| SYNC1
+    SYNC0 -->|rgbd_image0| XSYNC
+    SYNC1 -->|rgbd_image1| XSYNC
+    XSYNC -->|rgbd_images| MAP & VIZ
+    ODOM --> ODOMT
+    ODOMT --> MAP & VIZ
+```
+
+## Usage
+
+```bash
+ros2 run rtabmap_sync rgbdx_sync --ros-args \
+  -p rgbd_cameras:=3 \
+  -r rgbd_image0:=/camera_front/rgbd_image \
+  -r rgbd_image1:=/camera_left/rgbd_image \
+  -r rgbd_image2:=/camera_right/rgbd_image
+```
+
+```python
+ComposableNode(
+    package='rtabmap_sync',
+    plugin='rtabmap_sync::RGBDXSync',
+    name='rgbdx_sync',
+    parameters=[{'rgbd_cameras': 3}],
+    remappings=[('rgbd_image0', '/camera_front/rgbd_image'),
+                ('rgbd_image1', '/camera_left/rgbd_image'),
+                ('rgbd_image2', '/camera_right/rgbd_image')])
+```
+
+The topics are numbered from **0**, and only the first `rgbd_cameras` of them are subscribed.
+
+## Subscribed Topics
+
+| Topic | Type | Description |
+|---|---|---|
+| `rgbd_image0` … `rgbd_image7` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | One per camera. Only the first `rgbd_cameras` are subscribed. |
+
+## Published Topics
+
+| Topic | Type | Description |
+|---|---|---|
+| `rgbd_images` | [`rtabmap_msgs/msg/RGBDImages`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImages.html) | The set, in topic order. Stamped and framed with `rgbd_image0`'s header; each camera keeps its own header inside the array. |
+
+Unlike the other nodes in this package, this one publishes whether or not anyone is subscribed — it does no per-frame work worth skipping.
+
+## Parameters
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `rgbd_cameras` | `int` | `2` | How many cameras to group, 2 to 8. Anything outside that range aborts at start-up. |
+| `approx_sync` | `bool` | `true` | Match the cameras by nearest stamp. Set `false` only for hardware-triggered cameras. |
+| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. Worth setting; see below. |
+| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. |
+| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. |
+| `queue_size` | `int` | — | **Deprecated**, renamed to `sync_queue_size`. |
+| `qos` | `int` | `0` | Reliability of the subscriptions and the publisher: `0` system default, `1` reliable, `2` best effort. |
+
+`rgbd_cameras` outside 2–8 is a hard error rather than a clamp: one camera needs no grouping at all, and nine cannot be synchronized by any of the templates the node holds. For one camera, subscribe to its `RGBDImage` topic directly.
+
+## Order matters
+
+The array is published in topic order — `rgbd_image0` first — and consumers index into it. RTAB-Map matches each image against the calibration and the TF frame it saw at that index, so swapping two remappings places a camera's images at another camera's extrinsics, and the map comes out with the world duplicated at an angle.
+
+The order is a naming convention, not something the node can check. Keep the numbering consistent with whatever else refers to those cameras.
+
+## Synchronization
+
+Separate cameras are rarely triggered together, so `approx_sync` defaults to true. Nothing is published until **every** camera has contributed: a partial set would silently drop one camera's field of view from the map, which is worse than a dropped frame.
+
+That also makes one silent camera stop the whole node. If `rgbd_images` goes quiet, check each input in turn:
+
+```bash
+ros2 topic hz /camera_front/rgbd_image
+```
+
+Set `approx_sync_max_interval` here as well. With several free-running cameras the synchronizer has more opportunities to pair a fresh frame with a stale one, and each camera's images are placed in the map using the robot's pose at the *set's* stamp — so a camera whose frame is 200 ms old is placed wherever the robot was not.
+
+## Feeding it to rtabmap
+
+Set `rgbd_cameras` to **0** on the consumer, and remap its `rgbd_images` input to this node's output:
+
+```python
+Node(
+    package='rtabmap_slam', executable='rtabmap',
+    parameters=[{'subscribe_rgbd': True, 'rgbd_cameras': 0}],
+    remappings=[('rgbd_images', '/rgbd_images')])
+```
+
+`rgbd_cameras:=0` is what selects the `RGBDImages` interface: the count then comes from each message rather than from a parameter, so the same consumer handles any number of cameras without a rebuild.
+
+With `RTABMAP_SYNC_MULTI_RGBD=ON` instead, drop this node and point the consumer straight at the cameras — `rgbd_cameras:=3` and one remapping per `rgbd_image0`…`rgbd_image2`. Same topics, same order, one hop fewer.
+
+Either way, every camera needs its extrinsics in TF — a transform from the robot's base frame to each camera's frame, at each frame's stamp.
+
+## Diagnostics
+
+The node publishes to `/diagnostics`: the rate of `rgbd_image0`, the rate of published sets, and a warning in the log every 5 seconds while nothing is arriving. Since a set needs every camera, a healthy input rate with no output points at one of the *other* cameras.

rtabmap_sync/doc/stereo_sync.md

@@ -0,0 +1,135 @@
+# stereo_sync
+
+Groups a stereo pair's four topics — left image, right image and their two calibrations — into a single [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html).
+
+The same idea as [rgbd_sync](rgbd_sync.md), for a stereo camera: four topics that only mean anything together become one message, synchronized once instead of in every consumer.
+
+The default topic names say `image_rect` because rectified images are what a stereo pipeline normally carries, and what RTAB-Map assumes by default — but this node does not require it and does not rectify anything itself. Feeding it unrectified images is fine as long as you tell the consumer: set `Rtabmap/ImagesAlreadyRectified` to `false` on the `stereo_odometry` and `rtabmap` nodes, and they rectify from the calibration themselves. Otherwise run [`stereo_image_proc`](https://docs.ros.org/en/jazzy/p/stereo_image_proc/) upstream.
+
+In a pipeline, the one `RGBDImage` feeds everything downstream — odometry included, so every node works from the same pair:
+
+```mermaid
+flowchart LR
+    CAM["stereo driver"]
+    SYNC["stereo_sync"]
+    ODOM["stereo_odometry"]
+    ODOMT(["odometry"])
+    MAP["rtabmap"]
+    VIZ["rtabmap_viz"]
+    CAM -->|left/image_rect| SYNC
+    CAM -->|right/image_rect| SYNC
+    CAM -->|left/camera_info| SYNC
+    CAM -->|right/camera_info| SYNC
+    SYNC -->|rgbd_image| ODOM & MAP & VIZ
+    ODOM --> ODOMT
+    ODOMT --> MAP & VIZ
+```
+
+**With odometry from elsewhere** — a wheel encoder, a lidar, or an external VIO — the stereo pair feeds only the mapping side:
+
+```mermaid
+flowchart LR
+    CAM["stereo driver"]
+    SYNC["stereo_sync"]
+    ODOM["odometry source<br>wheel, lidar or external"]
+    ODOMT(["odometry"])
+    MAP["rtabmap"]
+    VIZ["rtabmap_viz"]
+    CAM -->|left/image_rect| SYNC
+    CAM -->|right/image_rect| SYNC
+    CAM -->|left/camera_info| SYNC
+    CAM -->|right/camera_info| SYNC
+    SYNC -->|rgbd_image| MAP & VIZ
+    ODOM --> ODOMT
+    ODOMT --> MAP & VIZ
+```
+
+## Usage
+
+```bash
+ros2 run rtabmap_sync stereo_sync --ros-args \
+  -r left/image_rect:=/stereo/left/image_rect \
+  -r right/image_rect:=/stereo/right/image_rect \
+  -r left/camera_info:=/stereo/left/camera_info \
+  -r right/camera_info:=/stereo/right/camera_info
+```
+
+```python
+ComposableNode(
+    package='rtabmap_sync',
+    plugin='rtabmap_sync::StereoSync',
+    name='stereo_sync',
+    remappings=[('left/image_rect', '/stereo/left/image_rect'),
+                ('right/image_rect', '/stereo/right/image_rect'),
+                ('left/camera_info', '/stereo/left/camera_info'),
+                ('right/camera_info', '/stereo/right/camera_info')])
+```
+
+As with `rgbd_sync`, compose it into the driver's process where you can: this node copies both images of every pair.
+
+## Subscribed Topics
+
+| Topic | Type | Description |
+|---|---|---|
+| `left/image_rect` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Rectified left image, mono or color. Goes through `image_transport`. |
+| `right/image_rect` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Rectified right image, same size and encoding. |
+| `left/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Calibration of the left camera. |
+| `right/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Calibration of the right camera. **Its `P[3]` must carry the baseline**; see below. |
+
+## Published Topics
+
+| Topic | Type | Description |
+|---|---|---|
+| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | Left image in the color slot, right image in the depth slot. Published only when someone is subscribed. |
+| `rgbd_image/compressed` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The same pair, both images JPEG. Published only when someone is subscribed. |
+
+The output's `header.frame_id` comes from the **left** camera_info, and its `header.stamp` is the later of the two image stamps.
+
+## Parameters
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `approx_sync` | `bool` | `false` | Match the inputs by nearest stamp. Defaults to **exact** here; see [Synchronization](#synchronization). |
+| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. Only meaningful with `approx_sync`. |
+| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. |
+| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. |
+| `queue_size` | `int` | — | **Deprecated**, renamed to `sync_queue_size`. |
+| `qos` | `int` | `0` | Reliability of the subscriptions and the publishers: `0` system default, `1` reliable, `2` best effort. |
+| `qos_camera_info` | `int` | value of `qos` | Reliability of the two `camera_info` subscriptions alone. |
+| `compressed_rate` | `double` | `0.0` | Maximum rate, in Hz, of `rgbd_image/compressed`. `0` means every frame. |
+| `image_transport` | `string` | `"raw"` | Transport for both images, e.g. `compressed`. |
+
+## How a stereo pair travels in an RGBDImage
+
+There is no separate stereo message: the left image goes where color goes and the right image goes where depth goes. What tells a consumer to read it as a stereo pair rather than as color plus depth is the **baseline** in the second calibration — `P[3]` of the right `camera_info`, which by the ROS convention is `-fx * baseline`.
+
+So a right `camera_info` with `P[3] == 0` describes a camera sitting exactly on top of the left one. Nothing downstream can triangulate from that, and the failure is silent: the pair is forwarded, RTAB-Map reads a zero baseline and produces no depth. If a stereo pipeline comes out with no 3D points at all, check `P[3]` of the right camera first:
+
+```bash
+ros2 topic echo --once /stereo/right/camera_info --field p
+```
+
+## Synchronization
+
+`approx_sync` defaults to **false** here, unlike the other nodes in this package. A stereo pair is normally hardware-triggered, so the two frames carry the same stamp, and the exact policy is both cheaper and impossible to mismatch. Mismatching a stereo pair is worse than mismatching color and depth: the disparity between two frames taken at different instants is a measurement of the camera's own motion, read as scene geometry.
+
+Set `approx_sync:=true` only for two free-running cameras that are not triggered together — and then set `approx_sync_max_interval` alongside it. The node warns whenever a pair's stamps differ by more than 10 ms regardless of the setting, because at that point the pair is unlikely to be worth anything.
+
+If the pipeline is silent with the default, the stamps are not identical. Check with:
+
+```bash
+ros2 topic echo --once /stereo/left/image_rect --field header.stamp
+ros2 topic echo --once /stereo/right/image_rect --field header.stamp
+```
+
+## Compressing for a slow link
+
+`rgbd_image/compressed` carries both images as **JPEG**. Unlike [rgbd_sync](rgbd_sync.md), there is no lossless path: both halves of a stereo pair are ordinary camera images, and neither is depth.
+
+JPEG artifacts do affect stereo matching, so a pipeline that computes odometry from the compressed stream will match slightly fewer features than one on the raw images. For sending frames to an operator, that does not matter; for running odometry at the far end of a link, prefer a higher JPEG quality over a lower frame rate.
+
+`compressed_rate` caps the compressed topic without touching `rgbd_image`.
+
+## Diagnostics
+
+The node publishes to `/diagnostics`: the rate of incoming left frames, the rate of published pairs, and a warning in the log every 5 seconds while nothing is arriving. A healthy input rate with no output means the pairs are not matching — the stamps and `approx_sync` are what to look at.

rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h

@@ -59,34 +59,177 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_sync/CommonDataSubscriberDefines.h>
 #include <rtabmap_sync/SyncDiagnostic.h>
 
+/**
+ * @namespace rtabmap_sync
+ * @brief Synchronization of the sensor topics RTAB-Map consumes.
+ *
+ * Two things live here: the standalone nodes that group a camera's topics into a single
+ * [RGBDImage](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html)
+ * (`rgbd_sync`, `stereo_sync`, `rgb_sync`, `rgbdx_sync`), and CommonDataSubscriber, the
+ * base class through which the consuming nodes subscribe.
+ */
 namespace rtabmap_sync {
 
+/**
+ * @brief Subscribes to whichever set of sensor topics a node was configured for, and
+ *        hands them over synchronized.
+ *
+ * RTAB-Map can be fed in a dozen shapes -- RGB-D, stereo, RGB-only, a pre-packed
+ * `RGBDImage` or several of them, a 2D or 3D scan, a whole `SensorData` -- each
+ * optionally alongside odometry, an `OdomInfo` and user data. That is far too many
+ * combinations for a node to wire by hand, so this class owns all of them: it reads the
+ * `subscribe_*` parameters, builds the one `message_filters` synchronizer that matches,
+ * and calls back with a uniform set of arguments no matter which inputs were used.
+ *
+ * `rtabmap_slam`'s `rtabmap` node and `rtabmap_viz` both derive from it, which is why
+ * they take identical topics and parameters.
+ *
+ * @par Using it
+ * Derive from both rclcpp::Node and this class, and call setupCallbacks() once the
+ * subclass is ready to receive data:
+ * @code
+ * class MyNode : public rclcpp::Node, public rtabmap_sync::CommonDataSubscriber
+ * {
+ * public:
+ *   explicit MyNode(const rclcpp::NodeOptions & options) :
+ *     Node("my_node", options),
+ *     CommonDataSubscriber(*this, false)
+ *   {
+ *     setupCallbacks(*this);
+ *   }
+ * protected:
+ *   void commonMultiCameraCallback(...) override { ... }
+ *   // ... and the three other callbacks
+ * };
+ * @endcode
+ * The constructor declares the parameters, so they are readable from the subclass
+ * constructor before setupCallbacks() is called.
+ *
+ * @par Which callback fires
+ * Exactly one of the four, decided once at setup:
+ * - commonMultiCameraCallback() for anything with a camera in it,
+ * - commonLaserScanCallback() for a scan with no camera,
+ * - commonSensorDataCallback() for `subscribe_sensor_data`,
+ * - commonOdomCallback() when odometry is the only input.
+ *
+ * @par Conflicting parameters
+ * Several `subscribe_*` flags describe the same slot. Rather than refusing to start,
+ * setupCallbacks() drops one of the two and logs which: stereo beats depth and RGB,
+ * `subscribe_rgbd` beats all three, `subscribe_sensor_data` beats everything including
+ * `subscribe_rgbd`, `subscribe_scan` beats `subscribe_scan_cloud`, and
+ * `subscribe_scan_descriptor` beats both. Setting `odom_frame_id` turns off
+ * `subscribe_odom`, since the pose is then read from TF instead.
+ *
+ * @par Build options
+ * Synchronizing several `RGBDImage` topics (`rgbd_cameras` > 1) needs
+ * `RTABMAP_SYNC_MULTI_RGBD`, and `subscribe_user_data` needs `RTABMAP_SYNC_USER_DATA`.
+ * Both are off by default because each multiplies the number of synchronizer templates
+ * the package instantiates. Turning the first on is the better of the two ways to take
+ * several cameras: the node subscribes to them directly, with nothing in between.
+ * Without it, `rgbd_cameras=0` selects the `RGBDImages` interface -- what `rgbdx_sync`
+ * publishes -- which needs no rebuild and has no camera-count limit, at the cost of one
+ * extra node and one full-frame copy per camera.
+ */
 class CommonDataSubscriber {
 public:
+	/**
+	 * @brief Declares the `subscribe_*`, queue and QoS parameters on @p node.
+	 *
+	 * Subscribing itself happens in setupCallbacks(), so that a subclass can read the
+	 * parameters and finish constructing before any message can arrive.
+	 *
+	 * @param node the node the parameters are declared on and the topics subscribed to
+	 * @param gui  true for a visualization node: `subscribe_depth` and `subscribe_rgb`
+	 *             then default to false, leaving odometry as the only default input
+	 */
 	RTABMAP_SYNC_PUBLIC
 	CommonDataSubscriber(rclcpp::Node & node, bool gui);
 	virtual ~CommonDataSubscriber();
 
+	/// True if subscribed to separate color, depth and camera_info topics.
 	bool isSubscribedToDepth() const  {return subscribedToDepth_;}
+	/// True if subscribed to a left/right image pair with their two camera_info topics.
 	bool isSubscribedToStereo() const {return subscribedToStereo_;}
+	/// True if subscribed to color and camera_info with no depth.
 	bool isSubscribedToRGB() const  {return subscribedToRGB_;}
+	/// True if odometry comes from the `odom` topic; false when `odom_frame_id` is set.
 	bool isSubscribedToOdom() const  {return subscribedToOdom_;}
+	/// True if subscribed to `RGBDImage` topics, or to the `RGBDImages` container.
 	bool isSubscribedToRGBD() const   {return subscribedToRGBD_;}
+	/// True if subscribed to a `LaserScan`.
 	bool isSubscribedToScan2d() const {return subscribedToScan2d_;}
+	/// True if subscribed to a `PointCloud2` scan.
 	bool isSubscribedToScan3d() const {return subscribedToScan3d_;}
+	/// True if subscribed to a whole `SensorData`.
 	bool isSubscribedToSensorData() const {return subscribedToSensorData_;}
+	/// True if an `OdomInfo` is synchronized with the data.
 	bool isSubscribedToOdomInfo() const {return subscribedToOdomInfo_;}
+	/// True if any input at all is subscribed. False means no callback can ever fire.
 	bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD() || isSubscribedToScan2d() || isSubscribedToScan3d() || isSubscribedToRGB() || isSubscribedToOdom() || isSubscribedToSensorData();}
+	/**
+	 * @brief Number of `RGBDImage` topics subscribed.
+	 * @return 0 when not subscribed to RGBD at all, and also on the `RGBDImages`
+	 *         interface (`rgbd_cameras=0`), where the count varies per message.
+	 */
 	int rgbdCameras() const {return isSubscribedToRGBD()?(int)rgbdSubs_.size():0;}
+	/// Queue depth of each individual subscription (`topic_queue_size`).
 	int getTopicQueueSize() const {return topicQueueSize_;}
+	/// Queue depth of the synchronizer (`sync_queue_size`).
 	int getSyncQueueSize() const {return syncQueueSize_;}
+	/**
+	 * @brief True if inputs are matched by nearest stamp rather than exact equality.
+	 *
+	 * The default depends on the inputs: false for stereo and for a scan with no camera,
+	 * true otherwise. The `approx_sync` parameter overrides it either way.
+	 */
 	bool isApproxSync() const {return approxSync_;}
+	/// The node name, as captured at construction.
 	const std::string & name() const {return name_;}
 
 protected:
+	/**
+	 * @brief Resolves the parameters into one synchronizer and subscribes.
+	 *
+	 * Call once from the subclass constructor, after the subclass is able to handle a
+	 * callback. This is also where the conflicting-parameter rules are applied and where
+	 * the /diagnostics reporting is set up.
+	 *
+	 * @param node       the node to subscribe on; pass the same one given to the constructor
+	 * @param otherTasks extra diagnostic tasks to publish alongside the input and output
+	 *                   rate, so the node reports its own state in the same message
+	 */
 	void setupCallbacks(
 			rclcpp::Node & node,
 			std::vector<diagnostic_updater::DiagnosticTask*> otherTasks = std::vector<diagnostic_updater::DiagnosticTask*>());
+	/**
+	 * @brief Called with one synchronized frame from one or more cameras.
+	 *
+	 * Fires for every configuration that has a camera in it, whichever way the camera was
+	 * subscribed. The vectors hold one entry per camera and are parallel; unused inputs
+	 * arrive empty or null rather than being signalled separately.
+	 *
+	 * @param odomMsg              the pose, or null when odometry is not subscribed
+	 * @param userDataMsg          user data, or null
+	 * @param imageMsgs            one color image per camera
+	 * @param depthMsgs            one depth image per camera, or the right image in
+	 *                             stereo; empty when there is no depth (RGB-only)
+	 * @param cameraInfoMsgs       calibration of each color camera
+	 * @param depthCameraInfoMsgs  calibration of each depth camera, or of the right
+	 *                             camera in stereo, whose P(0,3) carries the baseline
+	 * @param scanMsg              a 2D scan, or a default-constructed one if none
+	 * @param scan3dMsg            a 3D scan, or a default-constructed one if none
+	 * @param odomInfoMsg          odometry details, or null
+	 * @param globalDescriptorMsgs global descriptors, empty when none were computed
+	 * @param localKeyPoints       per-camera keypoints, in image coordinates; only ever
+	 *                             set by the RGBD inputs, which can carry the features
+	 *                             the odometry already extracted
+	 * @param localPoints3d        per-camera 3D points matching @p localKeyPoints, each
+	 *                             expressed in **its own camera's optical frame** -- not
+	 *                             in the robot's base frame. rtabmap_conversions'
+	 *                             `convertRGBDMsgs()` is what moves them to the base
+	 *                             frame, applying each camera's local transform.
+	 * @param localDescriptors     per-camera feature descriptors, already uncompressed
+	 */
 	virtual void commonMultiCameraCallback(
 				const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
 				const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
@@ -101,6 +244,17 @@ protected:
 				const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> >(),
 				const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_msgs::msg::Point3f> >(),
 				const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>()) = 0;
+	/**
+	 * @brief Called with one synchronized scan, when no camera is subscribed.
+	 *
+	 * @param odomMsg          the pose, or null when odometry is not subscribed
+	 * @param userDataMsg      user data, or null
+	 * @param scanMsg          the 2D scan, default-constructed if the scan is 3D
+	 * @param scan3dMsg        the 3D scan, default-constructed if the scan is 2D
+	 * @param odomInfoMsg      odometry details, or null
+	 * @param globalDescriptor the descriptor from a `ScanDescriptor` input; its `data`
+	 *                         is empty when none was computed
+	 */
 	virtual void commonLaserScanCallback(
 				const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
 				const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
@@ -108,15 +262,42 @@ protected:
 				const sensor_msgs::msg::PointCloud2 & scan3dMsg,
 				const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
 				const rtabmap_msgs::msg::GlobalDescriptor & globalDescriptor = rtabmap_msgs::msg::GlobalDescriptor()) = 0;
+	/**
+	 * @brief Called with odometry alone, when it is the only subscribed input.
+	 * @param odomMsg     the pose
+	 * @param userDataMsg user data, or null
+	 * @param odomInfoMsg odometry details, or null
+	 */
 	virtual void commonOdomCallback(
 				const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
 				const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
 				const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg) = 0;
+	/**
+	 * @brief Called with a whole `SensorData`, for `subscribe_sensor_data`.
+	 *
+	 * A `SensorData` already carries the images, the scan and the calibration of one
+	 * frame, so nothing is unpacked here: it is passed on as it arrived.
+	 *
+	 * @param sensorDataMsg the frame
+	 * @param odomMsg       the pose, or null when odometry is not subscribed
+	 * @param odomInfoMsg   odometry details, or null
+	 */
 	virtual void commonSensorDataCallback(
 				const rtabmap_msgs::msg::SensorData::ConstSharedPtr & sensorDataMsg,
 				const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
 				const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg) = 0;
 
+	/**
+	 * @brief Reports that the subclass produced an output, for /diagnostics.
+	 *
+	 * The input side is ticked automatically as messages arrive; this is the other half,
+	 * and it is what lets "one camera went quiet" be told apart from "the node is
+	 * receiving everything and falling behind". Call it once per published result.
+	 *
+	 * @param stamp           stamp of what was produced
+	 * @param targetFrequency the rate to be judged against, or 0 to inherit the rate
+	 *                        measured on the input side
+	 */
 	void tick(const rclcpp::Time & stamp, double targetFrequency = 0);
 
 private:

rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h

@@ -29,7 +29,40 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_
 
 #include <rtabmap/utilite/UConversion.h>
-#include <rtabmap_sync/GetTopicName.h>
+
+#include <string>
+
+/**
+ * @brief One name for the topic of a subscription, whichever kind it is.
+ *
+ * A `message_filters::Subscriber` exposes `get_topic_name()` through the subscription it
+ * holds, while an `image_transport::SubscriberFilter` exposes `getTopic()`. The SYNC_DECL
+ * macros below log what a node subscribed to and have to handle both, so this picks
+ * whichever the object actually has, resolved at compile time.
+ *
+ * @{
+ */
+template<class T>
+auto getTopicNameImpl(T const& obj, int)
+    -> decltype(obj->get_topic_name(), std::string())
+{
+   return obj->get_topic_name();
+}
+
+template<class T>
+auto getTopicNameImpl(T const& obj, long)
+    -> decltype(obj.getTopic(), std::string())
+{
+  return obj.getTopic();
+}
+
+template<class T>
+auto getTopicName(T const& obj)
+    -> decltype(getTopicNameImpl(obj, 0), std::string())
+{
+  return getTopicNameImpl(obj, 0);
+}
+/** @} */
 
 #define DATA_SYNC2(PREFIX, SYNC_NAME, MSG0, MSG1) \
 	typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1> PREFIX##SYNC_NAME##SyncPolicy; \

rtabmap_sync/include/rtabmap_sync/GetTopicName.h

@@ -1,35 +0,0 @@
-/*
- * GetTopicName.h
- *
- *  Created on: Oct 1, 2021
- *      Author: mathieu
- */
-
-#ifndef INCLUDE_RTABMAP_ROS_GETTOPICNAME_H_
-#define INCLUDE_RTABMAP_ROS_GETTOPICNAME_H_
-
-#include <string>
-
-template<class T>
-auto getTopicNameImpl(T const& obj, int)
-    -> decltype(obj->get_topic_name(), std::string())
-{
-   return obj->get_topic_name();
-}
-
-template<class T>
-auto getTopicNameImpl(T const& obj, long)
-    -> decltype(obj.getTopic(), std::string())
-{
-  return obj.getTopic();
-}
-
-template<class T>
-auto getTopicName(T const& obj)
-    -> decltype(getTopicNameImpl(obj, 0), std::string())
-{
-  return getTopicNameImpl(obj, 0);
-}
-
-
-#endif /* INCLUDE_RTABMAP_ROS_GETTOPICNAME_H_ */

rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h

@@ -14,8 +14,43 @@ using namespace std::chrono_literals;
 
 namespace rtabmap_sync {
 
+/**
+ * @brief Reports the rate going into a synchronizer and the rate coming out of it, on
+ *        /diagnostics.
+ *
+ * Every node in this package, and every node built on CommonDataSubscriber, publishes
+ * through one of these. Two statuses rather than one is the whole point: a node can be
+ * receiving all of its inputs and still publish nothing -- one camera lagging is enough
+ * to stop a synchronizer emitting -- and only the pair tells those cases apart.
+ *
+ * @par Expected rate
+ * With no rate given, the target is learned from the gaps between the message stamps,
+ * averaged over a sliding window, and only ever revised upwards to the fastest rate seen.
+ * A node that deliberately publishes slower than it receives -- a throttled or decimated
+ * output -- passes its own rate to tickOutput() instead, so it is judged against what it
+ * meant to do.
+ *
+ * @par Usage
+ * @code
+ * syncDiagnostic_.reset(new SyncDiagnostic(this));
+ * syncDiagnostic_->init(imageSub_.getTopic(), "Did not receive data since 5 seconds!...");
+ * // then, in the callback:
+ * syncDiagnostic_->tickInput(image->header.stamp);
+ * ...
+ * syncDiagnostic_->tickOutput(image->header.stamp);
+ * @endcode
+ *
+ * @note The node passed in is held as a raw pointer and must outlive this object.
+ */
 class SyncDiagnostic {
     public:
+        /**
+         * @param node       the node to publish /diagnostics from; must outlive this object
+         * @param tolerance  fraction by which the measured rate may differ from the
+         *                   expected one before the status stops being OK
+         * @param windowSize number of stamp intervals averaged when learning the expected
+         *                   rate; must be at least 1
+         */
         SyncDiagnostic(rclcpp::Node * node, double tolerance = 0.2, int windowSize = 5) :
 		node_(node),
 		diagnosticUpdater_(node, 2.0),
@@ -34,6 +69,20 @@ class SyncDiagnostic {
         UASSERT(windowSize_ >= 1);
     }
 
+    /**
+     * @brief Registers the tasks and starts publishing.
+     *
+     * @param topic one of the subscribed topics, used only to name the hardware the
+     *              status belongs to: the last two segments are dropped, so
+     *              `/back_camera/left/image` reports as `back_camera`. Pass an empty
+     *              string when no single topic identifies the device; the hardware id is
+     *              then `none`.
+     * @param topicsNotReceivedWarningMsg logged every 5 seconds while nothing is coming
+     *              in. Worth making specific: it is what a user sees when a pipeline is
+     *              silent, so it should name the topics and the likely causes.
+     * @param otherTasks extra tasks to publish in the same message, so a node's own state
+     *              arrives alongside its rates rather than in a separate update.
+     */
     void init(
         const std::string & topic,
         const std::string & topicsNotReceivedWarningMsg,
@@ -62,6 +111,12 @@ class SyncDiagnostic {
         diagnosticTimer_ = node_->create_wall_timer(5s, std::bind(&SyncDiagnostic::diagnosticTimerCallback, this), nullptr);
     }
 
+    /**
+     * @brief Records that one input message arrived.
+     * @param stamp             the message stamp; it is also checked against the clock,
+     *                          which is how an unsynchronized sender is caught
+     * @param expectedFrequency the rate to judge against, or 0 to learn it from the stamps
+     */
     void tickInput(const rclcpp::Time & stamp, double expectedFrequency = 0.0)
     {
         updateFrequency(
@@ -74,6 +129,13 @@ class SyncDiagnostic {
             lastTickInputStamp_);
     }
 
+    /**
+     * @brief Records that one output message was published.
+     * @param stamp             the stamp of what was published
+     * @param expectedFrequency the rate to judge against, or 0 to inherit the rate
+     *                          measured on the input side -- the right default for a
+     *                          node that publishes one output per input
+     */
     void tickOutput(const rclcpp::Time & stamp, double expectedFrequency = 0.0)
     {
         if(expectedFrequency == 0.0) {

rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp

@@ -44,6 +44,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap_sync
 {
 
+/**
+ * @brief Groups a camera's color and calibration topics into one `RGBDImage`, with no
+ *        depth.
+ *
+ * The RGB-only counterpart of RGBDSync, for a monocular camera feeding an appearance-only
+ * pipeline -- loop closure detection and relocalization without 3D reconstruction. With
+ * `fill_empty_depth` it adds an all-zero depth image for consumers that insist on one.
+ *
+ * See the [node documentation](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_sync/doc/rgb_sync.md)
+ * for topics and parameters.
+ */
 class RGBSync : public rclcpp::Node
 {
 public:
@@ -60,6 +71,9 @@ private:
 	double compressedRate_;
 	bool fillEmptyDepth_;
 
+	/// Stamp of the last compressed message published, for compressed_rate throttling.
+	/// Explicitly on the ROS clock: the default is the system clock, and rclcpp refuses
+	/// to compare two times that do not come from the same source.
 	rclcpp::Time lastCompressedPublished_;
 
 	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;

rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp

@@ -44,6 +44,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap_sync
 {
 
+/**
+ * @brief Groups an RGB-D camera's color, depth and calibration topics into one
+ *        `RGBDImage`.
+ *
+ * Three topics that have to stay together are easier to keep together as one message:
+ * remapping is a single line, nothing downstream re-synchronizes them, and a recording
+ * cannot end up with a depth frame and no color. It can also decimate, rescale depth and
+ * publish a compressed copy for a slow link.
+ *
+ * See the [node documentation](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_sync/doc/rgbd_sync.md)
+ * for topics and parameters.
+ */
 class RGBDSync : public rclcpp::Node
 {
 public:
@@ -63,6 +75,9 @@ private:
 	double compressedRate_;
 	double approxSyncMaxInterval_;
 
+	/// Stamp of the last compressed message published, for compressed_rate throttling.
+	/// Explicitly on the ROS clock: the default is the system clock, and rclcpp refuses
+	/// to compare two times that do not come from the same source.
 	rclcpp::Time lastCompressedPublished_;
 
 	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;

rtabmap_sync/include/rtabmap_sync/rgbdx_sync.hpp

@@ -46,6 +46,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap_sync
 {
 
+/**
+ * @brief Groups the `RGBDImage` topics of 2 to 8 cameras into one `RGBDImages`.
+ *
+ * For a robot carrying several RGB-D cameras. Synchronizing them here, once, means the
+ * consuming node subscribes to a single topic and needs no multi-camera build option --
+ * `rgbd_cameras=0` on CommonDataSubscriber takes the container this publishes.
+ *
+ * See the [node documentation](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_sync/doc/rgbdx_sync.md)
+ * for topics and parameters.
+ */
 class RGBDXSync : public rclcpp::Node
 {
 public:

rtabmap_sync/include/rtabmap_sync/stereo_sync.hpp

@@ -44,6 +44,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap_sync
 {
 
+/**
+ * @brief Groups a stereo pair's four topics into one `RGBDImage`.
+ *
+ * The left image goes in the color slot and the right image in the depth slot; what tells
+ * a consumer to read it as a stereo pair rather than as color plus depth is the baseline
+ * in the second calibration's P(0,3). Defaults to exact synchronization, since a stereo
+ * pair is normally hardware-triggered.
+ *
+ * See the [node documentation](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_sync/doc/stereo_sync.md)
+ * for topics and parameters.
+ */
 class StereoSync : public rclcpp::Node
 {
 public:
@@ -60,6 +71,9 @@ public:
 private:
 	double compressedRate_;
 	double approxSyncMaxInterval_;
+	/// Stamp of the last compressed message published, for compressed_rate throttling.
+	/// Explicitly on the ROS clock: the default is the system clock, and rclcpp refuses
+	/// to compare two times that do not come from the same source.
 	rclcpp::Time lastCompressedPublished_;
 
 	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;

rtabmap_sync/package.xml

@@ -25,7 +25,10 @@
   <depend>sensor_msgs</depend>
   <depend>diagnostic_updater</depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
+
   <export>
 	<build_type>ament_cmake</build_type>
+	<rosdoc2>rosdoc2.yaml</rosdoc2>
   </export>
 </package>

rtabmap_sync/rosdoc2.yaml

@@ -0,0 +1,35 @@
+## Configuration for rosdoc2, the documentation generator used by docs.ros.org.
+## Regenerate the annotated default with:
+##   rosdoc2 default_config --package-path rtabmap_sync
+## Build the docs locally with:
+##   rosdoc2 build --package-path rtabmap_sync --output-directory doc_output
+
+## This 'attic section' self-documents this file's type and version.
+type: 'rosdoc2 config'
+version: 1
+
+---
+
+settings:
+    ## Generate the standard index page from package.xml (description, maintainer,
+    ## license, links) and a table of contents for the builders below.
+    generate_package_index: true
+
+    ## This is an ament_cmake package, so doxygen runs on the public headers by
+    ## default and there are no Python modules to document.
+    always_run_doxygen: false
+    always_run_sphinx_apidoc: false
+
+builders:
+    ## Doxygen parses the public C++ API out of include/.
+    - doxygen: {
+        name: 'rtabmap_sync Public C/C++ API',
+        output_dir: 'generated/doxygen'
+    }
+    ## Sphinx renders the landing page and pulls the Doxygen XML in through
+    ## breathe/exhale so the API is browsable alongside the narrative docs.
+    - sphinx: {
+        name: 'rtabmap_sync',
+        doxygen_xml_directory: 'generated/doxygen/xml',
+        output_dir: ''
+    }

rtabmap_sync/src/nodelets/rgb_sync.cpp

@@ -46,15 +46,18 @@ namespace rtabmap_sync
 {
 
 RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
-	Node("rgbd_sync", options),
+	Node("rgb_sync", options),
 	compressedRate_(0),
 	fillEmptyDepth_(false),
+	lastCompressedPublished_(0, 0, RCL_ROS_TIME),
 	approxSync_(0),
 	exactSync_(0)
 {
 	int topicQueueSize = 10;
 	int syncQueueSize = 10;
-	bool approxSync = true;
+	// A camera publisher sends the image and its camera_info together, with the same
+	// stamp, so the exact policy is both cheaper and impossible to mismatch.
+	bool approxSync = false;
 	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	double approxSyncMaxInterval = 0.0;
 	approxSync = this->declare_parameter("approx_sync", approxSync);

rtabmap_sync/src/nodelets/rgbd_sync.cpp

@@ -51,6 +51,7 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
 	decimation_(1),
 	compressedRate_(0),
 	approxSyncMaxInterval_(0.0),
+	lastCompressedPublished_(0, 0, RCL_ROS_TIME),
 	approxSyncDepth_(0),
 	exactSyncDepth_(0)
 {

rtabmap_sync/src/nodelets/stereo_sync.cpp

@@ -48,6 +48,7 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
 		Node("stereo_sync", options),
 		compressedRate_(0),
 		approxSyncMaxInterval_(0.0),
+		lastCompressedPublished_(0, 0, RCL_ROS_TIME),
 		approxSync_(0),
 		exactSync_(0)
 {

rtabmap_sync/test/common_data_subscriber_fixture.hpp

@@ -0,0 +1,205 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#ifndef RTABMAP_SYNC_COMMON_DATA_SUBSCRIBER_FIXTURE_HPP_
+#define RTABMAP_SYNC_COMMON_DATA_SUBSCRIBER_FIXTURE_HPP_
+
+#include "node_test_utils.hpp"
+#include "msg_builders.hpp"
+
+#include <rtabmap_sync/CommonDataSubscriber.h>
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace rtabmap_sync_test {
+
+/**
+ * @brief A concrete CommonDataSubscriber that records what reached each callback.
+ *
+ * CommonDataSubscriber is abstract and does the subscribing and synchronizing for its
+ * subclass -- `rtabmap_slam`'s `rtabmap` node and `rtabmap_viz` are the two real ones.
+ * This stands in for them: it implements the four callbacks and remembers what it was
+ * handed, so a test can assert on what came out of the synchronizer.
+ */
+class RecordingSubscriber :
+		public rclcpp::Node,
+		public rtabmap_sync::CommonDataSubscriber
+{
+public:
+	/// One call of one of the four callbacks, flattened to what the tests assert on.
+	struct Record
+	{
+		enum Kind { kMultiCamera, kLaserScan, kOdom, kSensorData };
+
+		Kind kind = kMultiCamera;
+		double stamp = 0.0;              ///< stamp of whichever message drove the callback
+		size_t images = 0;               ///< number of color images
+		size_t depths = 0;               ///< number of depth (or right) images
+		size_t cameraInfos = 0;
+		bool hasOdom = false;
+		bool hasOdomInfo = false;
+		bool hasUserData = false;
+		bool hasScan2d = false;          ///< a non-empty LaserScan reached the callback
+		bool hasScan3d = false;          ///< a non-empty PointCloud2 reached the callback
+		size_t globalDescriptors = 0;
+		std::string frameId;
+	};
+
+	/**
+	 * @param options ROS options; the subscribe_* parameters go in here
+	 * @param gui     the flag the real subclasses pass: false for the SLAM node, true for
+	 *                the GUI, which defaults to subscribing to nothing but odometry
+	 */
+	RecordingSubscriber(const rclcpp::NodeOptions & options, bool gui = false) :
+		Node("recording_subscriber", options),
+		CommonDataSubscriber(*this, gui)
+	{
+		setupCallbacks(*this);
+	}
+
+	const std::vector<Record> & records() const { return records_; }
+	bool empty() const { return records_.empty(); }
+	size_t size() const { return records_.size(); }
+	const Record & back() const { return records_.back(); }
+
+protected:
+	void commonMultiCameraCallback(
+			const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
+			const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
+			const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
+			const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
+			const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
+			const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
+			const sensor_msgs::msg::LaserScan & scanMsg,
+			const sensor_msgs::msg::PointCloud2 & scan3dMsg,
+			const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr & odomInfoMsg,
+			const std::vector<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs,
+			const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > &,
+			const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > &,
+			const std::vector<cv::Mat> &) override
+	{
+		(void)depthCameraInfoMsgs;
+		Record record;
+		record.kind = Record::kMultiCamera;
+		record.images = imageMsgs.size();
+		record.depths = depthMsgs.size();
+		record.cameraInfos = cameraInfoMsgs.size();
+		record.hasOdom = odomMsg.get() != nullptr;
+		record.hasOdomInfo = odomInfoMsg.get() != nullptr;
+		record.hasUserData = userDataMsg.get() != nullptr;
+		record.hasScan2d = !scanMsg.ranges.empty();
+		record.hasScan3d = scan3dMsg.data.size() > 0;
+		record.globalDescriptors = globalDescriptorMsgs.size();
+		if(!cameraInfoMsgs.empty())
+		{
+			record.frameId = cameraInfoMsgs[0].header.frame_id;
+			record.stamp = rclcpp::Time(cameraInfoMsgs[0].header.stamp).seconds();
+		}
+		add(record);
+	}
+
+	void commonLaserScanCallback(
+			const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
+			const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
+			const sensor_msgs::msg::LaserScan & scanMsg,
+			const sensor_msgs::msg::PointCloud2 & scan3dMsg,
+			const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr & odomInfoMsg,
+			const rtabmap_msgs::msg::GlobalDescriptor & globalDescriptor) override
+	{
+		Record record;
+		record.kind = Record::kLaserScan;
+		record.hasOdom = odomMsg.get() != nullptr;
+		record.hasOdomInfo = odomInfoMsg.get() != nullptr;
+		record.hasUserData = userDataMsg.get() != nullptr;
+		record.hasScan2d = !scanMsg.ranges.empty();
+		record.hasScan3d = scan3dMsg.data.size() > 0;
+		record.globalDescriptors = globalDescriptor.data.empty() ? 0 : 1;
+		record.frameId = record.hasScan2d ?
+				scanMsg.header.frame_id : scan3dMsg.header.frame_id;
+		record.stamp = rclcpp::Time(record.hasScan2d ?
+				scanMsg.header.stamp : scan3dMsg.header.stamp).seconds();
+		add(record);
+	}
+
+	void commonOdomCallback(
+			const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
+			const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
+			const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr & odomInfoMsg) override
+	{
+		Record record;
+		record.kind = Record::kOdom;
+		record.hasOdom = odomMsg.get() != nullptr;
+		record.hasOdomInfo = odomInfoMsg.get() != nullptr;
+		record.hasUserData = userDataMsg.get() != nullptr;
+		if(odomMsg.get())
+		{
+			record.frameId = odomMsg->header.frame_id;
+			record.stamp = rclcpp::Time(odomMsg->header.stamp).seconds();
+		}
+		add(record);
+	}
+
+	void commonSensorDataCallback(
+			const rtabmap_msgs::msg::SensorData::ConstSharedPtr & sensorDataMsg,
+			const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
+			const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr & odomInfoMsg) override
+	{
+		Record record;
+		record.kind = Record::kSensorData;
+		record.hasOdom = odomMsg.get() != nullptr;
+		record.hasOdomInfo = odomInfoMsg.get() != nullptr;
+		if(sensorDataMsg.get())
+		{
+			record.cameraInfos = sensorDataMsg->left_camera_info.size();
+			record.frameId = sensorDataMsg->header.frame_id;
+			record.stamp = rclcpp::Time(sensorDataMsg->header.stamp).seconds();
+		}
+		add(record);
+	}
+
+private:
+	/// Also drives the output half of the diagnostics, as the real subclasses do.
+	void add(const Record & record)
+	{
+		records_.push_back(record);
+		tick(stampOf(record.stamp));
+	}
+
+	std::vector<Record> records_;
+};
+
+/// Fixture that starts a RecordingSubscriber and publishes its inputs.
+class CommonDataSubscriberTest : public NodeTest
+{
+protected:
+	/// Starts the subscriber under test. @p gui mirrors rtabmap_viz's constructor flag.
+	std::shared_ptr<RecordingSubscriber> start(
+			const std::vector<rclcpp::Parameter> & params = {}, bool gui = false)
+	{
+		sub_ = addNode(std::make_shared<RecordingSubscriber>(
+				rclcpp::NodeOptions().parameter_overrides(params), gui));
+		return sub_;
+	}
+
+	/// Creates a publisher on @p topic and waits for the subscriber to discover it.
+	template <typename MsgT>
+	typename rclcpp::Publisher<MsgT>::SharedPtr advertise(const std::string & topic)
+	{
+		typename rclcpp::Publisher<MsgT>::SharedPtr publisher =
+				helper()->create_publisher<MsgT>(topic, 10);
+		EXPECT_TRUE(waitForSubscriber(publisher)) << "nobody subscribed to " << topic;
+		return publisher;
+	}
+
+	std::shared_ptr<RecordingSubscriber> sub_;
+};
+
+}  // namespace rtabmap_sync_test
+
+#endif /* RTABMAP_SYNC_COMMON_DATA_SUBSCRIBER_FIXTURE_HPP_ */

rtabmap_sync/test/msg_builders.hpp

@@ -0,0 +1,284 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#ifndef RTABMAP_SYNC_MSG_BUILDERS_HPP_
+#define RTABMAP_SYNC_MSG_BUILDERS_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/image_encodings.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <rtabmap_msgs/msg/odom_info.hpp>
+#include <rtabmap_msgs/msg/rgbd_image.hpp>
+#include <rtabmap_msgs/msg/scan_descriptor.hpp>
+#include <rtabmap_msgs/msg/sensor_data.hpp>
+#include <rtabmap_msgs/msg/user_data.hpp>
+
+#include <opencv2/core/core.hpp>
+#ifdef PRE_ROS_IRON
+#include <cv_bridge/cv_bridge.h>
+#else
+#include <cv_bridge/cv_bridge.hpp>
+#endif
+
+#include <string>
+#include <vector>
+
+namespace rtabmap_sync_test {
+
+/// A ROS time from a double, the way sensor stamps are written throughout these tests.
+inline rclcpp::Time stampOf(double seconds)
+{
+	return rclcpp::Time(
+			int32_t(seconds), uint32_t((seconds - int32_t(seconds)) * 1e9), RCL_ROS_TIME);
+}
+
+/// A rectified pinhole CameraInfo; @p tx is P(0,3), non-zero for a stereo right camera.
+inline sensor_msgs::msg::CameraInfo makeCameraInfo(
+		const std::string & frameId, double stamp, int width = 8, int height = 8,
+		double tx = 0.0, double fx = 100.0)
+{
+	sensor_msgs::msg::CameraInfo info;
+	info.header.frame_id = frameId;
+	info.header.stamp = stampOf(stamp);
+	info.width = width;
+	info.height = height;
+	info.distortion_model = "plumb_bob";
+	info.d = {0.0, 0.0, 0.0, 0.0, 0.0};
+	info.k = {fx, 0.0, width/2.0, 0.0, fx, height/2.0, 0.0, 0.0, 1.0};
+	info.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
+	info.p = {fx, 0.0, width/2.0, tx, 0.0, fx, height/2.0, 0.0, 0.0, 0.0, 1.0, 0.0};
+	return info;
+}
+
+inline sensor_msgs::msg::Image makeImage(
+		const std::string & frameId, double stamp,
+		const cv::Mat & image, const std::string & encoding)
+{
+	std_msgs::msg::Header header;
+	header.frame_id = frameId;
+	header.stamp = stampOf(stamp);
+	sensor_msgs::msg::Image msg;
+	cv_bridge::CvImage(header, encoding, image).toImageMsg(msg);
+	return msg;
+}
+
+/// A bgr8 color image of a single flat color.
+inline sensor_msgs::msg::Image makeRgbImage(
+		const std::string & frameId, double stamp, int width = 8, int height = 8,
+		const cv::Scalar & color = cv::Scalar(10, 20, 30))
+{
+	return makeImage(frameId, stamp, cv::Mat(height, width, CV_8UC3, color), "bgr8");
+}
+
+/// A 16UC1 depth image in millimeters, the encoding the RGB-D drivers publish.
+inline sensor_msgs::msg::Image makeDepthImage(
+		const std::string & frameId, double stamp, int width = 8, int height = 8,
+		uint16_t millimeters = 1500)
+{
+	return makeImage(frameId, stamp,
+			cv::Mat(height, width, CV_16UC1, cv::Scalar(millimeters)), "16UC1");
+}
+
+/// A mono8 image, used as a stereo left or right frame.
+inline sensor_msgs::msg::Image makeMonoImage(
+		const std::string & frameId, double stamp, int width = 8, int height = 8,
+		uint8_t value = 60)
+{
+	return makeImage(frameId, stamp,
+			cv::Mat(height, width, CV_8UC1, cv::Scalar(value)), "mono8");
+}
+
+/// An RGB-D message with raw bgr8 color and 16UC1 depth, as rgbd_sync publishes it.
+inline rtabmap_msgs::msg::RGBDImage makeRGBDImage(
+		const std::string & frameId, double stamp, int width = 8, int height = 8,
+		const cv::Scalar & rgbColor = cv::Scalar(10, 20, 30), uint16_t depthValue = 1500)
+{
+	rtabmap_msgs::msg::RGBDImage msg;
+	msg.header.frame_id = frameId;
+	msg.header.stamp = stampOf(stamp);
+	msg.rgb = makeRgbImage(frameId, stamp, width, height, rgbColor);
+	msg.depth = makeDepthImage(frameId, stamp, width, height, depthValue);
+	msg.rgb_camera_info = makeCameraInfo(frameId, stamp, width, height);
+	msg.depth_camera_info = makeCameraInfo(frameId, stamp, width, height);
+	return msg;
+}
+
+/// A flat LaserScan of @p count equal ranges over 180 degrees.
+inline sensor_msgs::msg::LaserScan makeLaserScan(
+		const std::string & frameId, double stamp, size_t count = 10, float range = 2.0f)
+{
+	sensor_msgs::msg::LaserScan scan;
+	scan.header.frame_id = frameId;
+	scan.header.stamp = stampOf(stamp);
+	scan.angle_min = -M_PI_2;
+	scan.angle_max = M_PI_2;
+	scan.angle_increment = count > 1 ? float(M_PI / double(count - 1)) : float(M_PI);
+	scan.time_increment = 0.0f;
+	scan.scan_time = 0.1f;
+	scan.range_min = 0.1f;
+	scan.range_max = 10.0f;
+	scan.ranges.assign(count, range);
+	return scan;
+}
+
+/// A dense unorganized XYZ float cloud, the shape a 3D lidar driver publishes.
+inline sensor_msgs::msg::PointCloud2 makeXYZCloud(
+		const std::string & frameId, double stamp,
+		const std::vector<cv::Point3f> & points)
+{
+	sensor_msgs::msg::PointCloud2 cloud;
+	cloud.header.frame_id = frameId;
+	cloud.header.stamp = stampOf(stamp);
+	cloud.height = 1;
+	cloud.width = points.size();
+	cloud.is_bigendian = false;
+	cloud.is_dense = true;
+
+	cloud.fields.resize(3);
+	const char * names[3] = {"x", "y", "z"};
+	for(int i=0; i<3; ++i)
+	{
+		cloud.fields[i].name = names[i];
+		cloud.fields[i].offset = 4 * i;
+		cloud.fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32;
+		cloud.fields[i].count = 1;
+	}
+	cloud.point_step = 12;
+	cloud.row_step = cloud.point_step * cloud.width;
+	cloud.data.resize(cloud.row_step * cloud.height);
+
+	for(size_t i=0; i<points.size(); ++i)
+	{
+		float * p = reinterpret_cast<float *>(&cloud.data[i * cloud.point_step]);
+		p[0] = points[i].x;
+		p[1] = points[i].y;
+		p[2] = points[i].z;
+	}
+	return cloud;
+}
+
+/// A small cloud on a line, enough to tell one scan from another.
+inline sensor_msgs::msg::PointCloud2 makeScanCloud(
+		const std::string & frameId, double stamp, size_t count = 4)
+{
+	std::vector<cv::Point3f> points;
+	points.reserve(count);
+	for(size_t i=0; i<count; ++i)
+	{
+		points.push_back(cv::Point3f(1.0f + float(i), 0.0f, 0.0f));
+	}
+	return makeXYZCloud(frameId, stamp, points);
+}
+
+/// A ScanDescriptor carrying a 2D scan, a 3D scan, or both, and optionally a descriptor.
+inline rtabmap_msgs::msg::ScanDescriptor makeScanDescriptor(
+		const std::string & frameId, double stamp,
+		bool with2d = true, bool with3d = false, bool withGlobalDescriptor = false)
+{
+	rtabmap_msgs::msg::ScanDescriptor msg;
+	msg.header.frame_id = frameId;
+	msg.header.stamp = stampOf(stamp);
+	if(with2d)
+	{
+		msg.scan = makeLaserScan(frameId, stamp);
+	}
+	if(with3d)
+	{
+		msg.scan_cloud = makeScanCloud(frameId, stamp);
+	}
+	if(withGlobalDescriptor)
+	{
+		// Only "not empty" matters here: consumers pass the payload straight to
+		// RTAB-Map, which is what knows how to decode it.
+		msg.global_descriptor.header = msg.header;
+		msg.global_descriptor.data = {1, 2, 3, 4};
+	}
+	return msg;
+}
+
+/// An identity-pose odometry message at @p x meters along the x axis.
+inline nav_msgs::msg::Odometry makeOdometry(
+		const std::string & frameId, double stamp, double x = 0.0,
+		const std::string & childFrameId = "base_link")
+{
+	nav_msgs::msg::Odometry msg;
+	msg.header.frame_id = frameId;
+	msg.header.stamp = stampOf(stamp);
+	msg.child_frame_id = childFrameId;
+	msg.pose.pose.position.x = x;
+	msg.pose.pose.orientation.w = 1.0;
+	return msg;
+}
+
+inline rtabmap_msgs::msg::OdomInfo makeOdomInfo(
+		const std::string & frameId, double stamp, int inliers = 50)
+{
+	rtabmap_msgs::msg::OdomInfo msg;
+	msg.header.frame_id = frameId;
+	msg.header.stamp = stampOf(stamp);
+	msg.inliers = inliers;
+	msg.matches = inliers;
+	return msg;
+}
+
+/// A SensorData carrying one RGB-D camera, as rtabmap_odom republishes it.
+inline rtabmap_msgs::msg::SensorData makeSensorData(
+		const std::string & frameId, double stamp, int width = 8, int height = 8)
+{
+	rtabmap_msgs::msg::SensorData msg;
+	msg.header.frame_id = frameId;
+	msg.header.stamp = stampOf(stamp);
+	msg.left = makeRgbImage(frameId, stamp, width, height);
+	msg.right = makeDepthImage(frameId, stamp, width, height);
+	msg.left_camera_info.push_back(makeCameraInfo(frameId, stamp, width, height));
+	msg.right_camera_info.push_back(makeCameraInfo(frameId, stamp, width, height));
+	geometry_msgs::msg::Transform localTransform;
+	localTransform.rotation.w = 1.0;
+	msg.local_transform.push_back(localTransform);
+	return msg;
+}
+
+/// An uncompressed user data matrix (several rows, so it is not taken as compressed).
+inline rtabmap_msgs::msg::UserData makeUserData(
+		const std::string & frameId, double stamp)
+{
+	rtabmap_msgs::msg::UserData msg;
+	msg.header.frame_id = frameId;
+	msg.header.stamp = stampOf(stamp);
+	msg.rows = 2;
+	msg.cols = 2;
+	msg.type = CV_8UC1;
+	msg.data = {1, 2, 3, 4};
+	return msg;
+}
+
+/**
+ * @brief Reads the x/y/z of a point from any FLOAT32 xyz cloud.
+ *
+ * Looks the offsets up in the field list rather than assuming they are 0/4/8.
+ */
+inline cv::Point3f readXYZ(const sensor_msgs::msg::PointCloud2 & cloud, size_t index)
+{
+	uint32_t xOffset = 0, yOffset = 4, zOffset = 8;
+	for(size_t i=0; i<cloud.fields.size(); ++i)
+	{
+		if(cloud.fields[i].name == "x") { xOffset = cloud.fields[i].offset; }
+		else if(cloud.fields[i].name == "y") { yOffset = cloud.fields[i].offset; }
+		else if(cloud.fields[i].name == "z") { zOffset = cloud.fields[i].offset; }
+	}
+	const unsigned char * base = &cloud.data[index * cloud.point_step];
+	return cv::Point3f(
+			*reinterpret_cast<const float *>(base + xOffset),
+			*reinterpret_cast<const float *>(base + yOffset),
+			*reinterpret_cast<const float *>(base + zOffset));
+}
+
+}  // namespace rtabmap_sync_test
+
+#endif /* RTABMAP_SYNC_MSG_BUILDERS_HPP_ */

rtabmap_sync/test/node_test_utils.hpp

@@ -0,0 +1,208 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_SYNC_NODE_TEST_UTILS_HPP_
+#define RTABMAP_SYNC_NODE_TEST_UTILS_HPP_
+
+#include <gtest/gtest.h>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace rtabmap_sync_test {
+
+/**
+ * @brief Brings rclcpp up once for the whole test binary.
+ *
+ * Registered as a gtest global environment so it runs before the first test and shuts
+ * down after the last one, which keeps gtest_main usable.
+ */
+class RclcppEnvironment : public ::testing::Environment
+{
+public:
+	void SetUp() override
+	{
+		if(!rclcpp::ok())
+		{
+			rclcpp::init(0, nullptr);
+		}
+	}
+	void TearDown() override
+	{
+		if(rclcpp::ok())
+		{
+			rclcpp::shutdown();
+		}
+	}
+};
+
+/// Registers RclcppEnvironment. Call once at file scope in each test binary.
+inline ::testing::Environment * registerRclcppEnvironment()
+{
+	static ::testing::Environment * const env =
+			::testing::AddGlobalTestEnvironment(new RclcppEnvironment);
+	return env;
+}
+
+/**
+ * @brief Base fixture for driving a node under test over real ROS topics.
+ *
+ * The node under test and a helper node share one single-threaded executor, so
+ * publishing, the node's callback and the assertion all happen on the same thread and
+ * the tests stay deterministic. No launch files and no separate processes are involved:
+ * everything runs in the gtest binary.
+ */
+class NodeTest : public ::testing::Test
+{
+protected:
+	void SetUp() override
+	{
+		executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+		helper_ = std::make_shared<rclcpp::Node>("rtabmap_sync_test_helper");
+		executor_->add_node(helper_);
+	}
+
+	void TearDown() override
+	{
+		for(const rclcpp::Node::SharedPtr & node : nodes_)
+		{
+			executor_->remove_node(node);
+		}
+		nodes_.clear();
+		executor_->remove_node(helper_);
+		helper_.reset();
+		executor_.reset();
+	}
+
+	/// Adds a node under test to the shared executor and keeps it alive for the test.
+	template <typename NodeT>
+	std::shared_ptr<NodeT> addNode(const std::shared_ptr<NodeT> & node)
+	{
+		executor_->add_node(node);
+		nodes_.push_back(node);
+		return node;
+	}
+
+	/// The helper node, used to publish inputs and subscribe to outputs.
+	rclcpp::Node::SharedPtr helper() { return helper_; }
+
+	/**
+	 * @brief Spins until @p done returns true, or the timeout elapses.
+	 * @return true if @p done became true
+	 */
+	bool spinUntil(
+			const std::function<bool()> & done,
+			std::chrono::milliseconds timeout = std::chrono::milliseconds(5000))
+	{
+		const std::chrono::steady_clock::time_point deadline =
+				std::chrono::steady_clock::now() + timeout;
+		while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline)
+		{
+			if(done())
+			{
+				return true;
+			}
+			executor_->spin_once(std::chrono::milliseconds(10));
+		}
+		return done();
+	}
+
+	/// Spins for a fixed duration, for the "nothing should happen" assertions.
+	void spinFor(std::chrono::milliseconds duration)
+	{
+		const std::chrono::steady_clock::time_point deadline =
+				std::chrono::steady_clock::now() + duration;
+		while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline)
+		{
+			executor_->spin_once(std::chrono::milliseconds(10));
+		}
+	}
+
+	/**
+	 * @brief Waits until @p publisher has at least @p count matched subscriptions.
+	 *
+	 * Publishing before the node under test has discovered the topic silently drops the
+	 * message, which is the most common cause of a flaky in-process node test.
+	 */
+	template <typename PublisherT>
+	bool waitForSubscriber(const PublisherT & publisher, size_t count = 1)
+	{
+		return spinUntil([&]() { return publisher->get_subscription_count() >= count; });
+	}
+
+	/**
+	 * @brief Waits until @p subscription sees at least one publisher.
+	 *
+	 * Every node here publishes only when it has subscribers, so the test's subscription
+	 * has to be discovered before the input is sent.
+	 */
+	template <typename SubscriptionT>
+	bool waitForPublisher(const SubscriptionT & subscription, size_t count = 1)
+	{
+		return spinUntil([&]() { return subscription->get_publisher_count() >= count; });
+	}
+
+	/// Collects every message received on @p topic, for later assertions.
+	template <typename MsgT>
+	struct Collector
+	{
+		typename rclcpp::Subscription<MsgT>::SharedPtr subscription;
+		std::vector<typename MsgT::ConstSharedPtr> messages;
+		size_t size() const { return messages.size(); }
+		bool empty() const { return messages.empty(); }
+		const MsgT & back() const { return *messages.back(); }
+		const MsgT & front() const { return *messages.front(); }
+	};
+
+	/// Subscribes the helper node to @p topic and records everything it receives.
+	template <typename MsgT>
+	std::shared_ptr<Collector<MsgT>> collect(
+			const std::string & topic, const rclcpp::QoS & qos = rclcpp::QoS(10))
+	{
+		std::shared_ptr<Collector<MsgT>> collector = std::make_shared<Collector<MsgT>>();
+		collector->subscription = helper_->create_subscription<MsgT>(
+				topic, qos,
+				[collector](const typename MsgT::ConstSharedPtr msg) {
+					collector->messages.push_back(msg);
+				});
+		return collector;
+	}
+
+private:
+	rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
+	rclcpp::Node::SharedPtr helper_;
+	std::vector<rclcpp::Node::SharedPtr> nodes_;
+};
+
+}  // namespace rtabmap_sync_test
+
+#endif /* RTABMAP_SYNC_NODE_TEST_UTILS_HPP_ */

rtabmap_sync/test/test_common_data_subscriber.cpp

@@ -0,0 +1,315 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#include "common_data_subscriber_fixture.hpp"
+
+using namespace rtabmap_sync_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+}
+
+/// The subscribe_* parameters: what they select, and how conflicts between them resolve.
+///
+/// CommonDataSubscriber builds one synchronizer out of whichever inputs are asked for,
+/// and several of the flags describe the same slot in that synchronizer. Rather than
+/// refusing to start, it drops one of the two and says so in the log. These tests pin
+/// down which one survives, because that is what decides the topics a user has to remap.
+class CommonDataSubscriberConfigTest : public CommonDataSubscriberTest {};
+
+TEST_F(CommonDataSubscriberConfigTest, DefaultsToAnRGBDCameraWithOdometry)
+{
+	std::shared_ptr<RecordingSubscriber> sub = start();
+
+	EXPECT_TRUE(sub->isSubscribedToDepth());
+	EXPECT_TRUE(sub->isSubscribedToRGB());
+	EXPECT_TRUE(sub->isSubscribedToOdom());
+	EXPECT_FALSE(sub->isSubscribedToStereo());
+	EXPECT_FALSE(sub->isSubscribedToRGBD());
+	EXPECT_FALSE(sub->isSubscribedToSensorData());
+	EXPECT_FALSE(sub->isSubscribedToScan2d());
+	EXPECT_FALSE(sub->isSubscribedToScan3d());
+	EXPECT_FALSE(sub->isSubscribedToOdomInfo());
+	EXPECT_TRUE(sub->isDataSubscribed());
+	EXPECT_STREQ(sub->name().c_str(), "recording_subscriber");
+}
+
+TEST_F(CommonDataSubscriberConfigTest, TheGuiFlagSubscribesToNothingButOdometry)
+{
+	// rtabmap_viz passes gui=true: it renders whatever the SLAM node publishes and has
+	// no reason to subscribe to the raw camera topics unless asked.
+	std::shared_ptr<RecordingSubscriber> sub = start({}, /*gui=*/true);
+
+	EXPECT_FALSE(sub->isSubscribedToDepth());
+	EXPECT_FALSE(sub->isSubscribedToRGB());
+	EXPECT_TRUE(sub->isSubscribedToOdom());
+	EXPECT_TRUE(sub->isDataSubscribed()) << "odometry alone still counts as data";
+}
+
+TEST_F(CommonDataSubscriberConfigTest, StereoWinsOverDepth)
+{
+	// Both describe the camera slot. Stereo is the more specific request, so it stays
+	// and depth -- along with the rgb flag that comes with it -- is dropped.
+	std::shared_ptr<RecordingSubscriber> sub = start({
+			rclcpp::Parameter("subscribe_depth", true),
+			rclcpp::Parameter("subscribe_stereo", true)});
+
+	EXPECT_TRUE(sub->isSubscribedToStereo());
+	EXPECT_FALSE(sub->isSubscribedToDepth());
+	EXPECT_FALSE(sub->isSubscribedToRGB());
+}
+
+TEST_F(CommonDataSubscriberConfigTest, StereoWinsOverRGB)
+{
+	std::shared_ptr<RecordingSubscriber> sub = start({
+			rclcpp::Parameter("subscribe_depth", false),
+			rclcpp::Parameter("subscribe_rgb", true),
+			rclcpp::Parameter("subscribe_stereo", true)});
+
+	EXPECT_TRUE(sub->isSubscribedToStereo());
+	EXPECT_FALSE(sub->isSubscribedToRGB());
+}
+
+TEST_F(CommonDataSubscriberConfigTest, RGBDWinsOverDepthRGBAndStereo)
+{
+	// An RGBDImage already carries color, depth and calibration in one message, so it
+	// replaces every other way of describing the camera.
+	std::shared_ptr<RecordingSubscriber> sub = start({
+			rclcpp::Parameter("subscribe_depth", true),
+			rclcpp::Parameter("subscribe_rgb", true),
+			rclcpp::Parameter("subscribe_stereo", true),
+			rclcpp::Parameter("subscribe_rgbd", true)});
+
+	EXPECT_TRUE(sub->isSubscribedToRGBD());
+	EXPECT_FALSE(sub->isSubscribedToDepth());
+	EXPECT_FALSE(sub->isSubscribedToRGB());
+	EXPECT_FALSE(sub->isSubscribedToStereo());
+}
+
+TEST_F(CommonDataSubscriberConfigTest, SensorDataWinsOverEveryCameraInput)
+{
+	// A SensorData is a whole RTAB-Map frame, images and scan together; nothing else is
+	// needed alongside it.
+	std::shared_ptr<RecordingSubscriber> sub = start({
+			rclcpp::Parameter("subscribe_depth", true),
+			rclcpp::Parameter("subscribe_rgb", true),
+			rclcpp::Parameter("subscribe_stereo", true),
+			rclcpp::Parameter("subscribe_sensor_data", true)});
+
+	EXPECT_TRUE(sub->isSubscribedToSensorData());
+	EXPECT_FALSE(sub->isSubscribedToDepth());
+	EXPECT_FALSE(sub->isSubscribedToRGB());
+	EXPECT_FALSE(sub->isSubscribedToStereo());
+}
+
+TEST_F(CommonDataSubscriberConfigTest, SensorDataWinsOverRGBD)
+{
+	std::shared_ptr<RecordingSubscriber> sub = start({
+			rclcpp::Parameter("subscribe_rgbd", true),
+			rclcpp::Parameter("subscribe_sensor_data", true)});
+
+	EXPECT_TRUE(sub->isSubscribedToSensorData());
+	EXPECT_FALSE(sub->isSubscribedToRGBD());
+}
+
+TEST_F(CommonDataSubscriberConfigTest, SensorDataWinsOverEveryScanInput)
+{
+	// The scan travels inside the SensorData, so a separate scan topic would be a second
+	// copy of the same measurement.
+	std::shared_ptr<RecordingSubscriber> sub = start({
+			rclcpp::Parameter("subscribe_sensor_data", true),
+			rclcpp::Parameter("subscribe_scan", true),
+			rclcpp::Parameter("subscribe_scan_cloud", true)});
+
+	EXPECT_TRUE(sub->isSubscribedToSensorData());
+	EXPECT_FALSE(sub->isSubscribedToScan2d());
+	EXPECT_FALSE(sub->isSubscribedToScan3d());
+}
+
+TEST_F(CommonDataSubscriberConfigTest, TheTwoDScanWinsOverTheThreeDOne)
+{
+	std::shared_ptr<RecordingSubscriber> sub = start({
+			rclcpp::Parameter("subscribe_scan", true),
+			rclcpp::Parameter("subscribe_scan_cloud", true)});
+
+	EXPECT_TRUE(sub->isSubscribedToScan2d());
+	EXPECT_FALSE(sub->isSubscribedToScan3d());
+}
+
+TEST_F(CommonDataSubscriberConfigTest, TheScanDescriptorWinsOverBothPlainScans)
+{
+	// A ScanDescriptor carries the scan plus the global descriptor computed from it, so
+	// it supersedes the plain scan topics rather than sitting beside them.
+	std::shared_ptr<RecordingSubscriber> sub = start({
+			rclcpp::Parameter("subscribe_scan", true),
+			rclcpp::Parameter("subscribe_scan_descriptor", true)});
+	EXPECT_FALSE(sub->isSubscribedToScan2d());
+
+	std::shared_ptr<RecordingSubscriber> other = addNode(
+			std::make_shared<RecordingSubscriber>(rclcpp::NodeOptions()
+					.parameter_overrides({
+						rclcpp::Parameter("subscribe_scan_cloud", true),
+						rclcpp::Parameter("subscribe_scan_descriptor", true)})));
+	EXPECT_FALSE(other->isSubscribedToScan3d());
+}
+
+TEST_F(CommonDataSubscriberConfigTest, AnOdomFrameIdReplacesTheOdometryTopic)
+{
+	// With odom_frame_id set, the pose is read from TF instead. Leaving the topic
+	// subscribed as well would stall the synchronizer on a topic nobody publishes.
+	std::shared_ptr<RecordingSubscriber> sub = start({
+			rclcpp::Parameter("subscribe_odom", true),
+			rclcpp::Parameter("odom_frame_id", "odom")});
+
+	EXPECT_FALSE(sub->isSubscribedToOdom());
+}
+
+TEST_F(CommonDataSubscriberConfigTest, CamerasDefaultToApproximateSync)
+{
+	// Color and depth come off the sensor at slightly different instants.
+	EXPECT_TRUE(start()->isApproxSync());
+}
+
+TEST_F(CommonDataSubscriberConfigTest, StereoDefaultsToExactSync)
+{
+	// A stereo pair is hardware-triggered, so the two frames share a stamp exactly.
+	std::shared_ptr<RecordingSubscriber> sub = start({
+			rclcpp::Parameter("subscribe_stereo", true)});
+
+	EXPECT_FALSE(sub->isApproxSync());
+}
+
+TEST_F(CommonDataSubscriberConfigTest, AScanOnlyPipelineDefaultsToExactSync)
+{
+	// With no camera in the picture the remaining inputs are the scan and the odometry
+	// computed from it, which carries the scan's own stamp.
+	std::shared_ptr<RecordingSubscriber> sub = start({
+			rclcpp::Parameter("subscribe_depth", false),
+			rclcpp::Parameter("subscribe_rgb", false),
+			rclcpp::Parameter("subscribe_scan_cloud", true)});
+
+	EXPECT_FALSE(sub->isApproxSync());
+}
+
+TEST_F(CommonDataSubscriberConfigTest, AScanNextToACameraKeepsApproximateSync)
+{
+	// The exact default only applies when the scan is alone; a camera in the set puts
+	// the default back to approximate.
+	std::shared_ptr<RecordingSubscriber> sub = start({
+			rclcpp::Parameter("subscribe_scan_cloud", true)});
+
+	EXPECT_TRUE(sub->isSubscribedToDepth());
+	EXPECT_TRUE(sub->isApproxSync());
+}
+
+TEST_F(CommonDataSubscriberConfigTest, ApproxSyncOverridesTheDefault)
+{
+	// The parameter is declared after the defaults are worked out, so an explicit value
+	// wins in both directions.
+	EXPECT_FALSE(start({rclcpp::Parameter("approx_sync", false)})->isApproxSync());
+
+	std::shared_ptr<RecordingSubscriber> stereo = addNode(
+			std::make_shared<RecordingSubscriber>(rclcpp::NodeOptions()
+					.parameter_overrides({
+						rclcpp::Parameter("subscribe_stereo", true),
+						rclcpp::Parameter("approx_sync", true)})));
+	EXPECT_TRUE(stereo->isApproxSync());
+}
+
+TEST_F(CommonDataSubscriberConfigTest, ReportsTheConfiguredQueueSizes)
+{
+	std::shared_ptr<RecordingSubscriber> sub = start({
+			rclcpp::Parameter("topic_queue_size", 3),
+			rclcpp::Parameter("sync_queue_size", 7)});
+
+	EXPECT_EQ(sub->getTopicQueueSize(), 3);
+	EXPECT_EQ(sub->getSyncQueueSize(), 7);
+}
+
+TEST_F(CommonDataSubscriberConfigTest, TheDeprecatedQueueSizeFeedsSyncQueueSize)
+{
+	// "queue_size" was split into the two above; the old name still has to work.
+	std::shared_ptr<RecordingSubscriber> sub = start({
+			rclcpp::Parameter("queue_size", 4)});
+
+	EXPECT_EQ(sub->getSyncQueueSize(), 4);
+	EXPECT_EQ(sub->getTopicQueueSize(), 10) << "the topic queue keeps its own default";
+}
+
+TEST_F(CommonDataSubscriberConfigTest, SyncQueueSizeWinsOverTheDeprecatedName)
+{
+	std::shared_ptr<RecordingSubscriber> sub = start({
+			rclcpp::Parameter("queue_size", 4),
+			rclcpp::Parameter("sync_queue_size", 9)});
+
+	EXPECT_EQ(sub->getSyncQueueSize(), 9);
+}
+
+TEST_F(CommonDataSubscriberConfigTest, CountsOneRGBDCamera)
+{
+	std::shared_ptr<RecordingSubscriber> sub = start({
+			rclcpp::Parameter("subscribe_rgbd", true)});
+
+	EXPECT_EQ(sub->rgbdCameras(), 1);
+}
+
+TEST_F(CommonDataSubscriberConfigTest, ReportsNoRGBDCamerasOnTheRGBDImagesInterface)
+{
+	// rgbd_cameras=0 switches to the single RGBDImages topic, whose camera count is only
+	// known per message -- so there is no fixed number to report.
+	std::shared_ptr<RecordingSubscriber> sub = start({
+			rclcpp::Parameter("subscribe_rgbd", true),
+			rclcpp::Parameter("rgbd_cameras", 0)});
+
+	EXPECT_TRUE(sub->isSubscribedToRGBD());
+	EXPECT_EQ(sub->rgbdCameras(), 0);
+}
+
+TEST_F(CommonDataSubscriberConfigTest, ReportsNoRGBDCamerasWhenNotSubscribedToRGBD)
+{
+	EXPECT_EQ(start()->rgbdCameras(), 0);
+}
+
+TEST_F(CommonDataSubscriberConfigTest, NothingIsSubscribedWhenEveryInputIsOff)
+{
+	std::shared_ptr<RecordingSubscriber> sub = start({
+			rclcpp::Parameter("subscribe_depth", false),
+			rclcpp::Parameter("subscribe_rgb", false),
+			rclcpp::Parameter("subscribe_odom", false)});
+
+	EXPECT_FALSE(sub->isDataSubscribed());
+	spinFor(std::chrono::milliseconds(200));
+	EXPECT_TRUE(sub->empty());
+}
+
+#ifndef RTABMAP_SYNC_USER_DATA
+TEST_F(CommonDataSubscriberConfigTest, UserDataIsRefusedUnlessBuiltIn)
+{
+	// The user-data synchronizers are behind a build option, because they double the
+	// number of synchronizer templates the package has to compile.
+	std::shared_ptr<RecordingSubscriber> sub = start({
+			rclcpp::Parameter("subscribe_user_data", true)});
+
+	EXPECT_TRUE(sub->isSubscribedToDepth()) << "the rest of the setup must still happen";
+	spinFor(std::chrono::milliseconds(100));
+	EXPECT_EQ(helper()->count_publishers("/user_data"), 0u);
+}
+#endif
+
+#ifndef RTABMAP_SYNC_MULTI_RGBD
+TEST_F(CommonDataSubscriberConfigTest, MoreThanOneRGBDCameraIsRefusedUnlessBuiltIn)
+{
+	// Synchronizing several RGBDImage topics is behind a build option for the same
+	// reason. Without it, nothing is subscribed -- rgbd_cameras=0 is the way out.
+	std::shared_ptr<RecordingSubscriber> sub = start({
+			rclcpp::Parameter("subscribe_rgbd", true),
+			rclcpp::Parameter("rgbd_cameras", 2)});
+
+	spinFor(std::chrono::milliseconds(200));
+	EXPECT_EQ(helper()->count_subscribers("/rgbd_image0"), 0u);
+	EXPECT_EQ(helper()->count_subscribers("/rgbd_image"), 0u);
+	EXPECT_TRUE(sub->empty());
+}
+#endif

rtabmap_sync/test/test_common_data_subscriber_sync.cpp

@@ -0,0 +1,540 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#include "common_data_subscriber_fixture.hpp"
+
+#include <rtabmap_msgs/msg/rgbd_images.hpp>
+
+using namespace rtabmap_sync_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+}
+
+/// End-to-end: real messages in on the topics each mode subscribes to, one callback out.
+///
+/// Every set below is published with identical stamps, so the result does not depend on
+/// which sync policy the mode defaults to. What each test pins down is the wiring: which
+/// topics a given combination of subscribe_* flags listens on, which of the four
+/// callbacks fires, and which slots of it are filled.
+class CommonDataSubscriberSyncTest : public CommonDataSubscriberTest {};
+
+TEST_F(CommonDataSubscriberSyncTest, DepthModeDeliversOneCameraToTheMultiCameraCallback)
+{
+	start({rclcpp::Parameter("subscribe_odom", false)});
+
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
+			advertise<sensor_msgs::msg::Image>("rgb/image");
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
+			advertise<sensor_msgs::msg::Image>("depth/image");
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
+			advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
+
+	rgb->publish(makeRgbImage("camera_link", 1000.0));
+	depth->publish(makeDepthImage("camera_link", 1000.0));
+	info->publish(makeCameraInfo("camera_link", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
+
+	const RecordingSubscriber::Record & got = sub_->back();
+	EXPECT_EQ(got.kind, RecordingSubscriber::Record::kMultiCamera);
+	EXPECT_EQ(got.images, 1u);
+	EXPECT_EQ(got.depths, 1u);
+	EXPECT_EQ(got.cameraInfos, 1u);
+	EXPECT_EQ(got.frameId, "camera_link");
+	EXPECT_DOUBLE_EQ(got.stamp, 1000.0);
+	EXPECT_FALSE(got.hasOdom);
+	EXPECT_FALSE(got.hasOdomInfo);
+	EXPECT_FALSE(got.hasScan2d);
+	EXPECT_FALSE(got.hasScan3d);
+}
+
+TEST_F(CommonDataSubscriberSyncTest, DepthModeWithOdometryWaitsForThePose)
+{
+	start();  // subscribe_odom defaults to true
+
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
+			advertise<sensor_msgs::msg::Image>("rgb/image");
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
+			advertise<sensor_msgs::msg::Image>("depth/image");
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
+			advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom =
+			advertise<nav_msgs::msg::Odometry>("odom");
+
+	// The camera alone is not a complete set.
+	rgb->publish(makeRgbImage("camera_link", 1000.0));
+	depth->publish(makeDepthImage("camera_link", 1000.0));
+	info->publish(makeCameraInfo("camera_link", 1000.0));
+	spinFor(std::chrono::milliseconds(300));
+	EXPECT_TRUE(sub_->empty()) << "without the pose the frame cannot be placed in the map";
+
+	odom->publish(makeOdometry("odom", 1000.0, 1.5));
+	ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
+	EXPECT_TRUE(sub_->back().hasOdom);
+}
+
+TEST_F(CommonDataSubscriberSyncTest, DepthModeCanAlsoTakeTheOdometryInfo)
+{
+	start({rclcpp::Parameter("subscribe_odom", false),
+		   rclcpp::Parameter("subscribe_odom_info", true)});
+	EXPECT_TRUE(sub_->isSubscribedToOdomInfo());
+
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
+			advertise<sensor_msgs::msg::Image>("rgb/image");
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
+			advertise<sensor_msgs::msg::Image>("depth/image");
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
+			advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
+	rclcpp::Publisher<rtabmap_msgs::msg::OdomInfo>::SharedPtr odomInfo =
+			advertise<rtabmap_msgs::msg::OdomInfo>("odom_info");
+
+	rgb->publish(makeRgbImage("camera_link", 1000.0));
+	depth->publish(makeDepthImage("camera_link", 1000.0));
+	info->publish(makeCameraInfo("camera_link", 1000.0));
+	odomInfo->publish(makeOdomInfo("odom", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
+
+	EXPECT_TRUE(sub_->back().hasOdomInfo);
+	EXPECT_FALSE(sub_->back().hasOdom) << "the info is not the pose";
+}
+
+TEST_F(CommonDataSubscriberSyncTest, DepthModeCarriesATwoDScanAlongside)
+{
+	start({rclcpp::Parameter("subscribe_odom", false),
+		   rclcpp::Parameter("subscribe_scan", true)});
+
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
+			advertise<sensor_msgs::msg::Image>("rgb/image");
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
+			advertise<sensor_msgs::msg::Image>("depth/image");
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
+			advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan =
+			advertise<sensor_msgs::msg::LaserScan>("scan");
+
+	rgb->publish(makeRgbImage("camera_link", 1000.0));
+	depth->publish(makeDepthImage("camera_link", 1000.0));
+	info->publish(makeCameraInfo("camera_link", 1000.0));
+	scan->publish(makeLaserScan("base_scan", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
+
+	EXPECT_EQ(sub_->back().images, 1u);
+	EXPECT_TRUE(sub_->back().hasScan2d);
+	EXPECT_FALSE(sub_->back().hasScan3d);
+}
+
+TEST_F(CommonDataSubscriberSyncTest, DepthModeCarriesAThreeDScanAlongside)
+{
+	start({rclcpp::Parameter("subscribe_odom", false),
+		   rclcpp::Parameter("subscribe_scan_cloud", true)});
+
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
+			advertise<sensor_msgs::msg::Image>("rgb/image");
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
+			advertise<sensor_msgs::msg::Image>("depth/image");
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
+			advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud =
+			advertise<sensor_msgs::msg::PointCloud2>("scan_cloud");
+
+	rgb->publish(makeRgbImage("camera_link", 1000.0));
+	depth->publish(makeDepthImage("camera_link", 1000.0));
+	info->publish(makeCameraInfo("camera_link", 1000.0));
+	cloud->publish(makeScanCloud("lidar_link", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
+
+	EXPECT_TRUE(sub_->back().hasScan3d);
+	EXPECT_FALSE(sub_->back().hasScan2d);
+}
+
+TEST_F(CommonDataSubscriberSyncTest, AScanDescriptorIsUnpackedIntoScanAndDescriptor)
+{
+	// The descriptor topic replaces the scan topic and carries the scan inside it, plus
+	// the global descriptor computed from that same scan.
+	start({rclcpp::Parameter("subscribe_odom", false),
+		   rclcpp::Parameter("subscribe_scan_descriptor", true)});
+
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
+			advertise<sensor_msgs::msg::Image>("rgb/image");
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
+			advertise<sensor_msgs::msg::Image>("depth/image");
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
+			advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
+	rclcpp::Publisher<rtabmap_msgs::msg::ScanDescriptor>::SharedPtr descriptor =
+			advertise<rtabmap_msgs::msg::ScanDescriptor>("scan_descriptor");
+
+	rgb->publish(makeRgbImage("camera_link", 1000.0));
+	depth->publish(makeDepthImage("camera_link", 1000.0));
+	info->publish(makeCameraInfo("camera_link", 1000.0));
+	descriptor->publish(makeScanDescriptor("base_scan", 1000.0,
+			/*with2d=*/true, /*with3d=*/false, /*withGlobalDescriptor=*/true));
+	ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
+
+	EXPECT_TRUE(sub_->back().hasScan2d) << "the scan inside the descriptor must be used";
+	EXPECT_EQ(sub_->back().globalDescriptors, 1u);
+}
+
+TEST_F(CommonDataSubscriberSyncTest, AnEmptyGlobalDescriptorIsNotForwarded)
+{
+	// An empty descriptor is "none computed", not a descriptor of length zero.
+	start({rclcpp::Parameter("subscribe_odom", false),
+		   rclcpp::Parameter("subscribe_scan_descriptor", true)});
+
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
+			advertise<sensor_msgs::msg::Image>("rgb/image");
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
+			advertise<sensor_msgs::msg::Image>("depth/image");
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
+			advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
+	rclcpp::Publisher<rtabmap_msgs::msg::ScanDescriptor>::SharedPtr descriptor =
+			advertise<rtabmap_msgs::msg::ScanDescriptor>("scan_descriptor");
+
+	rgb->publish(makeRgbImage("camera_link", 1000.0));
+	depth->publish(makeDepthImage("camera_link", 1000.0));
+	info->publish(makeCameraInfo("camera_link", 1000.0));
+	descriptor->publish(makeScanDescriptor("base_scan", 1000.0,
+			/*with2d=*/true, /*with3d=*/false, /*withGlobalDescriptor=*/false));
+	ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
+
+	EXPECT_TRUE(sub_->back().hasScan2d);
+	EXPECT_EQ(sub_->back().globalDescriptors, 0u);
+}
+
+TEST_F(CommonDataSubscriberSyncTest, RGBModeDeliversNoDepth)
+{
+	start({rclcpp::Parameter("subscribe_depth", false),
+		   rclcpp::Parameter("subscribe_rgb", true),
+		   rclcpp::Parameter("subscribe_odom", false)});
+
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
+			advertise<sensor_msgs::msg::Image>("rgb/image");
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
+			advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
+
+	rgb->publish(makeRgbImage("camera_link", 1000.0));
+	info->publish(makeCameraInfo("camera_link", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
+
+	EXPECT_EQ(sub_->back().images, 1u);
+	EXPECT_EQ(sub_->back().depths, 0u)
+		<< "an empty depth vector is how the callback learns there is no depth";
+	EXPECT_EQ(sub_->back().cameraInfos, 1u);
+}
+
+TEST_F(CommonDataSubscriberSyncTest, StereoModeDeliversTheRightImageInTheDepthSlot)
+{
+	start({rclcpp::Parameter("subscribe_stereo", true),
+		   rclcpp::Parameter("subscribe_odom", false)});
+
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr left =
+			advertise<sensor_msgs::msg::Image>("left/image_rect");
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr right =
+			advertise<sensor_msgs::msg::Image>("right/image_rect");
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr leftInfo =
+			advertise<sensor_msgs::msg::CameraInfo>("left/camera_info");
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rightInfo =
+			advertise<sensor_msgs::msg::CameraInfo>("right/camera_info");
+
+	left->publish(makeMonoImage("left_frame", 1000.0));
+	right->publish(makeMonoImage("left_frame", 1000.0));
+	leftInfo->publish(makeCameraInfo("left_frame", 1000.0));
+	rightInfo->publish(makeCameraInfo("left_frame", 1000.0, 8, 8, /*tx=*/-12.0));
+	ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
+
+	EXPECT_EQ(sub_->back().images, 1u);
+	EXPECT_EQ(sub_->back().depths, 1u);
+	EXPECT_EQ(sub_->back().frameId, "left_frame");
+}
+
+TEST_F(CommonDataSubscriberSyncTest, RGBDModeUnpacksTheMessageIntoImages)
+{
+	start({rclcpp::Parameter("subscribe_rgbd", true),
+		   rclcpp::Parameter("subscribe_odom", false)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbd =
+			advertise<rtabmap_msgs::msg::RGBDImage>("rgbd_image");
+
+	rgbd->publish(makeRGBDImage("camera_link", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
+
+	const RecordingSubscriber::Record & got = sub_->back();
+	EXPECT_EQ(got.kind, RecordingSubscriber::Record::kMultiCamera);
+	EXPECT_EQ(got.images, 1u);
+	EXPECT_EQ(got.depths, 1u);
+	EXPECT_EQ(got.cameraInfos, 1u);
+	EXPECT_EQ(got.frameId, "camera_link");
+}
+
+TEST_F(CommonDataSubscriberSyncTest, RGBDModeCarriesAScanAlongside)
+{
+	start({rclcpp::Parameter("subscribe_rgbd", true),
+		   rclcpp::Parameter("subscribe_odom", false),
+		   rclcpp::Parameter("subscribe_scan_cloud", true)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbd =
+			advertise<rtabmap_msgs::msg::RGBDImage>("rgbd_image");
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud =
+			advertise<sensor_msgs::msg::PointCloud2>("scan_cloud");
+
+	rgbd->publish(makeRGBDImage("camera_link", 1000.0));
+	cloud->publish(makeScanCloud("lidar_link", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
+
+	EXPECT_EQ(sub_->back().images, 1u);
+	EXPECT_TRUE(sub_->back().hasScan3d);
+}
+
+TEST_F(CommonDataSubscriberSyncTest, TheRGBDImagesInterfaceDeliversEveryCamera)
+{
+	// rgbd_cameras=0 takes a pre-grouped RGBDImages -- what rgbdx_sync publishes -- so
+	// any number of cameras works without the multi-RGBD build option.
+	start({rclcpp::Parameter("subscribe_rgbd", true),
+		   rclcpp::Parameter("rgbd_cameras", 0),
+		   rclcpp::Parameter("subscribe_odom", false)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr rgbdx =
+			advertise<rtabmap_msgs::msg::RGBDImages>("rgbd_images");
+
+	rtabmap_msgs::msg::RGBDImages msg;
+	msg.header.frame_id = "camera0_link";
+	msg.header.stamp = stampOf(1000.0);
+	msg.rgbd_images.push_back(makeRGBDImage("camera0_link", 1000.0));
+	msg.rgbd_images.push_back(makeRGBDImage("camera1_link", 1000.0));
+	msg.rgbd_images.push_back(makeRGBDImage("camera2_link", 1000.0));
+	rgbdx->publish(msg);
+	ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
+
+	const RecordingSubscriber::Record & got = sub_->back();
+	EXPECT_EQ(got.images, 3u);
+	EXPECT_EQ(got.depths, 3u);
+	EXPECT_EQ(got.cameraInfos, 3u);
+	EXPECT_EQ(got.frameId, "camera0_link");
+}
+
+TEST_F(CommonDataSubscriberSyncTest, ATwoDScanAloneGoesToTheLaserScanCallback)
+{
+	start({rclcpp::Parameter("subscribe_depth", false),
+		   rclcpp::Parameter("subscribe_rgb", false),
+		   rclcpp::Parameter("subscribe_odom", false),
+		   rclcpp::Parameter("subscribe_scan", true)});
+
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan =
+			advertise<sensor_msgs::msg::LaserScan>("scan");
+
+	scan->publish(makeLaserScan("base_scan", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
+
+	const RecordingSubscriber::Record & got = sub_->back();
+	EXPECT_EQ(got.kind, RecordingSubscriber::Record::kLaserScan);
+	EXPECT_TRUE(got.hasScan2d);
+	EXPECT_FALSE(got.hasScan3d);
+	EXPECT_EQ(got.frameId, "base_scan");
+	EXPECT_DOUBLE_EQ(got.stamp, 1000.0);
+}
+
+TEST_F(CommonDataSubscriberSyncTest, AThreeDScanAloneGoesToTheLaserScanCallback)
+{
+	start({rclcpp::Parameter("subscribe_depth", false),
+		   rclcpp::Parameter("subscribe_rgb", false),
+		   rclcpp::Parameter("subscribe_odom", false),
+		   rclcpp::Parameter("subscribe_scan_cloud", true)});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud =
+			advertise<sensor_msgs::msg::PointCloud2>("scan_cloud");
+
+	cloud->publish(makeScanCloud("lidar_link", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
+
+	const RecordingSubscriber::Record & got = sub_->back();
+	EXPECT_EQ(got.kind, RecordingSubscriber::Record::kLaserScan);
+	EXPECT_TRUE(got.hasScan3d);
+	EXPECT_EQ(got.frameId, "lidar_link");
+}
+
+TEST_F(CommonDataSubscriberSyncTest, AScanWithOdometryIsSynchronizedWithIt)
+{
+	start({rclcpp::Parameter("subscribe_depth", false),
+		   rclcpp::Parameter("subscribe_rgb", false),
+		   rclcpp::Parameter("subscribe_scan_cloud", true)});
+	EXPECT_TRUE(sub_->isSubscribedToOdom());
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud =
+			advertise<sensor_msgs::msg::PointCloud2>("scan_cloud");
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom =
+			advertise<nav_msgs::msg::Odometry>("odom");
+
+	cloud->publish(makeScanCloud("lidar_link", 1000.0));
+	spinFor(std::chrono::milliseconds(300));
+	EXPECT_TRUE(sub_->empty());
+
+	odom->publish(makeOdometry("odom", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
+	EXPECT_TRUE(sub_->back().hasOdom);
+	EXPECT_TRUE(sub_->back().hasScan3d);
+}
+
+TEST_F(CommonDataSubscriberSyncTest, ASensorDataGoesToItsOwnCallback)
+{
+	start({rclcpp::Parameter("subscribe_sensor_data", true),
+		   rclcpp::Parameter("subscribe_odom", false)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::SensorData>::SharedPtr data =
+			advertise<rtabmap_msgs::msg::SensorData>("sensor_data");
+
+	data->publish(makeSensorData("camera_link", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
+
+	const RecordingSubscriber::Record & got = sub_->back();
+	EXPECT_EQ(got.kind, RecordingSubscriber::Record::kSensorData);
+	EXPECT_EQ(got.cameraInfos, 1u);
+	EXPECT_EQ(got.frameId, "camera_link");
+	EXPECT_FALSE(got.hasOdom);
+}
+
+TEST_F(CommonDataSubscriberSyncTest, ASensorDataCanBeSynchronizedWithOdometry)
+{
+	start({rclcpp::Parameter("subscribe_sensor_data", true)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::SensorData>::SharedPtr data =
+			advertise<rtabmap_msgs::msg::SensorData>("sensor_data");
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom =
+			advertise<nav_msgs::msg::Odometry>("odom");
+
+	data->publish(makeSensorData("camera_link", 1000.0));
+	odom->publish(makeOdometry("odom", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
+
+	EXPECT_EQ(sub_->back().kind, RecordingSubscriber::Record::kSensorData);
+	EXPECT_TRUE(sub_->back().hasOdom);
+}
+
+TEST_F(CommonDataSubscriberSyncTest, OdometryAloneGoesToTheOdomCallback)
+{
+	start({rclcpp::Parameter("subscribe_depth", false),
+		   rclcpp::Parameter("subscribe_rgb", false)});
+
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom =
+			advertise<nav_msgs::msg::Odometry>("odom");
+
+	odom->publish(makeOdometry("odom", 1000.0, 2.5));
+	ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
+
+	const RecordingSubscriber::Record & got = sub_->back();
+	EXPECT_EQ(got.kind, RecordingSubscriber::Record::kOdom);
+	EXPECT_TRUE(got.hasOdom);
+	EXPECT_FALSE(got.hasOdomInfo);
+	EXPECT_EQ(got.frameId, "odom");
+	EXPECT_DOUBLE_EQ(got.stamp, 1000.0);
+}
+
+TEST_F(CommonDataSubscriberSyncTest, OdometryAndItsInfoAreSynchronizedTogether)
+{
+	start({rclcpp::Parameter("subscribe_depth", false),
+		   rclcpp::Parameter("subscribe_rgb", false),
+		   rclcpp::Parameter("subscribe_odom_info", true)});
+
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom =
+			advertise<nav_msgs::msg::Odometry>("odom");
+	rclcpp::Publisher<rtabmap_msgs::msg::OdomInfo>::SharedPtr odomInfo =
+			advertise<rtabmap_msgs::msg::OdomInfo>("odom_info");
+
+	odom->publish(makeOdometry("odom", 1000.0));
+	spinFor(std::chrono::milliseconds(300));
+	EXPECT_TRUE(sub_->empty()) << "the pair is incomplete until the info arrives";
+
+	odomInfo->publish(makeOdomInfo("odom", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
+	EXPECT_TRUE(sub_->back().hasOdom);
+	EXPECT_TRUE(sub_->back().hasOdomInfo);
+}
+
+TEST_F(CommonDataSubscriberSyncTest, DeliversEveryFrameOfAStream)
+{
+	start({rclcpp::Parameter("subscribe_odom", false)});
+
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
+			advertise<sensor_msgs::msg::Image>("rgb/image");
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
+			advertise<sensor_msgs::msg::Image>("depth/image");
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
+			advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
+
+	for(int i=0; i<5; ++i)
+	{
+		const double stamp = 1000.0 + 0.1*double(i);
+		rgb->publish(makeRgbImage("camera_link", stamp));
+		depth->publish(makeDepthImage("camera_link", stamp));
+		info->publish(makeCameraInfo("camera_link", stamp));
+		ASSERT_TRUE(spinUntil([&, i]() { return sub_->size() == size_t(i+1); }))
+			<< "frame " << i << " never arrived";
+	}
+
+	ASSERT_EQ(sub_->size(), 5u);
+	for(size_t i=1; i<sub_->size(); ++i)
+	{
+		EXPECT_GT(sub_->records()[i].stamp, sub_->records()[i-1].stamp);
+	}
+}
+
+TEST_F(CommonDataSubscriberSyncTest, ExactSyncDropsAnIncompleteSet)
+{
+	// With approx_sync off every input has to carry the same stamp, which is the whole
+	// point of the setting -- and the most common reason a pipeline goes quiet.
+	start({rclcpp::Parameter("subscribe_odom", false),
+		   rclcpp::Parameter("approx_sync", false)});
+
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
+			advertise<sensor_msgs::msg::Image>("rgb/image");
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
+			advertise<sensor_msgs::msg::Image>("depth/image");
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
+			advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
+
+	rgb->publish(makeRgbImage("camera_link", 1000.000));
+	depth->publish(makeDepthImage("camera_link", 1000.002));
+	info->publish(makeCameraInfo("camera_link", 1000.000));
+	spinFor(std::chrono::milliseconds(400));
+	EXPECT_TRUE(sub_->empty());
+
+	rgb->publish(makeRgbImage("camera_link", 1001.0));
+	depth->publish(makeDepthImage("camera_link", 1001.0));
+	info->publish(makeCameraInfo("camera_link", 1001.0));
+	EXPECT_TRUE(spinUntil([&]() { return !sub_->empty(); }));
+}
+
+TEST_F(CommonDataSubscriberSyncTest, PublishesDiagnostics)
+{
+	start({rclcpp::Parameter("subscribe_odom", false)});
+
+	std::shared_ptr<Collector<diagnostic_msgs::msg::DiagnosticArray>> diagnostics =
+			collect<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics");
+
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
+			advertise<sensor_msgs::msg::Image>("rgb/image");
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
+			advertise<sensor_msgs::msg::Image>("depth/image");
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
+			advertise<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
+
+	rgb->publish(makeRgbImage("camera_link", 1000.0));
+	depth->publish(makeDepthImage("camera_link", 1000.0));
+	info->publish(makeCameraInfo("camera_link", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !diagnostics->empty(); },
+			std::chrono::milliseconds(10000)));
+
+	bool sawInput = false;
+	bool sawOutput = false;
+	for(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr & msg :
+			diagnostics->messages)
+	{
+		for(const diagnostic_msgs::msg::DiagnosticStatus & status : msg->status)
+		{
+			sawInput = sawInput || status.name.find("Input Status") != std::string::npos;
+			sawOutput = sawOutput || status.name.find("Output Status") != std::string::npos;
+		}
+	}
+	EXPECT_TRUE(sawInput);
+	EXPECT_TRUE(sawOutput) << "tick() is what the subclass calls to report its own rate";
+}

rtabmap_sync/test/test_rgb_sync.cpp

@@ -0,0 +1,258 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#include "node_test_utils.hpp"
+#include "msg_builders.hpp"
+
+#include <rtabmap_sync/rgb_sync.hpp>
+
+#include <rtabmap/core/Compression.h>
+
+#include <string>
+#include <vector>
+
+using namespace rtabmap_sync_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+}
+
+/// Drives rgb_sync over its two input topics and collects both outputs.
+class RGBSyncTest : public NodeTest
+{
+protected:
+	void start(const std::vector<rclcpp::Parameter> & params = {})
+	{
+		node_ = addNode(std::make_shared<rtabmap_sync::RGBSync>(
+				rclcpp::NodeOptions().parameter_overrides(params)));
+
+		out_ = collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image");
+		rgbPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
+		infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>(
+				"rgb/camera_info", 10);
+		ASSERT_TRUE(waitForSubscriber(rgbPub_));
+		ASSERT_TRUE(waitForSubscriber(infoPub_));
+		ASSERT_TRUE(waitForSubscribedFromNode("rgbd_image"));
+	}
+
+	void collectCompressed()
+	{
+		compressed_ = collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image/compressed");
+		ASSERT_TRUE(waitForSubscribedFromNode("rgbd_image/compressed"));
+	}
+
+	/// Waits until the node under test sees a subscriber on @p topic. @see RGBDSyncTest.
+	bool waitForSubscribedFromNode(const std::string & topic)
+	{
+		return spinUntil([&]() { return node_->count_subscribers(topic) > 0; });
+	}
+
+	void publish(double stamp, int width = 8, int height = 8)
+	{
+		rgbPub_->publish(makeRgbImage("camera_link", stamp, width, height));
+		infoPub_->publish(makeCameraInfo("camera_link", stamp, width, height));
+	}
+
+	std::shared_ptr<rtabmap_sync::RGBSync> node_;
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out_;
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> compressed_;
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgbPub_;
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub_;
+};
+
+TEST_F(RGBSyncTest, PacksColorAndCalibrationIntoAnRGBDImage)
+{
+	start();
+
+	publish(1000.0);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImage & got = out_->back();
+	EXPECT_EQ(got.header.frame_id, "camera_link") << "the frame comes from the camera_info";
+	EXPECT_DOUBLE_EQ(rclcpp::Time(got.header.stamp).seconds(), 1000.0);
+	EXPECT_EQ(got.rgb.encoding, "bgr8");
+	EXPECT_EQ(got.rgb.width, 8u);
+	EXPECT_NEAR(got.rgb_camera_info.k[0], 100.0, 1e-9);
+}
+
+TEST_F(RGBSyncTest, LeavesDepthEmptyByDefault)
+{
+	// The point of this node is an RGB-only pipeline: there is no depth to carry, and a
+	// consumer has to be able to tell that from an all-zero depth image.
+	start();
+
+	publish(1000.0);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImage & got = out_->back();
+	EXPECT_TRUE(got.depth.data.empty());
+	EXPECT_EQ(got.depth.width, 0u);
+	EXPECT_EQ(got.depth_camera_info.width, 0u) << "no depth means no depth calibration";
+}
+
+TEST_F(RGBSyncTest, FillEmptyDepthAddsAZeroedDepthImage)
+{
+	// Some consumers refuse a message without depth. This gives them one that is
+	// entirely "no reading", which is how zero is interpreted in a depth image.
+	start({rclcpp::Parameter("fill_empty_depth", true)});
+
+	publish(1000.0, /*width=*/8, /*height=*/8);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImage & got = out_->back();
+	ASSERT_FALSE(got.depth.data.empty());
+	EXPECT_EQ(got.depth.encoding, "16UC1");
+	EXPECT_EQ(got.depth.width, 8u);
+	EXPECT_EQ(got.depth.height, 8u);
+	for(size_t i=0; i<got.depth.data.size(); ++i)
+	{
+		ASSERT_EQ(got.depth.data[i], 0u) << "byte " << i << " is not zero";
+	}
+	EXPECT_EQ(got.depth_camera_info.width, 8u)
+		<< "the fake depth is registered to the color camera, so it shares its calibration";
+}
+
+TEST_F(RGBSyncTest, DefaultsToExactSync)
+{
+	// A camera publisher sends the image and its camera_info together with the same
+	// stamp, so there is nothing to approximate.
+	start();
+
+	rgbPub_->publish(makeRgbImage("camera_link", 1000.0));
+	infoPub_->publish(makeCameraInfo("camera_link", 1000.004));
+	spinFor(std::chrono::milliseconds(500));
+	EXPECT_TRUE(out_->empty()) << "the default must not pair stamps 4 ms apart";
+
+	publish(1001.0);
+	EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+}
+
+TEST_F(RGBSyncTest, ExactSyncRejectsFramesWithDifferentStamps)
+{
+	start({rclcpp::Parameter("approx_sync", false)});
+
+	rgbPub_->publish(makeRgbImage("camera_link", 1000.0));
+	infoPub_->publish(makeCameraInfo("camera_link", 1000.004));
+	spinFor(std::chrono::milliseconds(500));
+	EXPECT_TRUE(out_->empty());
+
+	publish(1001.0);
+	EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+}
+
+TEST_F(RGBSyncTest, ApproxSyncPairsAnImageWithANearbyCameraInfo)
+{
+	// A camera_info republished on its own timer does not carry the image's stamp.
+	start({rclcpp::Parameter("approx_sync", true)});
+
+	for(int i=0; i<5; ++i)
+	{
+		const double stamp = 1000.0 + 0.1*double(i);
+		rgbPub_->publish(makeRgbImage("camera_link", stamp));
+		infoPub_->publish(makeCameraInfo("camera_link", stamp + 0.004));
+		spinFor(std::chrono::milliseconds(20));
+	}
+	EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+}
+
+TEST_F(RGBSyncTest, CompressesColorAsJpeg)
+{
+	start();
+	collectCompressed();
+
+	publish(1000.0);
+	ASSERT_TRUE(spinUntil([&]() { return !compressed_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImage & got = compressed_->back();
+	ASSERT_FALSE(got.rgb_compressed.data.empty());
+	EXPECT_NE(got.rgb_compressed.format.find("jp"), std::string::npos)
+		<< "expected a jpeg format, got \"" << got.rgb_compressed.format << "\"";
+	EXPECT_TRUE(got.rgb.data.empty()) << "the compressed output carries no raw image";
+	EXPECT_TRUE(got.depth_compressed.data.empty())
+		<< "without fill_empty_depth there is nothing to compress on the depth side";
+}
+
+TEST_F(RGBSyncTest, CompressesTheFakeDepthAsPng)
+{
+	start({rclcpp::Parameter("fill_empty_depth", true)});
+	collectCompressed();
+
+	publish(1000.0);
+	ASSERT_TRUE(spinUntil([&]() { return !compressed_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImage & got = compressed_->back();
+	ASSERT_FALSE(got.depth_compressed.data.empty());
+	EXPECT_EQ(got.depth_compressed.format, "png");
+	const cv::Mat depth = rtabmap::uncompressImage(got.depth_compressed.data);
+	ASSERT_FALSE(depth.empty());
+	EXPECT_EQ(depth.type(), CV_16UC1);
+	EXPECT_EQ(cv::countNonZero(depth), 0) << "the fake depth is all zeros";
+}
+
+TEST_F(RGBSyncTest, CompressedRateThrottlesTheCompressedOutputOnly)
+{
+	// A long window (0.2 Hz = five seconds); the throttle runs off the wall clock, so a
+	// slow machine must not spill the four frames into a second one.
+	start({rclcpp::Parameter("compressed_rate", 0.2)});
+	collectCompressed();
+
+	for(int i=0; i<4; ++i)
+	{
+		publish(1000.0 + 0.01*double(i));
+		ASSERT_TRUE(spinUntil([&, i]() { return out_->size() == size_t(i+1); }));
+	}
+
+	spinFor(std::chrono::milliseconds(200));
+	EXPECT_EQ(out_->size(), 4u) << "the raw output is never throttled";
+	EXPECT_EQ(compressed_->size(), 1u)
+		<< "only the first of four back-to-back frames may be compressed";
+}
+
+TEST_F(RGBSyncTest, StaysSilentWithoutASubscriber)
+{
+	addNode(std::make_shared<rtabmap_sync::RGBSync>(rclcpp::NodeOptions()));
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgbPub =
+			helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub =
+			helper()->create_publisher<sensor_msgs::msg::CameraInfo>("rgb/camera_info", 10);
+	ASSERT_TRUE(waitForSubscriber(rgbPub));
+	ASSERT_TRUE(waitForSubscriber(infoPub));
+
+	rgbPub->publish(makeRgbImage("camera_link", 1000.0));
+	infoPub->publish(makeCameraInfo("camera_link", 1000.0));
+	spinFor(std::chrono::milliseconds(300));
+
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> late =
+			collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image");
+	spinFor(std::chrono::milliseconds(200));
+	EXPECT_TRUE(late->empty());
+}
+
+TEST_F(RGBSyncTest, IsNamedAfterItself)
+{
+	// It used to default to "rgbd_sync", which put it on top of the other node's name
+	// in the graph whenever both were launched without an explicit name.
+	start();
+	EXPECT_STREQ(node_->get_name(), "rgb_sync");
+}
+
+TEST_F(RGBSyncTest, AcceptsTheDeprecatedQueueSizeParameter)
+{
+	start({rclcpp::Parameter("queue_size", 5)});
+
+	publish(1000.0);
+	EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+}
+
+TEST_F(RGBSyncTest, SubscribesBestEffortWhenAsked)
+{
+	addNode(std::make_shared<rtabmap_sync::RGBSync>(rclcpp::NodeOptions()
+			.parameter_overrides({rclcpp::Parameter("qos", 2)})));
+
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr bestEffort =
+			helper()->create_publisher<sensor_msgs::msg::Image>(
+					"rgb/image", rclcpp::QoS(10).best_effort());
+	EXPECT_TRUE(waitForSubscriber(bestEffort));
+}

rtabmap_sync/test/test_rgbd_sync.cpp

@@ -0,0 +1,530 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#include "node_test_utils.hpp"
+#include "msg_builders.hpp"
+
+#include <rtabmap_sync/rgbd_sync.hpp>
+
+#include <rtabmap/core/Compression.h>
+
+#include <cmath>
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+
+using namespace rtabmap_sync_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+}
+
+/// Drives rgbd_sync over its three input topics and collects both outputs.
+class RGBDSyncTest : public NodeTest
+{
+protected:
+	/// Starts the node with @p params and wires up the inputs and the raw output.
+	void start(const std::vector<rclcpp::Parameter> & params = {})
+	{
+		node_ = addNode(std::make_shared<rtabmap_sync::RGBDSync>(
+				rclcpp::NodeOptions().parameter_overrides(params)));
+
+		out_ = collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image");
+		rgbPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
+		depthPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
+		infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>(
+				"rgb/camera_info", 10);
+		ASSERT_TRUE(waitForSubscriber(rgbPub_));
+		ASSERT_TRUE(waitForSubscriber(depthPub_));
+		ASSERT_TRUE(waitForSubscriber(infoPub_));
+		ASSERT_TRUE(waitForSubscribedFromNode("rgbd_image"));
+	}
+
+	/// Also subscribes to the compressed output. Call right after start().
+	void collectCompressed()
+	{
+		compressed_ = collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image/compressed");
+		ASSERT_TRUE(waitForSubscribedFromNode("rgbd_image/compressed"));
+	}
+
+	/**
+	 * @brief Waits until the node under test sees a subscriber on @p topic.
+	 *
+	 * Both outputs are published only when subscribed, and it is the node's own view of
+	 * the graph that decides. Waiting on the subscriber's side instead leaves a window
+	 * in which the test is connected but the node does not know it yet, and the first
+	 * frame is silently dropped.
+	 */
+	bool waitForSubscribedFromNode(const std::string & topic)
+	{
+		return spinUntil([&]() { return node_->count_subscribers(topic) > 0; });
+	}
+
+	/// Publishes one set of inputs, letting each carry its own stamp.
+	void publishStamps(double rgbStamp, double depthStamp, double infoStamp)
+	{
+		rgbPub_->publish(makeRgbImage("camera_link", rgbStamp));
+		depthPub_->publish(makeDepthImage("camera_link", depthStamp));
+		infoPub_->publish(makeCameraInfo("camera_link", infoStamp));
+	}
+
+	/// Publishes one hardware-synchronized set: every input carries the same stamp.
+	void publish(double stamp, int width = 8, int height = 8,
+			uint16_t depthMillimeters = 1500)
+	{
+		rgbPub_->publish(makeRgbImage("camera_link", stamp, width, height));
+		depthPub_->publish(
+				makeDepthImage("camera_link", stamp, width, height, depthMillimeters));
+		infoPub_->publish(makeCameraInfo("camera_link", stamp, width, height));
+	}
+
+	/**
+	 * @brief Publishes @p count frames 100 ms apart, with depth trailing color.
+	 *
+	 * The approximate policy cannot emit a pair the moment it arrives: it has to wait
+	 * until a later message proves no better match is coming. Feeding it a stream is
+	 * therefore the only way to observe approximate matching at all.
+	 *
+	 * @param depthOffset seconds added to the depth stamp; color and camera_info share
+	 *                    the frame stamp.
+	 */
+	void publishStream(size_t count, double depthOffset, double start = 1000.0)
+	{
+		for(size_t i=0; i<count; ++i)
+		{
+			const double stamp = start + 0.1*double(i);
+			rgbStamps_.push_back(stamp);
+			depthStamps_.push_back(stamp + depthOffset);
+			rgbPub_->publish(makeRgbImage("camera_link", stamp));
+			depthPub_->publish(makeDepthImage("camera_link", stamp + depthOffset));
+			infoPub_->publish(makeCameraInfo("camera_link", stamp));
+			spinFor(std::chrono::milliseconds(20));
+		}
+	}
+
+	/// True if @p stamp is one of @p stamps, to the nanosecond the stamp was built from.
+	static bool isOneOf(const std::vector<double> & stamps, double stamp)
+	{
+		for(double candidate : stamps)
+		{
+			if(std::fabs(candidate - stamp) < 1e-6)
+			{
+				return true;
+			}
+		}
+		return false;
+	}
+
+	std::shared_ptr<rtabmap_sync::RGBDSync> node_;
+	std::vector<double> rgbStamps_;
+	std::vector<double> depthStamps_;
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out_;
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> compressed_;
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgbPub_;
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depthPub_;
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub_;
+};
+
+TEST_F(RGBDSyncTest, PacksTheThreeInputsIntoOneMessage)
+{
+	start();
+
+	publish(1000.0);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImage & got = out_->back();
+	EXPECT_EQ(got.header.frame_id, "camera_link");
+	EXPECT_DOUBLE_EQ(rclcpp::Time(got.header.stamp).seconds(), 1000.0);
+	EXPECT_EQ(got.rgb.encoding, "bgr8");
+	EXPECT_EQ(got.rgb.width, 8u);
+	EXPECT_EQ(got.depth.encoding, "16UC1");
+	EXPECT_EQ(got.depth.width, 8u);
+	EXPECT_NEAR(got.rgb_camera_info.k[0], 100.0, 1e-9);
+	EXPECT_NEAR(got.depth_camera_info.k[0], 100.0, 1e-9)
+		<< "a single camera_info is copied into both slots";
+	EXPECT_TRUE(got.rgb_compressed.data.empty()) << "the raw output carries raw images";
+	EXPECT_TRUE(got.depth_compressed.data.empty());
+}
+
+TEST_F(RGBDSyncTest, TakesTheFrameIdFromTheCameraInfo)
+{
+	// The images may be stamped in an optical frame while the camera_info names the
+	// frame the calibration is expressed in; the latter is what the output must carry.
+	start();
+
+	rgbPub_->publish(makeRgbImage("camera_rgb_optical_frame", 1000.0));
+	depthPub_->publish(makeDepthImage("camera_depth_optical_frame", 1000.0));
+	infoPub_->publish(makeCameraInfo("camera_link", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_EQ(out_->back().header.frame_id, "camera_link");
+}
+
+TEST_F(RGBDSyncTest, StampsTheOutputWithTheLaterOfTheTwoImages)
+{
+	// Approximate sync pairs frames that are close but not equal. The output stamp is
+	// the later of the two, so the message is never stamped before data it contains.
+	start({rclcpp::Parameter("approx_sync", true)});
+
+	// Depth trails color by 5 ms, so every output must carry its depth frame's stamp.
+	publishStream(/*count=*/5, /*depthOffset=*/0.005);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	for(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr & msg : out_->messages)
+	{
+		const double stamp = rclcpp::Time(msg->header.stamp).seconds();
+		EXPECT_TRUE(isOneOf(depthStamps_, stamp))
+			<< "expected the later (depth) stamp, got " << stamp;
+		EXPECT_FALSE(isOneOf(rgbStamps_, stamp));
+	}
+}
+
+TEST_F(RGBDSyncTest, ApproxSyncPairsFramesWithDifferentStamps)
+{
+	start({rclcpp::Parameter("approx_sync", true)});
+
+	publishStream(/*count=*/5, /*depthOffset=*/0.004);
+	EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); }))
+		<< "approximate sync must pair inputs whose stamps only nearly agree";
+}
+
+TEST_F(RGBDSyncTest, ExactSyncRejectsFramesWithDifferentStamps)
+{
+	start({rclcpp::Parameter("approx_sync", false)});
+
+	publishStamps(/*rgb=*/1000.000, /*depth=*/1000.004, /*info=*/1000.008);
+	spinFor(std::chrono::milliseconds(500));
+	EXPECT_TRUE(out_->empty()) << "exact sync must not pair mismatched stamps";
+
+	// The same node does produce output once the stamps agree exactly.
+	publish(1001.0);
+	EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+}
+
+TEST_F(RGBDSyncTest, ApproxSyncMaxIntervalRejectsDistantFrames)
+{
+	// The guard against silently pairing a stale frame with a fresh one.
+	start({rclcpp::Parameter("approx_sync", true),
+		   rclcpp::Parameter("approx_sync_max_interval", 0.01)});
+
+	// Depth lags by 550 ms. The frames are 100 ms apart, so no depth frame lands within
+	// 10 ms of any color frame -- not even a much older one.
+	publishStream(/*count=*/6, /*depthOffset=*/0.55);
+	spinFor(std::chrono::milliseconds(500));
+	EXPECT_TRUE(out_->empty()) << "no pair is within the 10 ms interval";
+
+	publishStream(/*count=*/6, /*depthOffset=*/0.002, /*start=*/2000.0);
+	EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); }))
+		<< "2 ms apart is within the interval and must still be paired";
+}
+
+TEST_F(RGBDSyncTest, DecimationScalesTheImagesAndTheCalibration)
+{
+	start({rclcpp::Parameter("decimation", 2)});
+
+	publish(1000.0, /*width=*/8, /*height=*/8);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImage & got = out_->back();
+	EXPECT_EQ(got.rgb.width, 4u);
+	EXPECT_EQ(got.rgb.height, 4u);
+	EXPECT_EQ(got.depth.width, 4u);
+	EXPECT_EQ(got.depth.height, 4u);
+	EXPECT_NEAR(got.rgb_camera_info.k[0], 50.0, 1e-6)
+		<< "the focal length must be halved with the image, or the cloud comes out wrong";
+	EXPECT_EQ(got.rgb_camera_info.width, 4u);
+	EXPECT_EQ(got.depth_camera_info.width, 4u);
+}
+
+TEST_F(RGBDSyncTest, DecimationIsDisabledWhenItWouldNotDivideTheDepthImage)
+{
+	// A decimation that does not divide the depth size exactly would misalign depth
+	// against color, so the node gives up on it rather than producing a wrong cloud.
+	start({rclcpp::Parameter("decimation", 3)});
+
+	publish(1000.0, /*width=*/8, /*height=*/8);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_EQ(out_->back().rgb.width, 8u) << "images must be passed through unresized";
+	EXPECT_EQ(out_->back().depth.width, 8u);
+	EXPECT_NEAR(out_->back().rgb_camera_info.k[0], 100.0, 1e-9);
+}
+
+TEST_F(RGBDSyncTest, ADecimationBelowOneIsClampedToOne)
+{
+	start({rclcpp::Parameter("decimation", 0)});
+
+	publish(1000.0);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+	EXPECT_EQ(out_->back().rgb.width, 8u);
+}
+
+TEST_F(RGBDSyncTest, DepthScaleMultipliesTheDepthValues)
+{
+	// For a driver that publishes depth in the wrong unit: 1500 in a 16UC1 image is
+	// 1.5 m only if the unit really is millimeters.
+	start({rclcpp::Parameter("depth_scale", 2.0)});
+
+	publish(1000.0, 8, 8, /*depthMillimeters=*/1500);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImage & got = out_->back();
+	ASSERT_EQ(got.depth.encoding, "16UC1");
+	ASSERT_GE(got.depth.data.size(), 2u);
+	EXPECT_EQ(*reinterpret_cast<const uint16_t *>(got.depth.data.data()), 3000)
+		<< "every depth pixel must be scaled";
+}
+
+TEST_F(RGBDSyncTest, CompressesColorAsJpegAndDepthAsPng)
+{
+	start();
+	collectCompressed();
+
+	publish(1000.0);
+	ASSERT_TRUE(spinUntil([&]() { return !compressed_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImage & got = compressed_->back();
+	EXPECT_FALSE(got.rgb_compressed.data.empty());
+	EXPECT_FALSE(got.depth_compressed.data.empty());
+	EXPECT_EQ(got.depth_compressed.format, "png") << "depth must stay lossless";
+	EXPECT_NE(got.rgb_compressed.format.find("jp"), std::string::npos)
+		<< "expected a jpeg format, got \"" << got.rgb_compressed.format << "\"";
+	EXPECT_TRUE(got.rgb.data.empty()) << "the compressed output carries no raw images";
+	EXPECT_TRUE(got.depth.data.empty());
+	EXPECT_EQ(got.header.frame_id, "camera_link");
+}
+
+TEST_F(RGBDSyncTest, TheCompressedDepthDecompressesBackToTheInput)
+{
+	start();
+	collectCompressed();
+
+	publish(1000.0, 8, 8, /*depthMillimeters=*/1234);
+	ASSERT_TRUE(spinUntil([&]() { return !compressed_->empty(); }));
+
+	const cv::Mat depth =
+			rtabmap::uncompressImage(compressed_->back().depth_compressed.data);
+	ASSERT_FALSE(depth.empty());
+	EXPECT_EQ(depth.type(), CV_16UC1);
+	EXPECT_EQ(depth.cols, 8);
+	EXPECT_EQ(depth.rows, 8);
+	EXPECT_EQ(depth.at<uint16_t>(0, 0), 1234)
+		<< "png is lossless, so the value must survive the round trip exactly";
+}
+
+TEST_F(RGBDSyncTest, CompressedRateThrottlesTheCompressedOutputOnly)
+{
+	// Compression is expensive and the compressed topic usually feeds a slow link, so
+	// it can be published at a lower rate than the raw one.
+	// The throttle is measured against the wall clock, not the message stamps, so the
+	// window has to be long enough that a slow machine still gets all four frames
+	// inside it -- 0.2 Hz gives five seconds for what takes milliseconds when idle.
+	start({rclcpp::Parameter("compressed_rate", 0.2)});
+	collectCompressed();
+
+	for(int i=0; i<4; ++i)
+	{
+		publish(1000.0 + 0.01*double(i));
+		ASSERT_TRUE(spinUntil([&, i]() { return out_->size() == size_t(i+1); }));
+	}
+
+	spinFor(std::chrono::milliseconds(200));
+	EXPECT_EQ(out_->size(), 4u) << "the raw output is never throttled";
+	EXPECT_EQ(compressed_->size(), 1u)
+		<< "only the first of four back-to-back frames may be compressed";
+}
+
+TEST_F(RGBDSyncTest, PublishesEveryFrameCompressedWhenTheRateIsUnset)
+{
+	start();
+	collectCompressed();
+
+	for(int i=0; i<3; ++i)
+	{
+		publish(1000.0 + 0.01*double(i));
+		ASSERT_TRUE(spinUntil([&, i]() { return out_->size() == size_t(i+1); }));
+	}
+
+	// The compressed message is published before the raw one but may be delivered after.
+	spinUntil([&]() { return compressed_->size() == 3u; });
+	EXPECT_EQ(compressed_->size(), 3u) << "compressed_rate 0 means no throttling";
+}
+
+TEST_F(RGBDSyncTest, PublishesBothOutputsWhenBothHaveSubscribers)
+{
+	start();
+	collectCompressed();
+
+	publish(1000.0);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty() && !compressed_->empty(); }));
+
+	EXPECT_FALSE(out_->back().rgb.data.empty());
+	EXPECT_FALSE(compressed_->back().rgb_compressed.data.empty());
+	EXPECT_EQ(out_->back().header.stamp, compressed_->back().header.stamp);
+}
+
+TEST_F(RGBDSyncTest, DoesNotCompressWhenOnlyTheRawOutputIsSubscribed)
+{
+	// Compression is the expensive half of this node; it must not run for nobody.
+	start();
+
+	publish(1000.0);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> late =
+			collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image/compressed");
+	ASSERT_TRUE(waitForPublisher(late->subscription));
+	spinFor(std::chrono::milliseconds(200));
+	EXPECT_TRUE(late->empty()) << "subscribing late must not deliver a back catalogue";
+}
+
+TEST_F(RGBDSyncTest, StaysSilentWithoutAnySubscriber)
+{
+	addNode(std::make_shared<rtabmap_sync::RGBDSync>(rclcpp::NodeOptions()));
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgbPub =
+			helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depthPub =
+			helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub =
+			helper()->create_publisher<sensor_msgs::msg::CameraInfo>("rgb/camera_info", 10);
+	ASSERT_TRUE(waitForSubscriber(rgbPub));
+	ASSERT_TRUE(waitForSubscriber(depthPub));
+	ASSERT_TRUE(waitForSubscriber(infoPub));
+
+	rgbPub->publish(makeRgbImage("camera_link", 1000.0));
+	depthPub->publish(makeDepthImage("camera_link", 1000.0));
+	infoPub->publish(makeCameraInfo("camera_link", 1000.0));
+	spinFor(std::chrono::milliseconds(300));
+
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> late =
+			collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image");
+	spinFor(std::chrono::milliseconds(200));
+	EXPECT_TRUE(late->empty());
+}
+
+TEST_F(RGBDSyncTest, SyncsRepeatedFramesInOrder)
+{
+	start();
+
+	for(int i=0; i<5; ++i)
+	{
+		publish(1000.0 + 0.1*double(i));
+		ASSERT_TRUE(spinUntil([&, i]() { return out_->size() == size_t(i+1); }))
+			<< "frame " << i << " was not synchronized";
+	}
+
+	ASSERT_EQ(out_->size(), 5u);
+	for(size_t i=1; i<out_->size(); ++i)
+	{
+		EXPECT_GT(rclcpp::Time(out_->messages[i]->header.stamp).seconds(),
+				  rclcpp::Time(out_->messages[i-1]->header.stamp).seconds())
+			<< "frames must come out in the order they went in";
+	}
+}
+
+TEST_F(RGBDSyncTest, AcceptsTheDeprecatedQueueSizeParameter)
+{
+	// "queue_size" was renamed to "sync_queue_size"; the old name still has to work.
+	start({rclcpp::Parameter("queue_size", 5)});
+
+	publish(1000.0);
+	EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+}
+
+TEST_F(RGBDSyncTest, PublishesDiagnostics)
+{
+	start();
+
+	std::shared_ptr<Collector<diagnostic_msgs::msg::DiagnosticArray>> diagnostics =
+			collect<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics");
+
+	publish(1000.0);
+	ASSERT_TRUE(spinUntil([&]() { return !diagnostics->empty(); },
+			std::chrono::milliseconds(10000)));
+
+	bool sawInput = false;
+	bool sawOutput = false;
+	for(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr & msg :
+			diagnostics->messages)
+	{
+		for(const diagnostic_msgs::msg::DiagnosticStatus & status : msg->status)
+		{
+			sawInput = sawInput || status.name.find("Input Status") != std::string::npos;
+			sawOutput = sawOutput || status.name.find("Output Status") != std::string::npos;
+		}
+	}
+	EXPECT_TRUE(sawInput) << "the input rate is what tells an operator a topic went quiet";
+	EXPECT_TRUE(sawOutput);
+}
+
+/// QoS of the subscriptions, which has to match the driver or nothing arrives at all.
+///
+/// A reliable subscription refuses to match a best-effort publisher, while a best-effort
+/// subscription matches either. Whether a connection is established at all is therefore
+/// what tells us which reliability the node picked.
+class RGBDSyncQosTest : public NodeTest
+{
+protected:
+	enum Reliability { kSystemDefault = 0, kReliable = 1, kBestEffort = 2 };
+
+	void startSync(const std::vector<rclcpp::Parameter> & params)
+	{
+		addNode(std::make_shared<rtabmap_sync::RGBDSync>(
+				rclcpp::NodeOptions().parameter_overrides(params)));
+	}
+
+	template <typename MsgT>
+	typename rclcpp::Publisher<MsgT>::SharedPtr input(
+			const std::string & topic, Reliability reliability)
+	{
+		rclcpp::QoS qos(10);
+		reliability == kBestEffort ? qos.best_effort() : qos.reliable();
+		return helper()->create_publisher<MsgT>(topic, qos);
+	}
+};
+
+TEST_F(RGBDSyncQosTest, SubscribesBestEffortWhenAsked)
+{
+	// The common case: a camera driver publishing sensor data best effort.
+	startSync({rclcpp::Parameter("qos", int(kBestEffort))});
+
+	EXPECT_TRUE(waitForSubscriber(
+			input<sensor_msgs::msg::Image>("rgb/image", kBestEffort)));
+	EXPECT_TRUE(waitForSubscriber(
+			input<sensor_msgs::msg::CameraInfo>("rgb/camera_info", kBestEffort)));
+}
+
+TEST_F(RGBDSyncQosTest, QosCameraInfoOverridesQosOnTheCameraInfoOnly)
+{
+	// Drivers commonly publish images best effort but camera_info reliable, so the two
+	// have to be settable apart.
+	startSync({rclcpp::Parameter("qos", int(kBestEffort)),
+			   rclcpp::Parameter("qos_camera_info", int(kReliable))});
+
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr bestEffortInfo =
+			input<sensor_msgs::msg::CameraInfo>("rgb/camera_info", kBestEffort);
+	spinFor(std::chrono::milliseconds(500));
+	EXPECT_EQ(bestEffortInfo->get_subscription_count(), 0u)
+		<< "a reliable camera_info subscription must refuse a best-effort publisher";
+
+	EXPECT_TRUE(waitForSubscriber(input<sensor_msgs::msg::Image>("rgb/image", kBestEffort)))
+		<< "the image side must have kept qos";
+}
+
+TEST_F(RGBDSyncQosTest, PublishesWithTheConfiguredReliability)
+{
+	startSync({rclcpp::Parameter("qos", int(kBestEffort))});
+
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> reliable =
+			collect<rtabmap_msgs::msg::RGBDImage>(
+					"rgbd_image", rclcpp::QoS(10).reliable());
+	spinFor(std::chrono::milliseconds(500));
+	EXPECT_EQ(reliable->subscription->get_publisher_count(), 0u)
+		<< "the output must be best effort too, so a reliable consumer cannot match it";
+
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> bestEffort =
+			collect<rtabmap_msgs::msg::RGBDImage>(
+					"rgbd_image", rclcpp::QoS(10).best_effort());
+	EXPECT_TRUE(waitForPublisher(bestEffort->subscription));
+}

rtabmap_sync/test/test_rgbdx_sync.cpp

@@ -0,0 +1,263 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#include "node_test_utils.hpp"
+#include "msg_builders.hpp"
+
+#include <rtabmap_sync/rgbdx_sync.hpp>
+
+#include <rtabmap/utilite/UException.h>
+
+#include <rtabmap_msgs/msg/rgbd_images.hpp>
+
+#include <string>
+#include <vector>
+
+using namespace rtabmap_sync_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+}
+
+/// Drives rgbdx_sync over the rgbd_image0..N topics for a configurable camera count.
+class RGBDXSyncTest : public NodeTest
+{
+protected:
+	/// Starts the node for @p cameras cameras and wires up one publisher per camera.
+	void start(int cameras, const std::vector<rclcpp::Parameter> & extra = {})
+	{
+		std::vector<rclcpp::Parameter> params = extra;
+		params.push_back(rclcpp::Parameter("rgbd_cameras", cameras));
+		addNode(std::make_shared<rtabmap_sync::RGBDXSync>(
+				rclcpp::NodeOptions().parameter_overrides(params)));
+
+		out_ = collect<rtabmap_msgs::msg::RGBDImages>("rgbd_images");
+		for(int i=0; i<cameras; ++i)
+		{
+			pubs_.push_back(helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>(
+					"rgbd_image" + std::to_string(i), 10));
+			ASSERT_TRUE(waitForSubscriber(pubs_.back()));
+		}
+		ASSERT_TRUE(waitForPublisher(out_->subscription));
+	}
+
+	/// Publishes one frame per camera, all carrying @p stamp.
+	void publish(double stamp)
+	{
+		for(size_t i=0; i<pubs_.size(); ++i)
+		{
+			pubs_[i]->publish(makeRGBDImage(
+					"camera" + std::to_string(i) + "_link", stamp, 8, 8,
+					// A distinct color per camera, so the order can be checked.
+					cv::Scalar(double(10*(i+1)), 20, 30)));
+		}
+	}
+
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImages>> out_;
+	std::vector<rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr> pubs_;
+};
+
+TEST_F(RGBDXSyncTest, PacksTwoCamerasIntoOneMessage)
+{
+	start(2);
+
+	publish(1000.0);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImages & got = out_->back();
+	ASSERT_EQ(got.rgbd_images.size(), 2u);
+	EXPECT_EQ(got.header.frame_id, "camera0_link")
+		<< "the container takes the first camera's header";
+	EXPECT_DOUBLE_EQ(rclcpp::Time(got.header.stamp).seconds(), 1000.0);
+	EXPECT_EQ(got.rgbd_images[0].header.frame_id, "camera0_link");
+	EXPECT_EQ(got.rgbd_images[1].header.frame_id, "camera1_link");
+}
+
+TEST_F(RGBDXSyncTest, KeepsTheCamerasInTopicOrder)
+{
+	// Downstream matches each image against a calibration by index, so the order of the
+	// array has to follow the rgbd_imageN numbering and nothing else.
+	start(3);
+
+	publish(1000.0);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImages & got = out_->back();
+	ASSERT_EQ(got.rgbd_images.size(), 3u);
+	for(size_t i=0; i<3; ++i)
+	{
+		ASSERT_FALSE(got.rgbd_images[i].rgb.data.empty());
+		EXPECT_EQ(got.rgbd_images[i].rgb.data[0], uint8_t(10*(i+1)))
+			<< "camera " << i << " is not where it should be";
+	}
+}
+
+TEST_F(RGBDXSyncTest, CarriesTheImagesThroughUnchanged)
+{
+	start(2);
+
+	publish(1000.0);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImage & first = out_->back().rgbd_images[0];
+	EXPECT_EQ(first.rgb.encoding, "bgr8");
+	EXPECT_EQ(first.rgb.width, 8u);
+	EXPECT_EQ(first.depth.encoding, "16UC1");
+	EXPECT_NEAR(first.rgb_camera_info.k[0], 100.0, 1e-9)
+		<< "this node only groups messages; it never touches their content";
+}
+
+TEST_F(RGBDXSyncTest, SupportsUpToEightCameras)
+{
+	start(8);
+
+	publish(1000.0);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+	EXPECT_EQ(out_->back().rgbd_images.size(), 8u);
+}
+
+TEST_F(RGBDXSyncTest, RejectsACameraCountBelowTwo)
+{
+	// One camera needs no grouping at all -- use the RGBDImage topic directly. Saying so
+	// at construction beats starting a node that can never publish.
+	EXPECT_THROW(
+		addNode(std::make_shared<rtabmap_sync::RGBDXSync>(rclcpp::NodeOptions()
+				.parameter_overrides({rclcpp::Parameter("rgbd_cameras", 1)}))),
+		UException);
+}
+
+TEST_F(RGBDXSyncTest, RejectsACameraCountAboveEight)
+{
+	EXPECT_THROW(
+		addNode(std::make_shared<rtabmap_sync::RGBDXSync>(rclcpp::NodeOptions()
+				.parameter_overrides({rclcpp::Parameter("rgbd_cameras", 9)}))),
+		UException);
+}
+
+TEST_F(RGBDXSyncTest, WaitsForEveryCamera)
+{
+	// A set is only published once every camera has contributed: a partial set would
+	// silently drop a camera's field of view from the map.
+	start(3);
+
+	pubs_[0]->publish(makeRGBDImage("camera0_link", 1000.0));
+	pubs_[1]->publish(makeRGBDImage("camera1_link", 1000.0));
+	spinFor(std::chrono::milliseconds(500));
+	EXPECT_TRUE(out_->empty()) << "two of three cameras is not a set";
+
+	pubs_[2]->publish(makeRGBDImage("camera2_link", 1000.0));
+	EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+}
+
+TEST_F(RGBDXSyncTest, ApproxSyncPairsCamerasWithDifferentStamps)
+{
+	// Separate USB cameras never share a stamp, which is why approximate is the default.
+	start(2);
+
+	for(int i=0; i<5; ++i)
+	{
+		const double stamp = 1000.0 + 0.1*double(i);
+		pubs_[0]->publish(makeRGBDImage("camera0_link", stamp));
+		pubs_[1]->publish(makeRGBDImage("camera1_link", stamp + 0.004));
+		spinFor(std::chrono::milliseconds(20));
+	}
+	EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+}
+
+TEST_F(RGBDXSyncTest, ExactSyncRejectsCamerasWithDifferentStamps)
+{
+	start(2, {rclcpp::Parameter("approx_sync", false)});
+
+	pubs_[0]->publish(makeRGBDImage("camera0_link", 1000.000));
+	pubs_[1]->publish(makeRGBDImage("camera1_link", 1000.004));
+	spinFor(std::chrono::milliseconds(500));
+	EXPECT_TRUE(out_->empty());
+
+	publish(1001.0);
+	EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+}
+
+TEST_F(RGBDXSyncTest, ApproxSyncMaxIntervalRejectsDistantFrames)
+{
+	start(2, {rclcpp::Parameter("approx_sync_max_interval", 0.01)});
+
+	for(int i=0; i<6; ++i)
+	{
+		const double stamp = 1000.0 + 0.1*double(i);
+		pubs_[0]->publish(makeRGBDImage("camera0_link", stamp));
+		pubs_[1]->publish(makeRGBDImage("camera1_link", stamp + 0.55));
+		spinFor(std::chrono::milliseconds(20));
+	}
+	spinFor(std::chrono::milliseconds(300));
+	EXPECT_TRUE(out_->empty()) << "no pair is within the 10 ms interval";
+
+	for(int i=0; i<6; ++i)
+	{
+		const double stamp = 2000.0 + 0.1*double(i);
+		pubs_[0]->publish(makeRGBDImage("camera0_link", stamp));
+		pubs_[1]->publish(makeRGBDImage("camera1_link", stamp + 0.002));
+		spinFor(std::chrono::milliseconds(20));
+	}
+	EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+}
+
+TEST_F(RGBDXSyncTest, StampsTheOutputWithTheFirstCamera)
+{
+	// Unlike the two-image nodes, which take the later stamp, this one is a container:
+	// each image keeps its own stamp and the container takes camera 0's.
+	start(2);
+
+	for(int i=0; i<5; ++i)
+	{
+		const double stamp = 1000.0 + 0.1*double(i);
+		pubs_[0]->publish(makeRGBDImage("camera0_link", stamp));
+		pubs_[1]->publish(makeRGBDImage("camera1_link", stamp + 0.005));
+		spinFor(std::chrono::milliseconds(20));
+	}
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImages & got = out_->back();
+	ASSERT_EQ(got.rgbd_images.size(), 2u);
+	EXPECT_EQ(got.header.stamp, got.rgbd_images[0].header.stamp);
+	EXPECT_NE(got.header.stamp, got.rgbd_images[1].header.stamp)
+		<< "the second camera must keep its own stamp";
+}
+
+TEST_F(RGBDXSyncTest, SyncsRepeatedSetsInOrder)
+{
+	start(2);
+
+	for(int i=0; i<5; ++i)
+	{
+		publish(1000.0 + 0.1*double(i));
+		ASSERT_TRUE(spinUntil([&, i]() { return out_->size() == size_t(i+1); }));
+	}
+
+	ASSERT_EQ(out_->size(), 5u);
+	for(size_t i=1; i<out_->size(); ++i)
+	{
+		EXPECT_GT(rclcpp::Time(out_->messages[i]->header.stamp).seconds(),
+				  rclcpp::Time(out_->messages[i-1]->header.stamp).seconds());
+	}
+}
+
+TEST_F(RGBDXSyncTest, AcceptsTheDeprecatedQueueSizeParameter)
+{
+	start(2, {rclcpp::Parameter("queue_size", 5)});
+
+	publish(1000.0);
+	EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+}
+
+TEST_F(RGBDXSyncTest, SubscribesBestEffortWhenAsked)
+{
+	addNode(std::make_shared<rtabmap_sync::RGBDXSync>(rclcpp::NodeOptions()
+			.parameter_overrides({rclcpp::Parameter("qos", 2)})));
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr bestEffort =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>(
+					"rgbd_image0", rclcpp::QoS(10).best_effort());
+	EXPECT_TRUE(waitForSubscriber(bestEffort));
+}

rtabmap_sync/test/test_stereo_sync.cpp

@@ -0,0 +1,323 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#include "node_test_utils.hpp"
+#include "msg_builders.hpp"
+
+#include <rtabmap_sync/stereo_sync.hpp>
+
+#include <string>
+#include <vector>
+
+using namespace rtabmap_sync_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+}
+
+/// Drives stereo_sync over its four input topics and collects both outputs.
+class StereoSyncTest : public NodeTest
+{
+protected:
+	void start(const std::vector<rclcpp::Parameter> & params = {})
+	{
+		node_ = addNode(std::make_shared<rtabmap_sync::StereoSync>(
+				rclcpp::NodeOptions().parameter_overrides(params)));
+
+		out_ = collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image");
+		leftPub_ = helper()->create_publisher<sensor_msgs::msg::Image>(
+				"left/image_rect", 10);
+		rightPub_ = helper()->create_publisher<sensor_msgs::msg::Image>(
+				"right/image_rect", 10);
+		leftInfoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>(
+				"left/camera_info", 10);
+		rightInfoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>(
+				"right/camera_info", 10);
+		ASSERT_TRUE(waitForSubscriber(leftPub_));
+		ASSERT_TRUE(waitForSubscriber(rightPub_));
+		ASSERT_TRUE(waitForSubscriber(leftInfoPub_));
+		ASSERT_TRUE(waitForSubscriber(rightInfoPub_));
+		ASSERT_TRUE(waitForSubscribedFromNode("rgbd_image"));
+	}
+
+	void collectCompressed()
+	{
+		compressed_ = collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image/compressed");
+		ASSERT_TRUE(waitForSubscribedFromNode("rgbd_image/compressed"));
+	}
+
+	/// Waits until the node under test sees a subscriber on @p topic. @see RGBDSyncTest.
+	bool waitForSubscribedFromNode(const std::string & topic)
+	{
+		return spinUntil([&]() { return node_->count_subscribers(topic) > 0; });
+	}
+
+	/// Publishes a hardware-synchronized stereo pair, which is what this node expects.
+	void publish(double stamp, int width = 8, int height = 8)
+	{
+		publishStamps(stamp, stamp, width, height);
+	}
+
+	/// Publishes a pair whose two images carry different stamps.
+	void publishStamps(double leftStamp, double rightStamp,
+			int width = 8, int height = 8)
+	{
+		leftPub_->publish(makeMonoImage("left_frame", leftStamp, width, height, 60));
+		rightPub_->publish(makeMonoImage("right_frame", rightStamp, width, height, 90));
+		leftInfoPub_->publish(makeCameraInfo("left_frame", leftStamp, width, height));
+		// The right camera carries the baseline in P(0,3): -fx * baseline.
+		rightInfoPub_->publish(
+				makeCameraInfo("left_frame", rightStamp, width, height, kBaselineTx));
+	}
+
+	/// P(0,3) of the right camera for a 100 px focal length and a 12 cm baseline.
+	static constexpr double kBaselineTx = -12.0;
+
+	std::shared_ptr<rtabmap_sync::StereoSync> node_;
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out_;
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> compressed_;
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr leftPub_;
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rightPub_;
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr leftInfoPub_;
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rightInfoPub_;
+};
+
+constexpr double StereoSyncTest::kBaselineTx;
+
+TEST_F(StereoSyncTest, PacksTheStereoPairIntoTheRgbAndDepthSlots)
+{
+	// An RGBDImage carrying a stereo pair puts the left image where color goes and the
+	// right image where depth goes; the baseline in the second camera_info is what tells
+	// a consumer to read it that way.
+	start();
+
+	publish(1000.0);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImage & got = out_->back();
+	EXPECT_EQ(got.header.frame_id, "left_frame");
+	EXPECT_DOUBLE_EQ(rclcpp::Time(got.header.stamp).seconds(), 1000.0);
+	ASSERT_FALSE(got.rgb.data.empty());
+	ASSERT_FALSE(got.depth.data.empty());
+	EXPECT_EQ(got.rgb.encoding, "mono8");
+	EXPECT_EQ(got.depth.encoding, "mono8") << "the right image is not depth";
+	EXPECT_EQ(got.rgb.data[0], 60) << "rgb must be the left image";
+	EXPECT_EQ(got.depth.data[0], 90) << "depth must be the right image";
+}
+
+TEST_F(StereoSyncTest, CarriesTheBaselineInTheSecondCameraInfo)
+{
+	start();
+
+	publish(1000.0);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImage & got = out_->back();
+	EXPECT_DOUBLE_EQ(got.rgb_camera_info.p[3], 0.0) << "the left camera is the origin";
+	EXPECT_DOUBLE_EQ(got.depth_camera_info.p[3], kBaselineTx)
+		<< "without the baseline nothing downstream can triangulate";
+}
+
+TEST_F(StereoSyncTest, DefaultsToExactSync)
+{
+	// Stereo pairs come off hardware-triggered sensors, so the default is the exact
+	// policy: it is cheaper and cannot mismatch left with right.
+	start();
+
+	publishStamps(/*left=*/1000.0, /*right=*/1000.004);
+	spinFor(std::chrono::milliseconds(500));
+	EXPECT_TRUE(out_->empty()) << "the default must not pair frames 4 ms apart";
+
+	publish(1001.0);
+	EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+}
+
+TEST_F(StereoSyncTest, ApproxSyncPairsFramesWithDifferentStamps)
+{
+	// For a pair of free-running cameras, which is what approx_sync is there for.
+	start({rclcpp::Parameter("approx_sync", true)});
+
+	for(int i=0; i<5; ++i)
+	{
+		const double stamp = 1000.0 + 0.1*double(i);
+		publishStamps(stamp, stamp + 0.004);
+		spinFor(std::chrono::milliseconds(20));
+	}
+	EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+}
+
+TEST_F(StereoSyncTest, StampsTheOutputWithTheLaterOfTheTwoImages)
+{
+	start({rclcpp::Parameter("approx_sync", true)});
+
+	std::vector<double> rightStamps;
+	for(int i=0; i<5; ++i)
+	{
+		const double stamp = 1000.0 + 0.1*double(i);
+		rightStamps.push_back(stamp + 0.005);
+		publishStamps(stamp, stamp + 0.005);
+		spinFor(std::chrono::milliseconds(20));
+	}
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	for(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr & msg : out_->messages)
+	{
+		const double stamp = rclcpp::Time(msg->header.stamp).seconds();
+		bool matched = false;
+		for(double candidate : rightStamps)
+		{
+			matched = matched || std::fabs(candidate - stamp) < 1e-6;
+		}
+		EXPECT_TRUE(matched) << "expected the later (right) stamp, got " << stamp;
+	}
+}
+
+TEST_F(StereoSyncTest, ApproxSyncMaxIntervalRejectsDistantFrames)
+{
+	start({rclcpp::Parameter("approx_sync", true),
+		   rclcpp::Parameter("approx_sync_max_interval", 0.01)});
+
+	// The right camera lags by 550 ms; the frames are 100 ms apart, so nothing lands
+	// within the interval, not even an older frame.
+	for(int i=0; i<6; ++i)
+	{
+		const double stamp = 1000.0 + 0.1*double(i);
+		publishStamps(stamp, stamp + 0.55);
+		spinFor(std::chrono::milliseconds(20));
+	}
+	spinFor(std::chrono::milliseconds(300));
+	EXPECT_TRUE(out_->empty());
+
+	for(int i=0; i<6; ++i)
+	{
+		const double stamp = 2000.0 + 0.1*double(i);
+		publishStamps(stamp, stamp + 0.002);
+		spinFor(std::chrono::milliseconds(20));
+	}
+	EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+}
+
+TEST_F(StereoSyncTest, CompressesBothImagesAsJpeg)
+{
+	// Both halves of a stereo pair are ordinary images, so both take the lossy path --
+	// unlike rgbd_sync, where depth has to stay lossless.
+	start();
+	collectCompressed();
+
+	publish(1000.0);
+	ASSERT_TRUE(spinUntil([&]() { return !compressed_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImage & got = compressed_->back();
+	ASSERT_FALSE(got.rgb_compressed.data.empty());
+	ASSERT_FALSE(got.depth_compressed.data.empty());
+	EXPECT_NE(got.rgb_compressed.format.find("jp"), std::string::npos)
+		<< "expected a jpeg format, got \"" << got.rgb_compressed.format << "\"";
+	EXPECT_NE(got.depth_compressed.format.find("jp"), std::string::npos)
+		<< "expected a jpeg format, got \"" << got.depth_compressed.format << "\"";
+	EXPECT_NE(got.depth_compressed.format, "png")
+		<< "the right image must not take the lossless depth path";
+	EXPECT_TRUE(got.rgb.data.empty()) << "the compressed output carries no raw images";
+	EXPECT_TRUE(got.depth.data.empty());
+	EXPECT_DOUBLE_EQ(got.depth_camera_info.p[3], kBaselineTx)
+		<< "the calibration must survive compression";
+}
+
+TEST_F(StereoSyncTest, CompressedRateThrottlesTheCompressedOutputOnly)
+{
+	// A long window (0.2 Hz = five seconds); the throttle runs off the wall clock, so a
+	// slow machine must not spill the four frames into a second one.
+	start({rclcpp::Parameter("compressed_rate", 0.2)});
+	collectCompressed();
+
+	for(int i=0; i<4; ++i)
+	{
+		publish(1000.0 + 0.01*double(i));
+		ASSERT_TRUE(spinUntil([&, i]() { return out_->size() == size_t(i+1); }));
+	}
+
+	spinFor(std::chrono::milliseconds(200));
+	EXPECT_EQ(out_->size(), 4u) << "the raw output is never throttled";
+	EXPECT_EQ(compressed_->size(), 1u);
+}
+
+TEST_F(StereoSyncTest, StaysSilentWithoutASubscriber)
+{
+	addNode(std::make_shared<rtabmap_sync::StereoSync>(rclcpp::NodeOptions()));
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr leftPub =
+			helper()->create_publisher<sensor_msgs::msg::Image>("left/image_rect", 10);
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rightPub =
+			helper()->create_publisher<sensor_msgs::msg::Image>("right/image_rect", 10);
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr leftInfo =
+			helper()->create_publisher<sensor_msgs::msg::CameraInfo>("left/camera_info", 10);
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rightInfo =
+			helper()->create_publisher<sensor_msgs::msg::CameraInfo>("right/camera_info", 10);
+	ASSERT_TRUE(waitForSubscriber(leftPub));
+	ASSERT_TRUE(waitForSubscriber(rightPub));
+	ASSERT_TRUE(waitForSubscriber(leftInfo));
+	ASSERT_TRUE(waitForSubscriber(rightInfo));
+
+	leftPub->publish(makeMonoImage("left_frame", 1000.0));
+	rightPub->publish(makeMonoImage("right_frame", 1000.0));
+	leftInfo->publish(makeCameraInfo("left_frame", 1000.0));
+	rightInfo->publish(makeCameraInfo("left_frame", 1000.0, 8, 8, kBaselineTx));
+	spinFor(std::chrono::milliseconds(300));
+
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> late =
+			collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image");
+	spinFor(std::chrono::milliseconds(200));
+	EXPECT_TRUE(late->empty());
+}
+
+TEST_F(StereoSyncTest, SyncsRepeatedPairsInOrder)
+{
+	start();
+
+	for(int i=0; i<5; ++i)
+	{
+		publish(1000.0 + 0.1*double(i));
+		ASSERT_TRUE(spinUntil([&, i]() { return out_->size() == size_t(i+1); }));
+	}
+
+	ASSERT_EQ(out_->size(), 5u);
+	for(size_t i=1; i<out_->size(); ++i)
+	{
+		EXPECT_GT(rclcpp::Time(out_->messages[i]->header.stamp).seconds(),
+				  rclcpp::Time(out_->messages[i-1]->header.stamp).seconds());
+	}
+}
+
+TEST_F(StereoSyncTest, AcceptsColorInputToo)
+{
+	// A color stereo pair is just as valid; the encoding is carried through untouched.
+	start();
+
+	leftPub_->publish(makeRgbImage("left_frame", 1000.0));
+	rightPub_->publish(makeRgbImage("left_frame", 1000.0));
+	leftInfoPub_->publish(makeCameraInfo("left_frame", 1000.0));
+	rightInfoPub_->publish(makeCameraInfo("left_frame", 1000.0, 8, 8, kBaselineTx));
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_EQ(out_->back().rgb.encoding, "bgr8");
+	EXPECT_EQ(out_->back().depth.encoding, "bgr8");
+}
+
+TEST_F(StereoSyncTest, AcceptsTheDeprecatedQueueSizeParameter)
+{
+	start({rclcpp::Parameter("queue_size", 5)});
+
+	publish(1000.0);
+	EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+}
+
+TEST_F(StereoSyncTest, SubscribesBestEffortWhenAsked)
+{
+	addNode(std::make_shared<rtabmap_sync::StereoSync>(rclcpp::NodeOptions()
+			.parameter_overrides({rclcpp::Parameter("qos", 2)})));
+
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr bestEffort =
+			helper()->create_publisher<sensor_msgs::msg::Image>(
+					"left/image_rect", rclcpp::QoS(10).best_effort());
+	EXPECT_TRUE(waitForSubscriber(bestEffort));
+}

rtabmap_sync/test/test_sync_diagnostic.cpp

@@ -0,0 +1,301 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#include "node_test_utils.hpp"
+#include "msg_builders.hpp"
+
+#include <rtabmap_sync/SyncDiagnostic.h>
+
+#include <rtabmap/utilite/UException.h>
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+
+#include <memory>
+#include <string>
+
+using namespace rtabmap_sync_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+
+/// A DiagnosticTask that only exists to be recognized by name in the output.
+class NamedTask : public diagnostic_updater::DiagnosticTask
+{
+public:
+	explicit NamedTask(const std::string & name) : DiagnosticTask(name) {}
+	void run(diagnostic_updater::DiagnosticStatusWrapper & stat) override
+	{
+		stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "reporting for duty");
+	}
+};
+}  // namespace
+
+/**
+ * @brief Drives a SyncDiagnostic directly and reads what it publishes on /diagnostics.
+ *
+ * The class is what every node in this package reports through: it watches the rate of
+ * the messages going into a synchronizer and the rate coming out, so that "the map
+ * stopped updating" can be told apart from "one camera went quiet".
+ */
+class SyncDiagnosticTest : public NodeTest
+{
+protected:
+	/**
+	 * @brief Creates and initializes the diagnostic, then starts spinning its node.
+	 *
+	 * The node joins the executor only once the diagnostic has created its publisher and
+	 * its timers, and /diagnostics is subscribed only after that. Everything the tests
+	 * then see is a periodic update; the one-off "Node starting up" notices the updater
+	 * emits as each task is added are over with before anyone is listening.
+	 */
+	void start(const std::string & topic,
+			double tolerance = 0.5, int windowSize = 5,
+			std::vector<diagnostic_updater::DiagnosticTask*> otherTasks = {})
+	{
+		node_ = std::make_shared<rclcpp::Node>("sync_diagnostic_test_node");
+		diagnostic_ = std::make_unique<rtabmap_sync::SyncDiagnostic>(
+				node_.get(), tolerance, windowSize);
+		diagnostic_->init(topic, "nothing received", otherTasks);
+		addNode(node_);
+
+		out_ = collect<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics");
+		ASSERT_TRUE(waitForPublisher(out_->subscription));
+	}
+
+	void TearDown() override
+	{
+		diagnostic_.reset();
+		node_.reset();
+		NodeTest::TearDown();
+	}
+
+	/// The most recent status named @p name, or nullptr if none was ever published.
+	const diagnostic_msgs::msg::DiagnosticStatus * latest(const std::string & name) const
+	{
+		const diagnostic_msgs::msg::DiagnosticStatus * found = nullptr;
+		for(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr & msg :
+				out_->messages)
+		{
+			for(const diagnostic_msgs::msg::DiagnosticStatus & status : msg->status)
+			{
+				if(status.name.find(name) != std::string::npos)
+				{
+					found = &status;
+				}
+			}
+		}
+		return found;
+	}
+
+	/// Spins until a status named @p name has been published at least once.
+	bool waitForStatus(const std::string & name)
+	{
+		return spinUntil([&]() { return latest(name) != nullptr; },
+				std::chrono::milliseconds(10000));
+	}
+
+	/**
+	 * @brief Ticks the input at @p hertz in real time, with stamps advancing to match.
+	 *
+	 * Both halves matter: the target rate is learned from the gaps between stamps, while
+	 * the rate that is checked against it is measured off the wall clock.
+	 */
+	void tickInputFor(int count, double hertz)
+	{
+		const double period = 1.0/hertz;
+		const double start = nowSeconds();
+		for(int i=0; i<count; ++i)
+		{
+			diagnostic_->tickInput(stampOf(start + period*double(i)));
+			spinFor(std::chrono::milliseconds(int(period*1000.0)));
+		}
+	}
+
+	/**
+	 * @brief The node's clock, which is where the stamps in these tests start.
+	 *
+	 * Each status also carries a TimeStampStatus, which fails a stamp more than a few
+	 * seconds away from now -- the diagnostic for the unsynchronized-clock case. Stamps
+	 * out of a fixed epoch would trip it and mask whatever the test was about.
+	 */
+	double nowSeconds() const { return node_->now().seconds(); }
+
+	rclcpp::Node::SharedPtr node_;
+	std::unique_ptr<rtabmap_sync::SyncDiagnostic> diagnostic_;
+	std::shared_ptr<Collector<diagnostic_msgs::msg::DiagnosticArray>> out_;
+};
+
+TEST_F(SyncDiagnosticTest, PublishesAnInputAndAnOutputStatus)
+{
+	// Two statuses, not one: a node can be receiving everything it asked for and still
+	// publish nothing, and the pair is what tells those apart.
+	start("/camera/rgb/image");
+
+	ASSERT_TRUE(waitForStatus("Input Status"));
+	EXPECT_TRUE(waitForStatus("Output Status"));
+}
+
+TEST_F(SyncDiagnosticTest, DerivesTheHardwareIdFromTheTopic)
+{
+	// The last two segments of an image topic are the image and its side, so dropping
+	// them leaves the device: /back_camera/left/image belongs to "back_camera".
+	start("/back_camera/left/image");
+
+	ASSERT_TRUE(waitForStatus("Input Status"));
+	EXPECT_EQ(latest("Input Status")->hardware_id, "back_camera");
+}
+
+TEST_F(SyncDiagnosticTest, KeepsTheNamespaceOfADeeperTopic)
+{
+	start("/robot/front_camera/rgb/image_raw");
+
+	ASSERT_TRUE(waitForStatus("Input Status"));
+	EXPECT_EQ(latest("Input Status")->hardware_id, "robot/front_camera");
+}
+
+TEST_F(SyncDiagnosticTest, ReportsNoHardwareIdWhenThereIsNoTopicToNameIt)
+{
+	// The nodes that synchronize several topics at once pass an empty name, because no
+	// single one of them identifies the device.
+	start("");
+
+	ASSERT_TRUE(waitForStatus("Input Status"));
+	EXPECT_EQ(latest("Input Status")->hardware_id, "none");
+}
+
+TEST_F(SyncDiagnosticTest, AddsTheTasksItIsHandedAlongsideItsOwn)
+{
+	// rtabmap_slam adds its own task this way, so that the rate and the SLAM state come
+	// out in one /diagnostics message instead of two.
+	NamedTask task("Extra Task");
+	start("/camera/rgb/image", 0.5, 5, {&task});
+
+	ASSERT_TRUE(waitForStatus("Extra Task"));
+	EXPECT_EQ(latest("Extra Task")->message, "reporting for duty");
+	EXPECT_TRUE(waitForStatus("Input Status")) << "its own tasks must still be there";
+}
+
+TEST_F(SyncDiagnosticTest, ReportsAnErrorBeforeAnythingHasArrived)
+{
+	// A node that has never received a message is the failure this exists to surface.
+	start("/camera/rgb/image");
+
+	ASSERT_TRUE(waitForStatus("Input Status"));
+	EXPECT_NE(latest("Input Status")->level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+}
+
+TEST_F(SyncDiagnosticTest, LearnsTheRateFromTheStampsAndReportsOkAtThatRate)
+{
+	start("/camera/rgb/image", /*tolerance=*/0.5, /*windowSize=*/5);
+
+	// 20 Hz, with the stamps advancing 50 ms per tick to match.
+	tickInputFor(/*count=*/25, /*hertz=*/20.0);
+
+	ASSERT_TRUE(waitForStatus("Input Status"));
+	EXPECT_EQ(latest("Input Status")->level, diagnostic_msgs::msg::DiagnosticStatus::OK)
+		<< "status was: " << latest("Input Status")->message;
+}
+
+TEST_F(SyncDiagnosticTest, ComplainsOnceAKnownInputGoesQuiet)
+{
+	start("/camera/rgb/image", /*tolerance=*/0.5, /*windowSize=*/5);
+
+	tickInputFor(/*count=*/25, /*hertz=*/20.0);
+	ASSERT_TRUE(waitForStatus("Input Status"));
+	ASSERT_EQ(latest("Input Status")->level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+	// The camera stops. The learned rate stays, so the measured one now falls short.
+	// The updater is built with a 2 s period, so this has to span more than one of them.
+	out_->messages.clear();
+	spinFor(std::chrono::milliseconds(3000));
+
+	ASSERT_NE(latest("Input Status"), nullptr);
+	EXPECT_NE(latest("Input Status")->level, diagnostic_msgs::msg::DiagnosticStatus::OK)
+		<< "a silent camera must not keep reporting OK";
+}
+
+TEST_F(SyncDiagnosticTest, TheOutputStatusFollowsTheInputRateByDefault)
+{
+	// A synchronizer that drops nothing publishes as fast as it receives, so the input
+	// rate is the right expectation for the output side until told otherwise.
+	start("/camera/rgb/image", /*tolerance=*/0.5, /*windowSize=*/5);
+
+	const double period = 1.0/20.0;
+	const double start = nowSeconds();
+	for(int i=0; i<25; ++i)
+	{
+		const rclcpp::Time stamp = stampOf(start + period*double(i));
+		diagnostic_->tickInput(stamp);
+		diagnostic_->tickOutput(stamp);
+		spinFor(std::chrono::milliseconds(50));
+	}
+
+	ASSERT_TRUE(waitForStatus("Output Status"));
+	EXPECT_EQ(latest("Output Status")->level, diagnostic_msgs::msg::DiagnosticStatus::OK)
+		<< "status was: " << latest("Output Status")->message;
+}
+
+TEST_F(SyncDiagnosticTest, AnOutputSlowerThanItsInputIsReported)
+{
+	// The case worth catching: everything arrives, but the node only manages to produce
+	// half of it -- a dropped frame is invisible on the input side alone.
+	start("/camera/rgb/image", /*tolerance=*/0.5, /*windowSize=*/5);
+
+	// Both sides declare 20 Hz, but only every fourth frame makes it out.
+	const double period = 1.0/20.0;
+	const double start = nowSeconds();
+	for(int i=0; i<25; ++i)
+	{
+		const rclcpp::Time stamp = stampOf(start + period*double(i));
+		diagnostic_->tickInput(stamp, /*expectedFrequency=*/20.0);
+		if(i % 4 == 0)
+		{
+			diagnostic_->tickOutput(stamp, /*expectedFrequency=*/20.0);
+		}
+		spinFor(std::chrono::milliseconds(50));
+	}
+
+	ASSERT_TRUE(waitForStatus("Output Status"));
+	EXPECT_NE(latest("Output Status")->level, diagnostic_msgs::msg::DiagnosticStatus::OK)
+		<< "status was: " << latest("Output Status")->message;
+	EXPECT_EQ(latest("Input Status")->level, diagnostic_msgs::msg::DiagnosticStatus::OK)
+		<< "the input side is healthy and must say so";
+}
+
+TEST_F(SyncDiagnosticTest, AnExplicitRateOverridesTheLearnedOne)
+{
+	// A node that knows its own target rate -- a throttled or decimated output -- says
+	// so rather than letting the stamps imply a rate it was never going to reach.
+	start("/camera/rgb/image", /*tolerance=*/0.5, /*windowSize=*/5);
+
+	// Ticking at 5 Hz while declaring 5 Hz is fine, even though the stamps say 20 Hz.
+	const double start = nowSeconds();
+	for(int i=0; i<10; ++i)
+	{
+		diagnostic_->tickInput(stampOf(start + 0.05*double(i)), /*expectedFrequency=*/5.0);
+		spinFor(std::chrono::milliseconds(200));
+	}
+
+	ASSERT_TRUE(waitForStatus("Input Status"));
+	EXPECT_EQ(latest("Input Status")->level, diagnostic_msgs::msg::DiagnosticStatus::OK)
+		<< "status was: " << latest("Input Status")->message;
+}
+
+TEST_F(SyncDiagnosticTest, RejectsAWindowSizeBelowOne)
+{
+	// The window is averaged over, so an empty one would divide by zero.
+	rclcpp::Node::SharedPtr node =
+			addNode(std::make_shared<rclcpp::Node>("sync_diagnostic_bad_window"));
+	EXPECT_THROW(
+		rtabmap_sync::SyncDiagnostic(node.get(), 0.2, /*windowSize=*/0),
+		UException);
+}
+
+TEST_F(SyncDiagnosticTest, ASingleSampleWindowIsAccepted)
+{
+	rclcpp::Node::SharedPtr node =
+			addNode(std::make_shared<rclcpp::Node>("sync_diagnostic_small_window"));
+	EXPECT_NO_THROW(rtabmap_sync::SyncDiagnostic(node.get(), 0.2, /*windowSize=*/1));
+}

rtabmap_util/CMakeLists.txt

@@ -308,8 +308,17 @@ if(BUILD_TESTING)
 
   # Each node gets its own test binary: a crash or a stuck executor in one node cannot
   # take the others down, and every binary starts with a clean DDS graph.
+  #
+  # Each binary also gets its own DDS domain. colcon tests packages in parallel and ctest
+  # can run these binaries in parallel, while these suites share topic names -- rgb/image,
+  # rgbd_image, odom -- with rtabmap_sync's. On a shared domain they discover each other's
+  # publishers, and assertions then see traffic the test never sent. rtabmap_sync numbers
+  # its own from 50; keep the two ranges apart.
+  set(rtabmap_util_test_domain_id 30)
   macro(rtabmap_util_add_node_test test_name)
-    ament_add_gtest(${test_name} test/${test_name}.cpp)
+    ament_add_gtest(${test_name} test/${test_name}.cpp
+      ENV ROS_DOMAIN_ID=${rtabmap_util_test_domain_id})
+    math(EXPR rtabmap_util_test_domain_id "${rtabmap_util_test_domain_id} + 1")
     if(TARGET ${test_name})
       target_include_directories(${test_name} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test)
       target_link_libraries(${test_name} rtabmap_util_plugins rtabmap_util)

rtabmap_util/README.md

@@ -58,9 +58,3 @@ A few things recur across these nodes.
 **`fixed_frame_id`.** Where a node has to account for the robot moving between two stamps, it does so by asking TF how a frame moved relative to a fixed one — usually `odom`. Leaving it empty disables the compensation rather than erroring, so a moving robot then gets subtly misplaced data.
 
 **`Grid/*` parameters.** Nodes that segment or assemble maps use RTAB-Map's own [`LocalGridMaker`](https://introlab.github.io/rtabmap/api/latest/classrtabmap_1_1LocalGridMaker.html), and expose its parameters directly under their RTAB-Map names. Their meanings and defaults are in RTAB-Map's [parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html), which is the source of truth for them. One to know about: `Grid/RangeMax` is not unlimited by default, so distant points are dropped before anything else happens.
-
-## Building the documentation
-
-```bash
-rosdoc2 build --package-path rtabmap_util --output-directory doc_output
-```

rtabmap_util/doc/imu_to_tf.md

@@ -91,15 +91,45 @@ Node(
     remappings=[('scan_cloud', '/lidar/points/deskewed')])
 ```
 
-The three nodes chain into a single TF tree:
+The three nodes chain into a single TF tree. In terms of data, this node's job is to turn the IMU's orientation into a *frame* that `lidar_deskewing` and `icp_odometry` can look up — while the IMU topic itself still goes straight to the SLAM nodes, which use it for their own purposes:
 
-```text
+```mermaid
-map                                rtabmap
+flowchart LR
-└── icp_odom                       icp_odometry
+    IMU["IMU driver"]
-    └── base_link_stabilized       imu_to_tf
+    IMUT(["imu/data"])
-        └── base_link
+    I2T["imu_to_tf"]
-            ├── lidar_link         robot description (static)
+    LIDAR["lidar driver"]
-            └── imu_link
+    DESKEW["lidar_deskewing"]
+    ICP["icp_odometry"]
+    MAP["rtabmap"]
+    TF(["tf: base_link_stabilized"])
+    DESKEWED(["/lidar/points/deskewed"])
+    IMU --> IMUT
+    IMUT --> I2T & ICP & MAP
+    I2T --> TF
+    LIDAR -->|/lidar/points| DESKEW
+    DESKEW --> DESKEWED
+    DESKEWED -->|scan_cloud| ICP & MAP
+    ICP -->|odom| MAP
+    TF -.-> DESKEW
+    TF -.-> ICP
+```
+
+And the frames themselves:
+
+```mermaid
+flowchart TB
+    MAP("map")
+    ICPODOM("icp_odom")
+    STAB("base_link_stabilized")
+    BASE("base_link")
+    LIDAR("lidar_link")
+    IMULINK("imu_link")
+    MAP -->|rtabmap| ICPODOM
+    ICPODOM -->|icp_odometry| STAB
+    STAB -->|imu_to_tf| BASE
+    BASE -->|robot description| LIDAR
+    BASE -->|robot description| IMULINK
 ```
 
 | Edge | Published by |

rtabmap_util/doc/map_assembler.md

@@ -24,6 +24,23 @@ ComposableNode(
                  'cloud_output_voxelized': True}])
 ```
 
+The graph comes from the SLAM node; the maps are built here, off its critical path. Nothing forces the split across machines — a second process on the robot works too — but only `mapData` crosses the boundary, so putting the assembling on a workstation keeps the heavy topics off the link as well as off the robot's CPU:
+
+```mermaid
+flowchart LR
+    subgraph ROBOT["robot"]
+        SLAM["rtabmap"]
+    end
+    subgraph REMOTE["remote computer"]
+        ASM["map_assembler"]
+        RVIZ["RViz"]
+    end
+    SLAM -->|mapData| ASM
+    ASM -->|cloud_map| RVIZ
+    ASM -->|map| RVIZ
+    ASM -->|octomap_binary| RVIZ
+```
+
 ## Subscribed Topics
 
 | Topic | Type | Description |

rtabmap_util/doc/obstacles_detection.md

@@ -89,6 +89,20 @@ The `marking`/`clearing` split is the whole point. Ground points only clear: the
 
 Feeding the raw cloud in as a single source cannot do this — every floor point would mark an obstacle and the robot would refuse to move. Working from [`turtlebot3_rgbd.launch.py`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd.launch.py) and its [nav2 parameters](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/params/turtlebot3_rgbd_nav2_params.yaml) will save some time.
 
+A depth camera turned into ground and obstacle clouds for a costmap:
+
+```mermaid
+flowchart LR
+    CAM["camera driver"]
+    XYZ["point_cloud_xyz"]
+    OBST["obstacles_detection<br>frame_id: base_link"]
+    NAV["nav2 costmap"]
+    CAM -->|"depth/image,<br>camera_info"| XYZ
+    XYZ -->|cloud| OBST
+    OBST -->|ground| NAV
+    OBST -->|obstacles| NAV
+```
+
 ## Subscribed Topics
 
 | Topic | Type | Description |

rtabmap_util/doc/point_cloud_aggregator.md

@@ -33,6 +33,27 @@ With 2D or 3D lidars, feed the aggregator **deskewed** clouds: run a [lidar_desk
 
 The two nodes correct different motions and you generally want both. Deskewing removes the distortion *within* each sweep, point by point, because a spinning lidar measures each point from a slightly different pose. `fixed_frame_id` here places whole clouds relative to each other, because the sensors did not fire at the same instant. Merging raw sweeps only merges their distortions.
 
+One deskewing node per sensor, then this node, and optionally back to a `LaserScan`. Every stage that compensates motion needs the same fixed frame:
+
+```mermaid
+flowchart LR
+    L0["lidar_front driver"]
+    L1["lidar_left driver"]
+    L2["lidar_right driver"]
+    D0["lidar_deskewing<br>fixed_frame_id: odom"]
+    D1["lidar_deskewing<br>fixed_frame_id: odom"]
+    D2["lidar_deskewing<br>fixed_frame_id: odom"]
+    AGG["point_cloud_aggregator<br>count: 3<br>frame_id: base_link<br>fixed_frame_id: odom"]
+    SCAN["pointcloud_to_laserscan<br>optional"]
+    L0 -->|/lidar_front/points| D0
+    L1 -->|/lidar_left/points| D1
+    L2 -->|/lidar_right/points| D2
+    D0 -->|cloud1| AGG
+    D1 -->|cloud2| AGG
+    D2 -->|cloud3| AGG
+    AGG -->|combined_cloud| SCAN
+```
+
 ## Subscribed Topics
 
 | Topic | Type | Description |

rtabmap_util/doc/point_cloud_assembler.md

@@ -44,6 +44,26 @@ Node(
                 ('odom', 'icp_odom')]),
 ```
 
+The deskewing and `icp_odometry` both measure motion against the `odom` frame, while the assembler takes the pose from the `icp_odom` topic instead; the assembled cloud, not the raw sweep, is what `rtabmap` stores:
+
+```mermaid
+flowchart LR
+    LIDAR["lidar driver"]
+    DESKEW["lidar_deskewing<br>fixed_frame_id: odom"]
+    ICP["icp_odometry<br>guess_frame_id: odom"]
+    ASM["point_cloud_assembler<br>fixed_frame_id: ''"]
+    MAP["rtabmap"]
+    DESKEWED(["deskewed cloud"])
+    ICPODOM(["icp_odom"])
+    LIDAR -->|points| DESKEW
+    DESKEW --> DESKEWED
+    DESKEWED -->|scan_cloud| ICP
+    DESKEWED -->|cloud| ASM
+    ICP --> ICPODOM
+    ICPODOM -->|odom| ASM & MAP
+    ASM -->|assembled_cloud| MAP
+```
+
 Note `fixed_frame_id: ''`. Clearing it switches the node from TF to the `odom` topic, pairing each cloud with the exact odometry message that goes with it rather than an interpolated TF lookup — see [Where the poses come from](#where-the-poses-come-from). Feeding the result to `rtabmap` as `scan_cloud` means the assembled cloud, not the raw sweep, is what gets stored.
 
 ### Widening a narrow field of view
@@ -62,10 +82,24 @@ Node(
     remappings=[('cloud', '/camera/scan/deskewed')]),
 ```
 
+Here the pose comes from the robot's wheel odometry, through the `odom` frame in TF. Nothing is being deskewed — `lidar_deskewing` is in the chain purely because this node takes `PointCloud2` and `depthimage_to_laserscan` emits a `LaserScan`:
+
+```mermaid
+flowchart LR
+    D2S["depthimage_to_laserscan"]
+    CONV["lidar_deskewing<br>LaserScan → PointCloud2"]
+    ASM["point_cloud_assembler<br>circular_buffer<br>max_clouds: 20<br>frame_id: base_link<br>fixed_frame_id: odom"]
+    MAP["rtabmap"]
+    D2S -->|input_scan| CONV
+    CONV -->|cloud| ASM
+    ASM -->|assembled_cloud| MAP
+```
+
 `circular_buffer` is what makes this work as a live input: the window rolls, so every incoming scan produces a full assembled cloud rather than one per twenty. `linear_update` and `angular_update` stop a stationary robot from filling the buffer with twenty copies of the same view, which would leave it with nothing but the current scan the moment it moved off again.
 
 The cloud goes to `rtabmap` as `scan_cloud`, with `scan_cloud_is_2d` set since the points all came from one row of pixels.
 
+
 ## Subscribed Topics
 
 | Topic | Type | Description |

rtabmap_util/doc/pointcloud_to_depthimage.md

@@ -28,6 +28,23 @@ ComposableNode(
                 ('camera_info', '/camera/color/camera_info')])
 ```
 
+The lidar supplies the geometry, the camera supplies the pose and the color, and the result joins an ordinary RGB-D pipeline:
+
+```mermaid
+flowchart LR
+    LIDAR["lidar driver"]
+    CAM["camera driver"]
+    P2D["pointcloud_to_depthimage<br>fixed_frame_id: odom"]
+    SYNC["rgbd_sync"]
+    MAP["rtabmap"]
+    LIDAR -->|cloud| P2D
+    CAM -->|camera_info| P2D
+    CAM -->|rgb/image| SYNC
+    CAM -->|rgb/camera_info| SYNC
+    P2D -->|image_raw as depth/image| SYNC
+    SYNC -->|rgbd_image| MAP
+```
+
 ## Subscribed Topics
 
 | Topic | Type | Description |

rtabmap_util/doc/rgbd_relay.md

@@ -25,6 +25,32 @@ ComposableNode(
     remappings=[('rgbd_image', '/camera/rgbd_image')])
 ```
 
+
+A relay at each end of the link, so only the compressed form crosses it. Once it is raw again it feeds the SLAM node directly, and [rgbd_split](rgbd_split.md) unpacks it into the plain `Image` topics RViz can display:
+
+```mermaid
+flowchart LR
+    subgraph ROBOT["robot"]
+        CAM["camera driver"]
+        SYNC["rgbd_sync"]
+        RELAY1["rgbd_relay<br>compress: true"]
+    end
+    subgraph REMOTE["remote computer"]
+        RELAY2["rgbd_relay<br>uncompress: true"]
+        RAW(["rgbd_image_relay"])
+        MAP["rtabmap"]
+        SPLIT["rgbd_split"]
+        RVIZ["RViz"]
+    end
+    CAM -->|"rgb, depth,<br>camera_info"| SYNC
+    SYNC -->|rgbd_image| RELAY1
+    RELAY1 -->|compressed| RELAY2
+    RELAY2 --> RAW
+    RAW --> MAP
+    RAW --> SPLIT
+    SPLIT -->|rgb, depth| RVIZ
+```
+
 ## Subscribed Topics
 
 | Topic | Type | Description |
@@ -62,6 +88,17 @@ ros2 run rtabmap_util rgbd_relay --ros-args \
   -p qos_pub:=1
 ```
 
+```mermaid
+flowchart LR
+    CAM["camera driver<br>publishes best effort"]
+    SYNC["rgbd_sync"]
+    RELAY["rgbd_relay<br>qos_sub: 2, qos_pub: 1"]
+    MAP["rtabmap<br>needs reliable"]
+    CAM -->|"rgb, depth,<br>camera_info"| SYNC
+    SYNC -->|rgbd_image| RELAY
+    RELAY -->|rgbd_image_relay| MAP
+```
+
 Both parameters default to `qos`, so setting `qos` alone configures both sides at once.
 
 Reliability is all that is bridged — durability is left at the default, so a transient-local publisher is not converted. The queue depths are separate too, through `queue_sub` and `queue_pub`.

rtabmap_util/doc/rgbd_split.md

@@ -20,6 +20,18 @@ ComposableNode(
     remappings=[('rgbd_image', '/camera/rgbd_image')])
 ```
 
+Unpacking a bundle for RViz:
+
+```mermaid
+flowchart LR
+    RGBD(["/camera/rgbd_image"])
+    SPLIT["rgbd_split"]
+    RVIZ["RViz"]
+    RGBD --> SPLIT
+    SPLIT -->|"rgb/image,<br>rgb/camera_info"| RVIZ
+    SPLIT -->|"depth/image,<br>depth/camera_info"| RVIZ
+```
+
 ## Subscribed Topics
 
 | Topic | Type | Description |